Sync with MoveIt's master branch up-to moveit/moveit@0d0a6a1

Author: JafarAbdi

.gitignore

@@ -58,3 +58,6 @@ qtcreator-*
 
 #gdb
 *.gdb
+
+_build
+_autosummary

MIGRATION.md

@@ -17,6 +17,10 @@ API changes in MoveIt releases
 - `CollisionRobot` and `CollisionWorld` are combined into a single `CollisionEnv` class. This applies for all derived collision checkers as `FCL`, `ALL_VALID`, `HYBRID` and `DISTANCE_FIELD`. Consequently, `getCollisionRobot[Unpadded] / getCollisionWorld` functions are replaced through a `getCollisionEnv` in the planning scene and return the new combined environment. This unified collision environment provides the union of all member functions of `CollisionRobot` and `CollisionWorld`. Note that calling `checkRobotCollision` of the `CollisionEnv` does not take a `CollisionRobot` as an argument anymore as it is implicitly contained in the `CollisionEnv`.
 - `RobotTrajectory` provides a copy constructor that allows a shallow (default) and deep copy of waypoints
 - Replace the redundant namespaces `robot_state::` and `robot_model::` with the actual namespace `moveit::core::`. The additional namespaces will disappear in the future (ROS2).
+- Moved the library `moveit_cpp` to `moveit_ros_planning`. The classes are now in namespace `moveit_cpp`, access via `moveit::planning_interface` is deprecated.
+- The joint states of `passive` joints must be published in ROS and the CurrentStateMonitor will now wait for them as well. Their semantics dictate that they cannot be actively controlled, but they must be known to use the full robot state in collision checks. (https://github.com/ros-planning/moveit/pull/2663)
+- Removed deprecated header `moveit/macros/deprecation.h`. Use `[[deprecated]]` instead.
+- All uses of `MOVEIT_CLASS_FORWARD` et. al. must now be followed by a semicolon for consistency (and to get -pedantic builds to pass for the codebase).
 
 ## ROS Melodic
 

moveit/CMakeLists.txt

@@ -4,6 +4,7 @@ find_package(ament_cmake REQUIRED)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_flake8_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
 endif()
 

moveit_commander/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>moveit_commander</name>
-  <version>1.1.1</version>
+  <version>1.1.5</version>
   <description>Python interfaces to MoveIt</description>
 
   <author email="[email protected]">Ioan Sucan</author>

moveit_commander/src/moveit_commander/move_group.py

@@ -44,6 +44,7 @@
     MoveItErrorCodes,
     TrajectoryConstraints,
     PlannerInterfaceDescription,
+    MotionPlanRequest,
 )
 from sensor_msgs.msg import JointState
 import rospy
@@ -532,12 +533,20 @@ def get_planning_time(self):
         """ Specify the amount of time to be used for motion planning. """
         return self._g.get_planning_time()
 
+    def set_planning_pipeline_id(self, planning_pipeline):
+        """ Specify which planning pipeline to use when motion planning (e.g. ompl, pilz_industrial_motion_planner) """
+        self._g.set_planning_pipeline_id(planning_pipeline)
+
+    def get_planning_pipeline_id(self, planning_pipeline):
+        """ Get the current planning_pipeline_id (e.g. ompl, pilz_industrial_motion_planner) """
+        self._g.get_planning_pipeline_id(planning_pipeline)
+
     def set_planner_id(self, planner_id):
-        """ Specify which planner to use when motion planning """
+        """ Specify which planner of the currently selected pipeline to use when motion planning (e.g. RRTConnect, LIN) """
         self._g.set_planner_id(planner_id)
 
     def get_planner_id(self):
-        """ Get the current planner_id """
+        """ Get the current planner_id (e.g. RRTConnect, LIN) of the currently selected pipeline """
         return self._g.get_planner_id()
 
     def set_num_planning_attempts(self, num_planning_attempts):
@@ -629,6 +638,11 @@ def plan(self, joints=None):
             error_code,
         )
 
