Moveit_cpp Tutorial

CMakeLists.txt

@@ -68,7 +68,7 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS})
 # add_subdirectory(doc/tests)
 # add_subdirectory(doc/trajopt_planner)
 # add_subdirectory(doc/creating_moveit_plugins/lerp_motion_planner)
-# add_subdirectory(doc/moveit_cpp)
+add_subdirectory(doc/moveit_cpp)
 # add_subdirectory(doc/collision_environments)
 # add_subdirectory(doc/visualizing_collisions)
 # add_subdirectory(doc/bullet_collision_checker)

doc/moveit_cpp/CMakeLists.txt

@@ -1,5 +1,13 @@
 add_executable(moveit_cpp_tutorial src/moveit_cpp_tutorial.cpp)
-target_link_libraries(moveit_cpp_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-install(TARGETS moveit_cpp_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+target_include_directories(moveit_cpp_tutorial PUBLIC include)
+ament_target_dependencies(moveit_cpp_tutorial  ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
 
-install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(TARGETS moveit_cpp_tutorial
+  DESTINATION lib/${PROJECT_NAME}
+)
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+install(DIRECTORY config
+  DESTINATION share/${PROJECT_NAME}
+)

doc/moveit_cpp/config/controllers.yaml

@@ -0,0 +1,15 @@
+controller_names:
+  - panda_arm_controller
+
+panda_arm_controller:
+  action_ns: follow_joint_trajectory
+  type: FollowJointTrajectory
+  default: true
+  joints:
+    - panda_joint1
+    - panda_joint2
+    - panda_joint3
+    - panda_joint4
+    - panda_joint5
+    - panda_joint6
+    - panda_joint7

doc/moveit_cpp/config/moveit_cpp.yaml

@@ -1,18 +1,20 @@
-planning_scene_monitor_options:
-  name: "planning_scene_monitor"
-  robot_description: "robot_description"
-  joint_state_topic: "/joint_states"
-  attached_collision_object_topic: "/planning_scene_monitor"
-  publish_planning_scene_topic: "/publish_planning_scene"
-  monitored_planning_scene_topic: "/monitored_planning_scene"
-  wait_for_initial_state_timeout: 10.0
-
-planning_pipelines:
-  pipeline_names:
-    - ompl
-
-plan_request_params:
-  planning_attempts: 1
-  planning_pipeline: ompl
-  max_velocity_scaling_factor: 1.0
-  max_acceleration_scaling_factor: 1.0
+moveit_cpp_tutorial:
+  ros__parameters:
+    planning_scene_monitor_options:
+      name: "planning_scene_monitor"
+      robot_description: "robot_description"
+      joint_state_topic: "/joint_states"
+      attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
+      publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene"
+      monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
+      wait_for_initial_state_timeout: 10.0
+
+    planning_pipelines:
+      #namespace: "moveit_cpp"  # optional, default is ~
+      pipeline_names: ["ompl"]
+
+    plan_request_params:
+      planning_attempts: 1
+      planning_pipeline: ompl
+      max_velocity_scaling_factor: 1.0
+      max_acceleration_scaling_factor: 1.0

doc/moveit_cpp/config/panda_controllers.yaml

@@ -0,0 +1,18 @@
+panda_arm_controller:
+  ros__parameters:
+    joints:
+      - panda_joint1
+      - panda_joint2
+      - panda_joint3
+      - panda_joint4
+      - panda_joint5
+      - panda_joint6
+      - panda_joint7
+    write_op_modes:
+      - panda_joint1
+      - panda_joint2
+      - panda_joint3
+      - panda_joint4
+      - panda_joint5
+      - panda_joint6
+      - panda_joint7

doc/moveit_cpp/config/start_positions.yaml

@@ -0,0 +1,19 @@
+fake_joint_driver_node:
+  ros__parameters:
+    start_position:
+      joints:
+        - panda_joint1
+        - panda_joint2
+        - panda_joint3
+        - panda_joint4
+        - panda_joint5
+        - panda_joint6
+        - panda_joint7
+      values:
+        - 0.0
+        - -0.785
+        - 0.0
+        - -2.356
+        - 0.0
+        - 1.571
+        - 0.785

doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py

@@ -0,0 +1,101 @@
+import os
+import yaml
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+def load_file(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, 'r') as file:
+            return file.read()
+    except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+def load_yaml(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, 'r') as file:
+            return yaml.safe_load(file)
+    except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def generate_launch_description():
+    # moveit_cpp.yaml is passed by filename for now since it's node specific
+    moveit_cpp_yaml_file_name = get_package_share_directory('moveit2_tutorials') + "/config/moveit_cpp.yaml"
+
+    # Component yaml files are grouped in separate namespaces
+    robot_description_config = load_file('moveit_resources_panda_description', 'urdf/panda.urdf')
+    robot_description = {'robot_description' : robot_description_config}
+
+    robot_description_semantic_config = load_file('moveit_resources_panda_moveit_config', 'config/panda.srdf')
+    robot_description_semantic = {'robot_description_semantic' : robot_description_semantic_config}
+
+    kinematics_yaml = load_yaml('moveit_resources_panda_moveit_config', 'config/kinematics.yaml')
+    robot_description_kinematics = { 'robot_description_kinematics' : kinematics_yaml }
+
+    controllers_yaml = load_yaml('moveit2_tutorials', 'config/controllers.yaml')
+    moveit_controllers = { 'moveit_simple_controller_manager' : controllers_yaml,
+                           'moveit_controller_manager': 'moveit_simple_controller_manager/MoveItSimpleControllerManager'}
+
+    ompl_planning_pipeline_config = { 'ompl' : {
+        'planning_plugin' : 'ompl_interface/OMPLPlanner',
+        'request_adapters' : """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""" ,
+        'start_state_max_bounds_error' : 0.1 } }
+    ompl_planning_yaml = load_yaml('moveit_resources_panda_moveit_config', 'config/ompl_planning.yaml')
+    ompl_planning_pipeline_config['ompl'].update(ompl_planning_yaml)
+
+    # MoveItCpp demo executable
+    moveit_cpp_node = Node(name='moveit_cpp_tutorial',
+                               package='moveit2_tutorials',
+                               prefix='xterm -e',
+                               executable='moveit_cpp_tutorial',
+                               output='screen',
+                               parameters=[moveit_cpp_yaml_file_name,
+                                           robot_description,
+                                           robot_description_semantic,
+                                           kinematics_yaml,
+                                           ompl_planning_pipeline_config,
+                                           moveit_controllers])
+
+    # RViz
+    rviz_config_file = get_package_share_directory('moveit2_tutorials') + "/launch/moveit_cpp_tutorial.rviz"
+    rviz_node = Node(package='rviz2',
+                     executable='rviz2',
+                     name='rviz2',
+                     output='log',
+                     arguments=['-d', rviz_config_file],
+                     parameters=[robot_description,
+                                 robot_description_semantic])
+
+    # Static TF
+    static_tf = Node(package='tf2_ros',
+                     executable='static_transform_publisher',
+                     name='static_transform_publisher',
+                     output='log',
+                     arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'panda_link0'])
+
+    # Publish TF
+    robot_state_publisher = Node(package='robot_state_publisher',
+                                 executable='robot_state_publisher',
+                                 name='robot_state_publisher',
+                                 output='both',
+                                 parameters=[robot_description])
+
+    # Fake joint driver
+    fake_joint_driver_node = Node(package='fake_joint_driver',
+                                  executable='fake_joint_driver_node',
+                                  # TODO(JafarAbdi): Why this launch the two nodes (controller manager and the fake joint driver) with the same name!
+                                  # name='fake_joint_driver_node',
+                                  parameters=[{'controller_name': 'panda_arm_controller'},
+                                              os.path.join(get_package_share_directory("moveit2_tutorials"), "config", "panda_controllers.yaml"),
+                                              os.path.join(get_package_share_directory("moveit2_tutorials"), "config", "start_positions.yaml"),
+                                              robot_description]
+                                  )
+
+    return LaunchDescription([ static_tf, robot_state_publisher, rviz_node, moveit_cpp_node, fake_joint_driver_node ])