Merge pull request #10 from AcutronicRobotics/planning_scene

Planning scene & trajectory_processing

moveit_core/CMakeLists.txt

@@ -104,10 +104,10 @@ set(THIS_PACKAGE_INCLUDE_DIRS
     macros/include
     planning_interface/include
     # planning_request_adapter/include
-    # planning_scene/include
+    planning_scene/include
     profiler/include
     sensor_manager/include
-    # trajectory_processing/include
+    trajectory_processing/include
     # utils/include
     logging/include
 )
@@ -119,6 +119,8 @@ set(dependencies
   moveit_collision_detection
   moveit_collision_detection_fcl
   moveit_kinematic_constraints
+  moveit_planning_scene
+  moveit_trajectory_processing
   random_numbers
   tf2_eigen
   tf2_kdl
@@ -189,11 +191,11 @@ add_subdirectory(robot_trajectory)
 add_subdirectory(collision_detection)
 add_subdirectory(collision_detection_fcl)
 add_subdirectory(kinematic_constraints)
-# add_subdirectory(planning_scene)
+add_subdirectory(planning_scene)
 # add_subdirectory(constraint_samplers)
 add_subdirectory(planning_interface)
 # add_subdirectory(planning_request_adapter)
-# add_subdirectory(trajectory_processing)
+add_subdirectory(trajectory_processing)
 # add_subdirectory(distance_field)
 # add_subdirectory(collision_distance_field)
 # add_subdirectory(kinematics_metrics)

moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h

@@ -149,7 +149,7 @@ class AllowedCollisionMatrix
    *  @param name2 name of second element
    *  @param fn A callback function that is used to decide if collisions are allowed between the two elements is
    * expected here */
-  void setEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn);
+  void setEntry(const std::string& name1, const std::string& name2, const DecideContactFn& fn);
 
   /** @brief Set the entries corresponding to a name. With each of the the known names in the collision matrix, form a
    * pair using the name
@@ -222,7 +222,7 @@ class AllowedCollisionMatrix
    *  @param name The name of the element for which to set the default value
    *  @param fn A callback function that is used to decide if collisions are allowed between \e name and some other
    * element is expected here. */
-  void setDefaultEntry(const std::string& name, DecideContactFn& fn);
+  void setDefaultEntry(const std::string& name, const DecideContactFn& fn);
 
   /** @brief Get the type of the allowed collision between to be considered by default for an element. Return true if a
    * default value was

moveit_core/collision_detection/include/moveit/collision_detection/world.h

@@ -155,7 +155,7 @@ class World
 
   /** \brief Update the pose of a shape in an object. Shape equality is
    * verified by comparing pointers. Returns true on success. */
-  bool moveShapeInObject(const std::string& id, const shapes::ShapeConstPtr& shape, const Eigen::Isometry3d& pose);
+  bool moveShapeInObject(const std::string& id, const shapes::ShapeConstPtr& shape, const Eigen::Affine3d& pose);
 
   /** \brief Move all shapes in an object according to the given transform specified in world frame */
   bool moveObject(const std::string& id, const Eigen::Isometry3d& transform);

moveit_core/collision_detection/src/collision_matrix.cpp

@@ -144,7 +144,7 @@ void AllowedCollisionMatrix::setEntry(const std::string& name1, const std::strin
   }
 }
 
-void AllowedCollisionMatrix::setEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn)
+void AllowedCollisionMatrix::setEntry(const std::string& name1, const std::string& name2, const DecideContactFn& fn)
 {
   entries_[name1][name2] = entries_[name2][name1] = AllowedCollision::CONDITIONAL;
   allowed_contacts_[name1][name2] = allowed_contacts_[name2][name1] = fn;
@@ -234,7 +234,7 @@ void AllowedCollisionMatrix::setDefaultEntry(const std::string& name, bool allow
   default_allowed_contacts_.erase(name);
 }
 
-void AllowedCollisionMatrix::setDefaultEntry(const std::string& name, DecideContactFn& fn)
+void AllowedCollisionMatrix::setDefaultEntry(const std::string& name, const DecideContactFn& fn)
 {
   default_entries_[name] = AllowedCollision::CONDITIONAL;
   default_allowed_contacts_[name] = fn;

moveit_core/collision_detection/src/world.cpp

@@ -136,7 +136,7 @@ bool World::hasObject(const std::string& id) const
   return objects_.find(id) != objects_.end();
 }
 
-bool World::moveShapeInObject(const std::string& id, const shapes::ShapeConstPtr& shape, const Eigen::Isometry3d& pose)
+bool World::moveShapeInObject(const std::string& id, const shapes::ShapeConstPtr& shape, const Eigen::Affine3d& pose)
 {
   auto it = objects_.find(id);
   if (it != objects_.end())

moveit_core/planning_scene/CMakeLists.txt

@@ -1,7 +1,8 @@
 set(MOVEIT_LIB_NAME moveit_planning_scene)
 
 add_library(${MOVEIT_LIB_NAME} src/planning_scene.cpp)
-set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+#TODO: Fix the versioning
+# set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
 target_link_libraries(${MOVEIT_LIB_NAME}
   moveit_robot_model
@@ -14,17 +15,16 @@ target_link_libraries(${MOVEIT_LIB_NAME}
   moveit_trajectory_processing
   ${LIBOCTOMAP_LIBRARIES} ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
 
-add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
-
 install(TARGETS ${MOVEIT_LIB_NAME}
-        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
-install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
-
-if(CATKIN_ENABLE_TESTING)
-  find_package(moveit_resources REQUIRED)
-  include_directories(${moveit_resources_INCLUDE_DIRS})
+        LIBRARY DESTINATION lib
+        ARCHIVE DESTINATION lib)
+install(DIRECTORY include/ DESTINATION include)
 
-  catkin_add_gtest(test_planning_scene test/test_planning_scene.cpp)
-  target_link_libraries(test_planning_scene ${MOVEIT_LIB_NAME})
-endif()
+#TODO: Fix this so it can be built with colcon
+# if(BUILD_TESTING)
+#   find_package(moveit_resources REQUIRED)
+#   include_directories(${moveit_resources_INCLUDE_DIRS})
+#
+#   ament_add_gtest(test_planning_scene test/test_planning_scene.cpp)
+#   target_link_libraries(test_planning_scene ${MOVEIT_LIB_NAME})
+# endif()