+    def construct_motion_plan_request(self):
+        """ Returns a MotionPlanRequest filled with the current goals of the move_group_interface"""
+        mpr = MotionPlanRequest()
+        return mpr.deserialize(self._g.construct_motion_plan_request())
+
     def compute_cartesian_path(
         self,
         waypoints,

moveit_commander/src/moveit_commander/planning_scene_interface.py

@@ -57,7 +57,12 @@
 
 
 class PlanningSceneInterface(object):
-    """ Simple interface to making updates to a planning scene """
+    """
+    Python interface for a C++ PlanningSceneInterface.
+    Uses both C++ wrapped methods and scene manipulation topics
+    to manipulate the PlanningScene managed by the PlanningSceneMonitor.
+    See wrap_python_planning_scene_interface.cpp for the wrapped methods.
+    """
 
     def __init__(self, ns="", synchronous=False, service_timeout=5.0):
         """ Create a planning scene interface; it uses both C++ wrapped methods and scene manipulation topics. """
@@ -88,31 +93,27 @@ def __submit(self, collision_object, attach=False):
             else:
                 self._pub_co.publish(collision_object)
 
+    def add_object(self, collision_object):
+        """ Add an object to the planning scene """
+        self.__submit(collision_object, attach=False)
+
     def add_sphere(self, name, pose, radius=1):
-        """
-        Add a sphere to the planning scene
-        """
+        """ Add a sphere to the planning scene """
         co = self.__make_sphere(name, pose, radius)
         self.__submit(co, attach=False)
 
     def add_cylinder(self, name, pose, height, radius):
-        """
-        Add a cylinder to the planning scene
-        """
+        """ Add a cylinder to the planning scene """
         co = self.__make_cylinder(name, pose, height, radius)
         self.__submit(co, attach=False)
 
     def add_mesh(self, name, pose, filename, size=(1, 1, 1)):
-        """
-        Add a mesh to the planning scene
-        """
+        """ Add a mesh to the planning scene """
         co = self.__make_mesh(name, pose, filename, size)
         self.__submit(co, attach=False)
 
     def add_box(self, name, pose, size=(1, 1, 1)):
-        """
-        Add a box to the planning scene
-        """
+        """ Add a box to the planning scene """
         co = self.__make_box(name, pose, size)
         self.__submit(co, attach=False)
 
@@ -129,6 +130,10 @@ def add_plane(self, name, pose, normal=(0, 0, 1), offset=0):
         co.plane_poses = [pose.pose]
         self.__submit(co, attach=False)
 
+    def attach_object(self, attached_collision_object):
+        """ Attach an object in the planning scene """
+        self.__submit(attached_collision_object, attach=True)
+
     def attach_mesh(
         self, link, name, pose=None, filename="", size=(1, 1, 1), touch_links=[]
     ):
@@ -156,6 +161,11 @@ def attach_box(self, link, name, pose=None, size=(1, 1, 1), touch_links=[]):
             aco.touch_links = [link]
         self.__submit(aco, attach=True)
 
+    def clear(self):
+        """ Remove all objects from the planning scene """
+        self.remove_attached_object()
+        self.remove_world_object()
+
     def remove_world_object(self, name=None):
         """
         Remove an object from planning scene, or all if no name is provided
@@ -166,13 +176,18 @@ def remove_world_object(self, name=None):
             co.id = name
         self.__submit(co, attach=False)
 
-    def remove_attached_object(self, link, name=None):
+    def remove_attached_object(self, link=None, name=None):
         """
-        Remove an attached object from planning scene, or all objects attached to this link if no name is provided
+        Remove an attached object from the robot, or all objects attached to the link if no name is provided,
+        or all attached objects in the scene if neither link nor name are provided.
+
+        Removed attached objects remain in the scene as world objects.
+        Call remove_world_object afterwards to remove them from the scene.
         """
         aco = AttachedCollisionObject()
         aco.object.operation = CollisionObject.REMOVE
-        aco.link_name = link
+        if link is not None:
+            aco.link_name = link
         if name is not None:
             aco.object.id = name
         self.__submit(aco, attach=True)
@@ -230,10 +245,18 @@ def get_attached_objects(self, object_ids=[]):
             aobjs[key] = msg
         return aobjs
 
+    def apply_planning_scene(self, planning_scene_message):
+        """
+        Applies the planning scene message.
+        """
+        return self._psi.apply_planning_scene(
+            conversions.msg_to_string(planning_scene_message)
+        )
+
     @staticmethod
     def __make_existing(name):
         """
-        Create an empty Collision Object, used when the object already exists
+        Create an empty Collision Object. Used when the object already exists
         """
         co = CollisionObject()
         co.id = name

moveit_commander/src/moveit_commander/robot.py

@@ -194,12 +194,25 @@ def get_root_link(self):
         """Get the name of the root link of the robot model """
         return self._r.get_robot_root_link()
 
+    def get_active_joint_names(self, group=None):
+        """
+        Get the names of all the movable joints that make up a group.
+        If no group name is specified, all joints in the robot model are returned.
+        Excludes fixed and mimic joints.
+        """
+        if group is not None:
+            if self.has_group(group):
+                return self._r.get_group_active_joint_names(group)
+            else:
+                raise MoveItCommanderException("There is no group named %s" % group)
+        else:
+            return self._r.get_active_joint_names()
+
     def get_joint_names(self, group=None):
         """
-        Get the names of all the movable joints that make up a group
-        (mimic joints and fixed joints are excluded). If no group name is
-        specified, all joints in the robot model are returned, including
-        fixed and mimic joints.
+        Get the names of all the movable joints that make up a group.
+        If no group name is specified, all joints in the robot model are returned.
+        Includes fixed and mimic joints.
         """
         if group is not None:
             if self.has_group(group):

moveit_core/CMakeLists.txt

@@ -49,6 +49,8 @@ find_package(trajectory_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(common_interfaces REQUIRED)
 find_package(pluginlib REQUIRED)
+# TODO: Port python bindings
+# find_package(pybind11 REQUIRED)
 
 # Set target file path for version.h
 set(VERSION_FILE_PATH ${CMAKE_BINARY_DIR}/include)
@@ -77,6 +79,8 @@ set(THIS_PACKAGE_INCLUDE_DIRS
     planning_request_adapter/include
     planning_scene/include
     profiler/include
+    # TODO: Port python bindings
+    # python/tools/include
     sensor_manager/include
     trajectory_processing/include
     utils/include
@@ -99,6 +103,8 @@ set(THIS_PACKAGE_LIBRARIES
     moveit_constraint_samplers
     moveit_planning_request_adapter
     moveit_profiler
+    # TODO: Port python bindings
+    # moveit_python_tools
     moveit_trajectory_processing
     moveit_distance_field
     moveit_collision_distance_field
@@ -183,6 +189,34 @@ else()
 install(FILES collision_detection_bullet/empty_description.xml DESTINATION share/${PROJECT_NAME} RENAME collision_detector_bullet_description.xml)
 endif()
 
+# TODO: Port python bindings
+# add_subdirectory(python)
+# set(pymoveit_libs
+#   moveit_collision_detection
+#   moveit_kinematic_constraints
+#   moveit_planning_scene
+#   moveit_python_tools
+#   moveit_robot_model
+#   moveit_robot_state
+#   moveit_transforms
+# )
+#
+# pybind_add_module(pymoveit_core
+#     python/pymoveit_core.cpp
+#     collision_detection/src/pycollision_detection.cpp
+#     robot_model/src/pyrobot_model.cpp
+#     robot_state/src/pyrobot_state.cpp
+#     transforms/src/pytransforms.cpp
+#     planning_scene/src/pyplanning_scene.cpp
+#     kinematic_constraints/src/pykinematic_constraint.cpp
+# )
+# target_include_directories(pymoveit_core SYSTEM PRIVATE ${catkin_INCLUDE_DIRS})
+# target_link_libraries(pymoveit_core PRIVATE ${pymoveit_libs} ${catkin_LIBRARIES})
+#
+# #catkin_lint: ignore_once undefined_target (pymoveit_core is defined by pybind_add_module)
+# install(TARGETS pymoveit_core LIBRARY DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION})
+
+
 install(
   TARGETS ${THIS_PACKAGE_LIBRARIES}
   EXPORT export_${PROJECT_NAME}
@@ -202,6 +236,7 @@ if(BUILD_TESTING)
   set(ament_cmake_copyright_FOUND TRUE)
   set(ament_cmake_cppcheck_FOUND TRUE)
   set(ament_cmake_cpplint_FOUND TRUE)
+  set(ament_cmake_flake8_FOUND TRUE)
   set(ament_cmake_uncrustify_FOUND TRUE)
   set(ament_cmake_lint_cmake_FOUND TRUE)
 

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

@@ -47,7 +47,7 @@
 
 namespace collision_detection
 {
-MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix)  // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc
+MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix);  // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc
 
 /** \brief The types of bodies that are considered for collision */
 namespace BodyTypes

moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h

@@ -41,7 +41,7 @@
 
 namespace collision_detection
 {
-MOVEIT_CLASS_FORWARD(CollisionDetectorAllocator)  // Defines CollisionDetectorAllocatorPtr, ConstPtr, WeakPtr... etc
+MOVEIT_CLASS_FORWARD(CollisionDetectorAllocator);  // Defines CollisionDetectorAllocatorPtr, ConstPtr, WeakPtr... etc
 
 /** \brief An allocator for a compatible CollisionWorld/CollisionRobot pair. */
 class CollisionDetectorAllocator