From 1e574995a0b224f41b0e8883c2cf1ef6346e232e Mon Sep 17 00:00:00 2001 From: marqrazz Date: Wed, 10 Mar 2021 14:38:40 -0700 Subject: [PATCH 1/8] initial port to ros2 --- CMakeLists.txt | 2 +- doc/move_group_interface/CMakeLists.txt | 18 +- .../config/controllers.yaml | 15 + .../config/panda_controllers.yaml | 18 + .../config/start_positions.yaml | 19 + .../launch/move_group.launch.py | 118 +++++ .../launch/move_group.rviz | 498 ++++++++++++++++++ .../move_group_interface_tutorial.launch | 6 - .../move_group_interface_tutorial.launch.py | 50 ++ .../src/move_group_interface_tutorial.cpp | 236 +++++---- 10 files changed, 865 insertions(+), 115 deletions(-) create mode 100644 doc/move_group_interface/config/controllers.yaml create mode 100644 doc/move_group_interface/config/panda_controllers.yaml create mode 100644 doc/move_group_interface/config/start_positions.yaml create mode 100644 doc/move_group_interface/launch/move_group.launch.py create mode 100644 doc/move_group_interface/launch/move_group.rviz delete mode 100644 doc/move_group_interface/launch/move_group_interface_tutorial.launch create mode 100644 doc/move_group_interface/launch/move_group_interface_tutorial.launch.py diff --git a/CMakeLists.txt b/CMakeLists.txt index d678bc9f0d..89193e9dd5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,7 +55,7 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) # add_subdirectory(doc/kinematics) # add_subdirectory(doc/motion_planning_api) # add_subdirectory(doc/motion_planning_pipeline) -# add_subdirectory(doc/move_group_interface) +add_subdirectory(doc/move_group_interface) # add_subdirectory(doc/move_group_python_interface) # add_subdirectory(doc/perception_pipeline) # add_subdirectory(doc/pick_place) diff --git a/doc/move_group_interface/CMakeLists.txt b/doc/move_group_interface/CMakeLists.txt index 03fcfd4976..926ca5cdfb 100644 --- a/doc/move_group_interface/CMakeLists.txt +++ b/doc/move_group_interface/CMakeLists.txt @@ -1,8 +1,16 @@ add_executable(move_group_interface_tutorial src/move_group_interface_tutorial.cpp) -target_link_libraries(move_group_interface_tutorial - ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS move_group_interface_tutorial DESTINATION - ${CATKIN_PACKAGE_BIN_DESTINATION}) +target_include_directories(move_group_interface_tutorial + PUBLIC include) +ament_target_dependencies(move_group_interface_tutorial + ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(TARGETS move_group_interface_tutorial + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) diff --git a/doc/move_group_interface/config/controllers.yaml b/doc/move_group_interface/config/controllers.yaml new file mode 100644 index 0000000000..58ea42051d --- /dev/null +++ b/doc/move_group_interface/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 diff --git a/doc/move_group_interface/config/panda_controllers.yaml b/doc/move_group_interface/config/panda_controllers.yaml new file mode 100644 index 0000000000..bc6e45d277 --- /dev/null +++ b/doc/move_group_interface/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 \ No newline at end of file diff --git a/doc/move_group_interface/config/start_positions.yaml b/doc/move_group_interface/config/start_positions.yaml new file mode 100644 index 0000000000..f4553b3e33 --- /dev/null +++ b/doc/move_group_interface/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 \ No newline at end of file diff --git a/doc/move_group_interface/launch/move_group.launch.py b/doc/move_group_interface/launch/move_group.launch.py new file mode 100644 index 0000000000..24939e1270 --- /dev/null +++ b/doc/move_group_interface/launch/move_group.launch.py @@ -0,0 +1,118 @@ +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(): + + # planning_context + 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 } + + # Planning Functionality + ompl_planning_pipeline_config = { 'move_group' : { + '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['move_group'].update(ompl_planning_yaml) + + # Trajectory Execution Functionality + 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'} + + trajectory_execution = {'moveit_manage_controllers': True, + 'trajectory_execution.allowed_execution_duration_scaling': 1.2, + 'trajectory_execution.allowed_goal_duration_margin': 0.5, + 'trajectory_execution.allowed_start_tolerance': 0.01} + + planning_scene_monitor_parameters = {"publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True} + + # Start the actual move_group node/action server + move_group_node = Node(package='moveit_ros_move_group', + executable='move_group', + output='screen', + parameters=[robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters]) + + # RViz + rviz_config_file = get_package_share_directory('moveit2_tutorials') + "/launch/move_group.rviz" + rviz_node = Node(package='rviz2', + executable='rviz2', + name='rviz2', + output='log', + arguments=['-d', rviz_config_file], + parameters=[robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + kinematics_yaml]) + + # 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', + 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] + ) + + # Warehouse mongodb server + mongodb_server_node = Node(package='warehouse_ros_mongo', + executable='mongo_wrapper_ros.py', + parameters=[{'warehouse_port': 33829}, + {'warehouse_host': 'localhost'}, + {'warehouse_plugin': 'warehouse_ros_mongo::MongoDatabaseConnection'}], + output='screen') + + return LaunchDescription([ rviz_node, static_tf, robot_state_publisher, move_group_node, fake_joint_driver_node, mongodb_server_node ]) diff --git a/doc/move_group_interface/launch/move_group.rviz b/doc/move_group_interface/launch/move_group.rviz new file mode 100644 index 0000000000..e7806a5e0d --- /dev/null +++ b/doc/move_group_interface/launch/move_group.rviz @@ -0,0 +1,498 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PlanningScene1 + - /Trajectory1 + Splitter Ratio: 0.5 + Tree Height: 202 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.20000000298023224 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: panda_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: panda_link0 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.8008623123168945 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5753980278968811 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.4903981685638428 + Saved: ~ +Window Geometry: + "": + collapsed: false + " - Trajectory Slider": + collapsed: false + Displays: + collapsed: false + Height: 812 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000257000002d6fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000153000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffbffffffff01000001940000017d0000017200ffffff000000010000010f000002d6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002d6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000028b000002d600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1533 + X: 199 + Y: 1107 diff --git a/doc/move_group_interface/launch/move_group_interface_tutorial.launch b/doc/move_group_interface/launch/move_group_interface_tutorial.launch deleted file mode 100644 index 052b4f234e..0000000000 --- a/doc/move_group_interface/launch/move_group_interface_tutorial.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/doc/move_group_interface/launch/move_group_interface_tutorial.launch.py b/doc/move_group_interface/launch/move_group_interface_tutorial.launch.py new file mode 100644 index 0000000000..439da96da8 --- /dev/null +++ b/doc/move_group_interface/launch/move_group_interface_tutorial.launch.py @@ -0,0 +1,50 @@ +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(): + # planning_context + 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') + + # MoveGroupInterface demo executable + move_group_demo = Node(name='move_group_interface_tutorial', + package='moveit2_tutorials', + executable='move_group_interface_tutorial', + prefix='xterm -e', + output='screen', + parameters=[robot_description, + robot_description_semantic, + kinematics_yaml]) + + return LaunchDescription([move_group_demo]) diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index 2407850603..fbf8fb6170 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -37,24 +37,41 @@ #include #include -#include -#include +#include +#include -#include -#include +#include +#include -#include +#include + +// #include This has not been ported to ros2 yet +// this is a standin for moveit_visual_tools visual_tools.prompt +void prompt(const std::string& message) +{ + printf(MOVEIT_CONSOLE_COLOR_GREEN "\n%s" MOVEIT_CONSOLE_COLOR_RESET, message.c_str()); + fflush(stdout); + while (std::cin.get() != '\n' && rclcpp::ok()) + ; +} + +// All source files that use ROS logging should define a file-specific +// static const rclcpp::Logger named LOGGER, located at the top of the file +// and inside the namespace with the narrowest scope (if there is one) +static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_demo"); int main(int argc, char** argv) { - ros::init(argc, argv, "move_group_interface_tutorial"); - ros::NodeHandle node_handle; + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto move_group_node = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options); - // ROS spinning must be running for the MoveGroupInterface to get information - // about the robot's state. One way to do this is to start an AsyncSpinner - // beforehand. - ros::AsyncSpinner spinner(1); - spinner.start(); + // We spin up a SingleThreadedExecutor for the current state monitor to get information + // about the robot's state. + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(move_group_node); + std::thread([&executor]() { executor.spin(); }).detach(); // BEGIN_TUTORIAL // @@ -68,7 +85,7 @@ int main(int argc, char** argv) // The :planning_interface:`MoveGroupInterface` class can be easily // setup using just the name of the planning group you would like to control and plan for. - moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP); + moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP); // We will use the :planning_interface:`PlanningSceneInterface` // class to add and remove collision objects in our "virtual world" scene @@ -81,41 +98,43 @@ int main(int argc, char** argv) // Visualization // ^^^^^^^^^^^^^ // + // **** MoveitVisualTools has not been ported to ROS2 yet **** // The package MoveItVisualTools provides many capabilities for visualizing objects, robots, // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script. - namespace rvt = rviz_visual_tools; - moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); - visual_tools.deleteAllMarkers(); + // namespace rvt = rviz_visual_tools; + // moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); + // visual_tools.deleteAllMarkers(); // Remote control is an introspection tool that allows users to step through a high level script // via buttons and keyboard shortcuts in RViz - visual_tools.loadRemoteControl(); + // visual_tools.loadRemoteControl(); // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres - Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); - text_pose.translation().z() = 1.0; - visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE); + // Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); + // text_pose.translation().z() = 1.0; + // visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE); // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations - visual_tools.trigger(); + // visual_tools.trigger(); // Getting Basic Information // ^^^^^^^^^^^^^^^^^^^^^^^^^ // // We can print the name of the reference frame for this robot. - ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str()); + RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group.getPlanningFrame().c_str()); // We can also print the name of the end-effector link for this group. - ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str()); + RCLCPP_INFO(LOGGER, "End effector link: %s", move_group.getEndEffectorLink().c_str()); // We can get a list of all the groups in the robot: - ROS_INFO_NAMED("tutorial", "Available Planning Groups:"); + RCLCPP_INFO(LOGGER, "Available Planning Groups:"); std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(), std::ostream_iterator(std::cout, ", ")); // Start the demo // ^^^^^^^^^^^^^^^^^^^^^^^^^ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); + prompt("Press 'Enter' to start the demo"); // .. _move_group_interface-planning-to-pose-goal: // @@ -123,7 +142,7 @@ int main(int argc, char** argv) // ^^^^^^^^^^^^^^^^^^^^^^^ // We can plan a motion for this group to a desired pose for the // end-effector. - geometry_msgs::Pose target_pose1; + geometry_msgs::msg::Pose target_pose1; target_pose1.orientation.w = 1.0; target_pose1.position.x = 0.28; target_pose1.position.y = -0.2; @@ -137,17 +156,18 @@ int main(int argc, char** argv) bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); + RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); // Visualizing plans // ^^^^^^^^^^^^^^^^^ // We can also visualize the plan as a line with markers in RViz. - ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line"); - visual_tools.publishAxisLabeled(target_pose1, "pose1"); - visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line"); + // visual_tools.publishAxisLabeled(target_pose1, "pose1"); + // visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE); + // visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + // visual_tools.trigger(); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + prompt("Press 'Enter' to continue the demo"); // Moving to a pose goal // ^^^^^^^^^^^^^^^^^^^^^ @@ -171,7 +191,7 @@ int main(int argc, char** argv) // // To start, we'll create an pointer that references the current robot's state. // RobotState is the object that contains all the current position/velocity/acceleration data. - moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); + moveit::core::RobotStatePtr current_state = move_group.getCurrentState(10); // // Next get the current set of joint values for the group. std::vector joint_group_positions; @@ -189,14 +209,15 @@ int main(int argc, char** argv) move_group.setMaxAccelerationScalingFactor(0.05); success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED"); + RCLCPP_INFO(LOGGER, "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED"); // Visualize the plan in RViz - visual_tools.deleteAllMarkers(); - visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + // visual_tools.deleteAllMarkers(); + // visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE); + // visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + // visual_tools.trigger(); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + prompt("Press 'Enter' to continue the demo"); // Planning with Path Constraints // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -204,7 +225,7 @@ int main(int argc, char** argv) // Path constraints can easily be specified for a link on the robot. // Let's specify a path constraint and a pose goal for our group. // First define the path constraint. - moveit_msgs::OrientationConstraint ocm; + moveit_msgs::msg::OrientationConstraint ocm; ocm.link_name = "panda_link7"; ocm.header.frame_id = "panda_link0"; ocm.orientation.w = 1.0; @@ -214,7 +235,7 @@ int main(int argc, char** argv) ocm.weight = 1.0; // Now, set it as the path constraint for the group. - moveit_msgs::Constraints test_constraints; + moveit_msgs::msg::Constraints test_constraints; test_constraints.orientation_constraints.push_back(ocm); move_group.setPathConstraints(test_constraints); @@ -239,7 +260,7 @@ int main(int argc, char** argv) // satisfies the path constraints. So we need to set the start // state to a new pose. moveit::core::RobotState start_state(*move_group.getCurrentState()); - geometry_msgs::Pose start_pose2; + geometry_msgs::msg::Pose start_pose2; start_pose2.orientation.w = 1.0; start_pose2.position.x = 0.55; start_pose2.position.y = -0.05; @@ -256,16 +277,17 @@ int main(int argc, char** argv) move_group.setPlanningTime(10.0); success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED"); + RCLCPP_INFO(LOGGER, "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED"); // Visualize the plan in RViz - visual_tools.deleteAllMarkers(); - visual_tools.publishAxisLabeled(start_pose2, "start"); - visual_tools.publishAxisLabeled(target_pose1, "goal"); - visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - visual_tools.trigger(); - visual_tools.prompt("next step"); + // visual_tools.deleteAllMarkers(); + // visual_tools.publishAxisLabeled(start_pose2, "start"); + // visual_tools.publishAxisLabeled(target_pose1, "goal"); + // visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE); + // visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + // visual_tools.trigger(); + // visual_tools.prompt("next step"); + prompt("Press 'Enter' to continue the demo"); // When done with the path constraint be sure to clear it. move_group.clearPathConstraints(); @@ -276,10 +298,10 @@ int main(int argc, char** argv) // for the end-effector to go through. Note that we are starting // from the new start state above. The initial pose (start state) does not // need to be added to the waypoint list but adding it can help with visualizations - std::vector waypoints; + std::vector waypoints; waypoints.push_back(start_pose2); - geometry_msgs::Pose target_pose3 = start_pose2; + geometry_msgs::msg::Pose target_pose3 = start_pose2; target_pose3.position.z -= 0.2; waypoints.push_back(target_pose3); // down @@ -297,20 +319,21 @@ int main(int argc, char** argv) // translation. We will specify the jump threshold as 0.0, effectively disabling it. // Warning - disabling the jump threshold while operating real hardware can cause // large unpredictable motions of redundant joints and could be a safety issue - moveit_msgs::RobotTrajectory trajectory; + moveit_msgs::msg::RobotTrajectory trajectory; const double jump_threshold = 0.0; const double eef_step = 0.01; double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); - ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0); + RCLCPP_INFO(LOGGER, "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0); // Visualize the plan in RViz - visual_tools.deleteAllMarkers(); - visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE); - visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL); - for (std::size_t i = 0; i < waypoints.size(); ++i) - visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL); - visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + // visual_tools.deleteAllMarkers(); + // visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE); + // visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL); + // for (std::size_t i = 0; i < waypoints.size(); ++i) + // visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL); + // visual_tools.trigger(); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + prompt("Press 'Enter' to continue the demo"); // Cartesian motions should often be slow, e.g. when approaching objects. The speed of cartesian // plans cannot currently be set through the maxVelocityScalingFactor, but requires you to time @@ -325,7 +348,7 @@ int main(int argc, char** argv) // // First let's plan to another simple goal with no objects in the way. move_group.setStartState(*move_group.getCurrentState()); - geometry_msgs::Pose another_pose; + geometry_msgs::msg::Pose another_pose; another_pose.orientation.x = 1.0; another_pose.position.x = 0.7; another_pose.position.y = 0.0; @@ -333,13 +356,14 @@ int main(int argc, char** argv) move_group.setPoseTarget(another_pose); success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - ROS_INFO_NAMED("tutorial", "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED"); + RCLCPP_INFO(LOGGER, "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED"); - visual_tools.deleteAllMarkers(); - visual_tools.publishText(text_pose, "Clear Goal", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - visual_tools.trigger(); - visual_tools.prompt("next step"); + // visual_tools.deleteAllMarkers(); + // visual_tools.publishText(text_pose, "Clear Goal", rvt::WHITE, rvt::XLARGE); + // visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + // visual_tools.trigger(); + // visual_tools.prompt("next step"); + prompt("Press 'Enter' to continue the demo"); // The result may look like this: // @@ -347,14 +371,14 @@ int main(int argc, char** argv) // :alt: animation showing the arm moving relatively straight toward the goal // // Now let's define a collision object ROS message for the robot to avoid. - moveit_msgs::CollisionObject collision_object; + moveit_msgs::msg::CollisionObject collision_object; collision_object.header.frame_id = move_group.getPlanningFrame(); // The id of the object is used to identify it. collision_object.id = "box1"; // Define a box to add to the world. - shape_msgs::SolidPrimitive primitive; + shape_msgs::msg::SolidPrimitive primitive; primitive.type = primitive.BOX; primitive.dimensions.resize(3); primitive.dimensions[primitive.BOX_X] = 0.1; @@ -362,7 +386,7 @@ int main(int argc, char** argv) primitive.dimensions[primitive.BOX_Z] = 0.5; // Define a pose for the box (specified relative to frame_id) - geometry_msgs::Pose box_pose; + geometry_msgs::msg::Pose box_pose; box_pose.orientation.w = 1.0; box_pose.position.x = 0.5; box_pose.position.y = 0.0; @@ -372,26 +396,28 @@ int main(int argc, char** argv) collision_object.primitive_poses.push_back(box_pose); collision_object.operation = collision_object.ADD; - std::vector collision_objects; + std::vector collision_objects; collision_objects.push_back(collision_object); // Now, let's add the collision object into the world // (using a vector that could contain additional objects) - ROS_INFO_NAMED("tutorial", "Add an object into the world"); + RCLCPP_INFO(LOGGER, "Add an object into the world"); planning_scene_interface.addCollisionObjects(collision_objects); // Show text in RViz of status and wait for MoveGroup to receive and process the collision object message - visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE); - visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); + // visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE); + // visual_tools.trigger(); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); + prompt("Press 'Enter' to continue once the collision object appears in RViz"); // Now when we plan a trajectory it will avoid the obstacle success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - ROS_INFO_NAMED("tutorial", "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED"); - visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); + RCLCPP_INFO(LOGGER, "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED"); + // visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE); + // visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + // visual_tools.trigger(); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); + prompt("Press 'Enter' to continue once the plan is complete"); // The result may look like this: // @@ -404,10 +430,10 @@ int main(int argc, char** argv) // You can attach objects to the robot, so that it moves with the robot geometry. // This simulates picking up the object for the purpose of manipulating it. // The motion planning should avoid collisions between the two objects as well. - moveit_msgs::CollisionObject object_to_attach; + moveit_msgs::msg::CollisionObject object_to_attach; object_to_attach.id = "cylinder1"; - shape_msgs::SolidPrimitive cylinder_primitive; + shape_msgs::msg::SolidPrimitive cylinder_primitive; cylinder_primitive.type = primitive.CYLINDER; cylinder_primitive.dimensions.resize(2); cylinder_primitive.dimensions[primitive.CYLINDER_HEIGHT] = 0.20; @@ -415,7 +441,7 @@ int main(int argc, char** argv) // We define the frame/pose for this cylinder so that it appears in the gripper object_to_attach.header.frame_id = move_group.getEndEffectorLink(); - geometry_msgs::Pose grab_pose; + geometry_msgs::msg::Pose grab_pose; grab_pose.orientation.w = 1.0; grab_pose.position.z = 0.2; @@ -427,22 +453,24 @@ int main(int argc, char** argv) // Then, we "attach" the object to the robot. It uses the frame_id to determine which robot link it is attached to. // You could also use applyAttachedCollisionObject to attach an object to the robot directly. - ROS_INFO_NAMED("tutorial", "Attach the object to the robot"); + RCLCPP_INFO(LOGGER, "Attach the object to the robot"); move_group.attachObject(object_to_attach.id, "panda_hand"); - visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE); - visual_tools.trigger(); + // visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE); + // visual_tools.trigger(); /* Wait for MoveGroup to receive and process the attached collision object message */ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot"); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot"); + prompt("Press 'Enter' once the collision object attaches to the robot"); // Replan, but now with the object in hand. move_group.setStartStateToCurrentState(); success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - ROS_INFO_NAMED("tutorial", "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED"); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); + RCLCPP_INFO(LOGGER, "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED"); + // visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + // visual_tools.trigger(); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); + prompt("Press 'Enter' once the plan is complete"); // The result may look something like this: // @@ -453,33 +481,35 @@ int main(int argc, char** argv) // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // // Now, let's detach the cylinder from the robot's gripper. - ROS_INFO_NAMED("tutorial", "Detach the object from the robot"); + RCLCPP_INFO(LOGGER, "Detach the object from the robot"); move_group.detachObject(object_to_attach.id); // Show text in RViz of status - visual_tools.deleteAllMarkers(); - visual_tools.publishText(text_pose, "Object detached from robot", rvt::WHITE, rvt::XLARGE); - visual_tools.trigger(); + // visual_tools.deleteAllMarkers(); + // visual_tools.publishText(text_pose, "Object detached from robot", rvt::WHITE, rvt::XLARGE); + // visual_tools.trigger(); /* Wait for MoveGroup to receive and process the attached collision object message */ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot"); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot"); + prompt("Press 'Enter' once the collision object detaches from the robot"); // Now, let's remove the objects from the world. - ROS_INFO_NAMED("tutorial", "Remove the objects from the world"); + RCLCPP_INFO(LOGGER, "Remove the objects from the world"); std::vector object_ids; object_ids.push_back(collision_object.id); object_ids.push_back(object_to_attach.id); planning_scene_interface.removeCollisionObjects(object_ids); // Show text in RViz of status - visual_tools.publishText(text_pose, "Objects removed", rvt::WHITE, rvt::XLARGE); - visual_tools.trigger(); + // visual_tools.publishText(text_pose, "Objects removed", rvt::WHITE, rvt::XLARGE); + // visual_tools.trigger(); /* Wait for MoveGroup to receive and process the attached collision object message */ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears"); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears"); + prompt("Press 'Enter' once the collision object disappears"); // END_TUTORIAL - ros::shutdown(); + rclcpp::shutdown(); return 0; } From 3ec9dc8887c188dd04b939f843e8046014b39ae5 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Wed, 10 Mar 2021 15:39:31 -0700 Subject: [PATCH 2/8] fixed code comments to work with html generator --- .../move_group_interface_tutorial.rst | 4 +- .../src/move_group_interface_tutorial.cpp | 154 ++++++++++-------- 2 files changed, 91 insertions(+), 67 deletions(-) diff --git a/doc/move_group_interface/move_group_interface_tutorial.rst b/doc/move_group_interface/move_group_interface_tutorial.rst index 7257b21b4c..b569e744f4 100644 --- a/doc/move_group_interface/move_group_interface_tutorial.rst +++ b/doc/move_group_interface/move_group_interface_tutorial.rst @@ -21,11 +21,11 @@ Running the Code ---------------- Open two shells. In the first shell start RViz and wait for everything to finish loading: :: - roslaunch panda_moveit_config demo.launch + ros2 launch moveit2_tutorials move_group.launch.py In the second shell, run the launch file: :: - roslaunch moveit_tutorials move_group_interface_tutorial.launch + ros2 launch moveit2_tutorials move_group_interface.launch.py **Note:** This tutorial uses the **RvizVisualToolsGui** panel to step through the demo. To add this panel to RViz, follow the instructions in the `Visualization Tutorial <../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#rviz-visual-tools>`_. diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index fbf8fb6170..7f67160853 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -45,8 +45,8 @@ #include -// #include This has not been ported to ros2 yet -// this is a standin for moveit_visual_tools visual_tools.prompt +/* #include This has not been ported to ros2 yet */ +/* this is a standin for moveit_visual_tools visual_tools.prompt */ void prompt(const std::string& message) { printf(MOVEIT_CONSOLE_COLOR_GREEN "\n%s" MOVEIT_CONSOLE_COLOR_RESET, message.c_str()); @@ -98,24 +98,26 @@ int main(int argc, char** argv) // Visualization // ^^^^^^^^^^^^^ // - // **** MoveitVisualTools has not been ported to ROS2 yet **** + // MoveitVisualTools has not been ported to ROS2 yet so this is disabled // The package MoveItVisualTools provides many capabilities for visualizing objects, robots, // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script. - // namespace rvt = rviz_visual_tools; - // moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); - // visual_tools.deleteAllMarkers(); + /* + namespace rvt = rviz_visual_tools; + moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); + visual_tools.deleteAllMarkers(); - // Remote control is an introspection tool that allows users to step through a high level script - // via buttons and keyboard shortcuts in RViz - // visual_tools.loadRemoteControl(); + Remote control is an introspection tool that allows users to step through a high level script + via buttons and keyboard shortcuts in RViz + visual_tools.loadRemoteControl(); - // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres - // Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); - // text_pose.translation().z() = 1.0; - // visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE); + RViz provides many types of markers, in this demo we will use text, cylinders, and spheres + Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); + text_pose.translation().z() = 1.0; + visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE); - // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations - // visual_tools.trigger(); + Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations + visual_tools.trigger(); + */ // Getting Basic Information // ^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -133,7 +135,7 @@ int main(int argc, char** argv) // Start the demo // ^^^^^^^^^^^^^^^^^^^^^^^^^ - // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); */ prompt("Press 'Enter' to start the demo"); // .. _move_group_interface-planning-to-pose-goal: @@ -162,11 +164,13 @@ int main(int argc, char** argv) // ^^^^^^^^^^^^^^^^^ // We can also visualize the plan as a line with markers in RViz. RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line"); - // visual_tools.publishAxisLabeled(target_pose1, "pose1"); - // visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE); - // visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - // visual_tools.trigger(); - // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + /* + visual_tools.publishAxisLabeled(target_pose1, "pose1"); + visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE); + visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + visual_tools.trigger(); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + */ prompt("Press 'Enter' to continue the demo"); // Moving to a pose goal @@ -211,12 +215,14 @@ int main(int argc, char** argv) success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED"); - // Visualize the plan in RViz - // visual_tools.deleteAllMarkers(); - // visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE); - // visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - // visual_tools.trigger(); - // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + // Visualize the plan in RViz + /* + visual_tools.deleteAllMarkers(); + visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE); + visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + visual_tools.trigger(); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + */ prompt("Press 'Enter' to continue the demo"); // Planning with Path Constraints @@ -280,13 +286,15 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED"); // Visualize the plan in RViz - // visual_tools.deleteAllMarkers(); - // visual_tools.publishAxisLabeled(start_pose2, "start"); - // visual_tools.publishAxisLabeled(target_pose1, "goal"); - // visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE); - // visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - // visual_tools.trigger(); - // visual_tools.prompt("next step"); + /* + visual_tools.deleteAllMarkers(); + visual_tools.publishAxisLabeled(start_pose2, "start"); + visual_tools.publishAxisLabeled(target_pose1, "goal"); + visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE); + visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + visual_tools.trigger(); + visual_tools.prompt("next step"); + */ prompt("Press 'Enter' to continue the demo"); // When done with the path constraint be sure to clear it. @@ -326,13 +334,15 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0); // Visualize the plan in RViz - // visual_tools.deleteAllMarkers(); - // visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE); - // visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL); - // for (std::size_t i = 0; i < waypoints.size(); ++i) - // visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL); - // visual_tools.trigger(); - // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + /* + visual_tools.deleteAllMarkers(); + visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE); + visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL); + for (std::size_t i = 0; i < waypoints.size(); ++i) + visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL); + visual_tools.trigger(); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + */ prompt("Press 'Enter' to continue the demo"); // Cartesian motions should often be slow, e.g. when approaching objects. The speed of cartesian @@ -358,11 +368,13 @@ int main(int argc, char** argv) success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED"); - // visual_tools.deleteAllMarkers(); - // visual_tools.publishText(text_pose, "Clear Goal", rvt::WHITE, rvt::XLARGE); - // visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - // visual_tools.trigger(); - // visual_tools.prompt("next step"); + /* + visual_tools.deleteAllMarkers(); + visual_tools.publishText(text_pose, "Clear Goal", rvt::WHITE, rvt::XLARGE); + visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + visual_tools.trigger(); + visual_tools.prompt("next step"); + */ prompt("Press 'Enter' to continue the demo"); // The result may look like this: @@ -405,18 +417,22 @@ int main(int argc, char** argv) planning_scene_interface.addCollisionObjects(collision_objects); // Show text in RViz of status and wait for MoveGroup to receive and process the collision object message - // visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE); - // visual_tools.trigger(); - // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); + /* + visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); + */ prompt("Press 'Enter' to continue once the collision object appears in RViz"); // Now when we plan a trajectory it will avoid the obstacle success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED"); - // visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE); - // visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - // visual_tools.trigger(); - // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); + /* + visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE); + visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + visual_tools.trigger(); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); + */ prompt("Press 'Enter' to continue once the plan is complete"); // The result may look like this: @@ -456,8 +472,10 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Attach the object to the robot"); move_group.attachObject(object_to_attach.id, "panda_hand"); - // visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE); - // visual_tools.trigger(); + /* + visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + */ /* Wait for MoveGroup to receive and process the attached collision object message */ // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot"); @@ -467,9 +485,11 @@ int main(int argc, char** argv) move_group.setStartStateToCurrentState(); success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED"); - // visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - // visual_tools.trigger(); - // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); + /* + visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + visual_tools.trigger(); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); + */ prompt("Press 'Enter' once the plan is complete"); // The result may look something like this: @@ -485,12 +505,14 @@ int main(int argc, char** argv) move_group.detachObject(object_to_attach.id); // Show text in RViz of status - // visual_tools.deleteAllMarkers(); - // visual_tools.publishText(text_pose, "Object detached from robot", rvt::WHITE, rvt::XLARGE); - // visual_tools.trigger(); + /* + visual_tools.deleteAllMarkers(); + visual_tools.publishText(text_pose, "Object detached from robot", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + */ /* Wait for MoveGroup to receive and process the attached collision object message */ - // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot"); */ prompt("Press 'Enter' once the collision object detaches from the robot"); // Now, let's remove the objects from the world. @@ -501,11 +523,13 @@ int main(int argc, char** argv) planning_scene_interface.removeCollisionObjects(object_ids); // Show text in RViz of status - // visual_tools.publishText(text_pose, "Objects removed", rvt::WHITE, rvt::XLARGE); - // visual_tools.trigger(); + /* + visual_tools.publishText(text_pose, "Objects removed", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + */ /* Wait for MoveGroup to receive and process the attached collision object message */ - // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears"); */ prompt("Press 'Enter' once the collision object disappears"); // END_TUTORIAL From 5787cb5174e6ba190f54f96905626ab8a515e7a5 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Fri, 12 Mar 2021 13:50:20 -0700 Subject: [PATCH 3/8] merged with main and clang formatted --- .../src/move_group_interface_tutorial.cpp | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index 7f67160853..9db8dba613 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -55,8 +55,8 @@ void prompt(const std::string& message) ; } -// All source files that use ROS logging should define a file-specific -// static const rclcpp::Logger named LOGGER, located at the top of the file +// All source files that use ROS logging should define a file-specific +// static const rclcpp::Logger named LOGGER, located at the top of the file // and inside the namespace with the narrowest scope (if there is one) static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_demo"); @@ -101,7 +101,7 @@ int main(int argc, char** argv) // MoveitVisualTools has not been ported to ROS2 yet so this is disabled // The package MoveItVisualTools provides many capabilities for visualizing objects, robots, // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script. - /* + /* namespace rvt = rviz_visual_tools; moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); visual_tools.deleteAllMarkers(); @@ -164,7 +164,7 @@ int main(int argc, char** argv) // ^^^^^^^^^^^^^^^^^ // We can also visualize the plan as a line with markers in RViz. RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line"); - /* + /* visual_tools.publishAxisLabeled(target_pose1, "pose1"); visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE); visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); @@ -286,7 +286,7 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED"); // Visualize the plan in RViz - /* + /* visual_tools.deleteAllMarkers(); visual_tools.publishAxisLabeled(start_pose2, "start"); visual_tools.publishAxisLabeled(target_pose1, "goal"); @@ -334,7 +334,7 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0); // Visualize the plan in RViz - /* + /* visual_tools.deleteAllMarkers(); visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE); visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL); @@ -368,7 +368,7 @@ int main(int argc, char** argv) success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED"); - /* + /* visual_tools.deleteAllMarkers(); visual_tools.publishText(text_pose, "Clear Goal", rvt::WHITE, rvt::XLARGE); visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); @@ -417,7 +417,7 @@ int main(int argc, char** argv) planning_scene_interface.addCollisionObjects(collision_objects); // Show text in RViz of status and wait for MoveGroup to receive and process the collision object message - /* + /* visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); @@ -427,7 +427,7 @@ int main(int argc, char** argv) // Now when we plan a trajectory it will avoid the obstacle success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED"); - /* + /* visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE); visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); visual_tools.trigger(); @@ -472,7 +472,7 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Attach the object to the robot"); move_group.attachObject(object_to_attach.id, "panda_hand"); - /* + /* visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); */ @@ -485,7 +485,7 @@ int main(int argc, char** argv) move_group.setStartStateToCurrentState(); success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED"); - /* + /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); @@ -505,7 +505,7 @@ int main(int argc, char** argv) move_group.detachObject(object_to_attach.id); // Show text in RViz of status - /* + /* visual_tools.deleteAllMarkers(); visual_tools.publishText(text_pose, "Object detached from robot", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); @@ -523,7 +523,7 @@ int main(int argc, char** argv) planning_scene_interface.removeCollisionObjects(object_ids); // Show text in RViz of status - /* + /* visual_tools.publishText(text_pose, "Objects removed", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); */ From 26787402578ecee686c38f78a5535cac0e3750a0 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Fri, 12 Mar 2021 15:06:29 -0700 Subject: [PATCH 4/8] enabled rviz_visual_tools --- .../src/move_group_interface_tutorial.cpp | 90 +++++++------------ 1 file changed, 34 insertions(+), 56 deletions(-) diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index 9db8dba613..ec162a9770 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -43,10 +43,10 @@ #include #include -#include - /* #include This has not been ported to ros2 yet */ +#include /* this is a standin for moveit_visual_tools visual_tools.prompt */ +#include void prompt(const std::string& message) { printf(MOVEIT_CONSOLE_COLOR_GREEN "\n%s" MOVEIT_CONSOLE_COLOR_RESET, message.c_str()); @@ -55,8 +55,8 @@ void prompt(const std::string& message) ; } -// All source files that use ROS logging should define a file-specific -// static const rclcpp::Logger named LOGGER, located at the top of the file +// All source files that use ROS logging should define a file-specific +// static const rclcpp::Logger named LOGGER, located at the top of the file // and inside the namespace with the narrowest scope (if there is one) static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_demo"); @@ -101,23 +101,23 @@ int main(int argc, char** argv) // MoveitVisualTools has not been ported to ROS2 yet so this is disabled // The package MoveItVisualTools provides many capabilities for visualizing objects, robots, // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script. - /* + namespace rvt = rviz_visual_tools; - moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); + rviz_visual_tools::RvizVisualTools visual_tools("panda_link0", "move_group_tutorial", move_group_node); + /* moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); */ visual_tools.deleteAllMarkers(); - Remote control is an introspection tool that allows users to step through a high level script - via buttons and keyboard shortcuts in RViz - visual_tools.loadRemoteControl(); + // Remote control is an introspection tool that allows users to step through a high level script + // via buttons and keyboard shortcuts in RViz + // visual_tools.loadRemoteControl(); - RViz provides many types of markers, in this demo we will use text, cylinders, and spheres + // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); text_pose.translation().z() = 1.0; visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE); - Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations + // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations visual_tools.trigger(); - */ // Getting Basic Information // ^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -163,14 +163,12 @@ int main(int argc, char** argv) // Visualizing plans // ^^^^^^^^^^^^^^^^^ // We can also visualize the plan as a line with markers in RViz. - RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line"); - /* + RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line"); visual_tools.publishAxisLabeled(target_pose1, "pose1"); visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - */ + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ prompt("Press 'Enter' to continue the demo"); // Moving to a pose goal @@ -216,13 +214,11 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED"); // Visualize the plan in RViz - /* visual_tools.deleteAllMarkers(); visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - */ + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ prompt("Press 'Enter' to continue the demo"); // Planning with Path Constraints @@ -285,16 +281,14 @@ int main(int argc, char** argv) success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED"); - // Visualize the plan in RViz - /* + // Visualize the plan in RViz visual_tools.deleteAllMarkers(); visual_tools.publishAxisLabeled(start_pose2, "start"); visual_tools.publishAxisLabeled(target_pose1, "goal"); visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); - visual_tools.prompt("next step"); - */ + /* visual_tools.prompt("next step"); */ prompt("Press 'Enter' to continue the demo"); // When done with the path constraint be sure to clear it. @@ -333,16 +327,14 @@ int main(int argc, char** argv) double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); RCLCPP_INFO(LOGGER, "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0); - // Visualize the plan in RViz - /* + // Visualize the plan in RViz visual_tools.deleteAllMarkers(); visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE); visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL); for (std::size_t i = 0; i < waypoints.size(); ++i) - visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL); + visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL); visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - */ + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ prompt("Press 'Enter' to continue the demo"); // Cartesian motions should often be slow, e.g. when approaching objects. The speed of cartesian @@ -367,14 +359,12 @@ int main(int argc, char** argv) success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED"); - - /* + visual_tools.deleteAllMarkers(); visual_tools.publishText(text_pose, "Clear Goal", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); - visual_tools.prompt("next step"); - */ + /* visual_tools.prompt("next step"); */ prompt("Press 'Enter' to continue the demo"); // The result may look like this: @@ -416,23 +406,19 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Add an object into the world"); planning_scene_interface.addCollisionObjects(collision_objects); - // Show text in RViz of status and wait for MoveGroup to receive and process the collision object message - /* + // Show text in RViz of status and wait for MoveGroup to receive and process the collision object message visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); - */ + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); */ prompt("Press 'Enter' to continue once the collision object appears in RViz"); // Now when we plan a trajectory it will avoid the obstacle success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED"); - /* visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); - */ + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); */ prompt("Press 'Enter' to continue once the plan is complete"); // The result may look like this: @@ -472,24 +458,20 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Attach the object to the robot"); move_group.attachObject(object_to_attach.id, "panda_hand"); - /* visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); - */ /* Wait for MoveGroup to receive and process the attached collision object message */ - // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot"); */ prompt("Press 'Enter' once the collision object attaches to the robot"); // Replan, but now with the object in hand. move_group.setStartStateToCurrentState(); success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED"); - /* - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); - */ + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); */ prompt("Press 'Enter' once the plan is complete"); // The result may look something like this: @@ -504,12 +486,10 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Detach the object from the robot"); move_group.detachObject(object_to_attach.id); - // Show text in RViz of status - /* + // Show text in RViz of status visual_tools.deleteAllMarkers(); visual_tools.publishText(text_pose, "Object detached from robot", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); - */ /* Wait for MoveGroup to receive and process the attached collision object message */ /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot"); */ @@ -523,10 +503,8 @@ int main(int argc, char** argv) planning_scene_interface.removeCollisionObjects(object_ids); // Show text in RViz of status - /* visual_tools.publishText(text_pose, "Objects removed", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); - */ /* Wait for MoveGroup to receive and process the attached collision object message */ /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears"); */ From 7169adb8891961856386dfe4638be644910e3422 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Fri, 12 Mar 2021 15:13:24 -0700 Subject: [PATCH 5/8] updated rviz config to include tutorial marker topic --- doc/move_group_interface/CMakeLists.txt | 2 +- .../launch/move_group.rviz | 41 +++++++++++-------- 2 files changed, 25 insertions(+), 18 deletions(-) diff --git a/doc/move_group_interface/CMakeLists.txt b/doc/move_group_interface/CMakeLists.txt index 926ca5cdfb..0a6955d15f 100644 --- a/doc/move_group_interface/CMakeLists.txt +++ b/doc/move_group_interface/CMakeLists.txt @@ -5,7 +5,7 @@ target_include_directories(move_group_interface_tutorial ament_target_dependencies(move_group_interface_tutorial ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) -install(TARGETS move_group_interface_tutorial +install(TARGETS move_group_interface_tutorial DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY launch diff --git a/doc/move_group_interface/launch/move_group.rviz b/doc/move_group_interface/launch/move_group.rviz index e7806a5e0d..d3af128c14 100644 --- a/doc/move_group_interface/launch/move_group.rviz +++ b/doc/move_group_interface/launch/move_group.rviz @@ -3,13 +3,9 @@ Panels: Help Height: 78 Name: Displays Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /PlanningScene1 - - /Trajectory1 + Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 202 + Tree Height: 203 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -217,7 +213,6 @@ Visualization Manager: MoveIt_Allow_External_Program: false MoveIt_Allow_Replanning: false MoveIt_Allow_Sensor_Positioning: false - MoveIt_Goal_Tolerance: 0 MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 MoveIt_Use_Cartesian_Path: false @@ -325,7 +320,7 @@ Visualization Manager: Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 Planning Group: panda_arm - Query Goal State: true + Query Goal State: false Query Start State: false Show Workspace: false Start State Alpha: 1 @@ -411,6 +406,18 @@ Visualization Manager: Show Robot Visual: true Value: true Velocity_Scaling_Factor: 0.1 + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_group_tutorial + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -475,16 +482,16 @@ Visualization Manager: Yaw: 0.4903981685638428 Saved: ~ Window Geometry: - "": - collapsed: false - " - Trajectory Slider": - collapsed: false Displays: collapsed: false - Height: 812 + Height: 983 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000257000002d6fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000153000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffbffffffff01000001940000017d0000017200ffffff000000010000010f000002d6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002d6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000028b000002d600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 Selection: collapsed: false Tool Properties: @@ -493,6 +500,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1533 - X: 199 - Y: 1107 + Width: 1848 + X: 72 + Y: 27 From a3eec3898213fe2643f11d73f1d4d3bb41731ce2 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Fri, 12 Mar 2021 15:46:18 -0700 Subject: [PATCH 6/8] updated webpage --- .../move_group_interface_tutorial.rst | 15 +++--- .../src/move_group_interface_tutorial.cpp | 50 +++++++++---------- 2 files changed, 33 insertions(+), 32 deletions(-) diff --git a/doc/move_group_interface/move_group_interface_tutorial.rst b/doc/move_group_interface/move_group_interface_tutorial.rst index b569e744f4..befb152746 100644 --- a/doc/move_group_interface/move_group_interface_tutorial.rst +++ b/doc/move_group_interface/move_group_interface_tutorial.rst @@ -1,8 +1,3 @@ -:moveit1: - -.. - Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag) - Move Group C++ Interface ================================== .. image:: move_group_interface_tutorial_start_screen.png @@ -17,6 +12,11 @@ Getting Started --------------- If you haven't already done so, make sure you've completed the steps in `Getting Started <../getting_started/getting_started.html>`_. +**Note:** Because **MovitVisualTools** and **RvizVisualToolsGui** have not been ported to ROS2 this tutoral has made use of xterm and a simple prompter to help the user progress through each demo step. +To install xterm please run the following command: + + sudo apt-get install -y xterm + Running the Code ---------------- Open two shells. In the first shell start RViz and wait for everything to finish loading: :: @@ -25,9 +25,10 @@ Open two shells. In the first shell start RViz and wait for everything to finish In the second shell, run the launch file: :: - ros2 launch moveit2_tutorials move_group_interface.launch.py + ros2 launch moveit2_tutorials move_group_interface_tutorial.launch.py -**Note:** This tutorial uses the **RvizVisualToolsGui** panel to step through the demo. To add this panel to RViz, follow the instructions in the `Visualization Tutorial <../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#rviz-visual-tools>`_. +**Note:** RvizVisualToolsGui panel has not been ported to ROS2 yet +This tutorial uses the **RvizVisualToolsGui** panel to step through the demo. To add this panel to RViz, follow the instructions in the `Visualization Tutorial <../quickstart_in_rviz/quickstart_in_rviz_tutorial.html#rviz-visual-tools>`_. After a short moment, the RViz window should appear and look similar to the one at the top of this page. To progress through each demo step either press the **Next** button in the **RvizVisualToolsGui** panel at the bottom of the screen or select **Key Tool** in the **Tools** panel at the top of the screen and then press **N** on your keyboard while RViz is focused. diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index ec162a9770..bf4f4559f8 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -55,8 +55,8 @@ void prompt(const std::string& message) ; } -// All source files that use ROS logging should define a file-specific -// static const rclcpp::Logger named LOGGER, located at the top of the file +// All source files that use ROS logging should define a file-specific +// static const rclcpp::Logger named LOGGER, located at the top of the file // and inside the namespace with the narrowest scope (if there is one) static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_demo"); @@ -98,18 +98,18 @@ int main(int argc, char** argv) // Visualization // ^^^^^^^^^^^^^ // - // MoveitVisualTools has not been ported to ROS2 yet so this is disabled + // MoveitVisualTools has not been ported to ROS2 yet so we make use of RvizVisualTools for visualization. // The package MoveItVisualTools provides many capabilities for visualizing objects, robots, // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script. - + namespace rvt = rviz_visual_tools; rviz_visual_tools::RvizVisualTools visual_tools("panda_link0", "move_group_tutorial", move_group_node); /* moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); */ visual_tools.deleteAllMarkers(); - // Remote control is an introspection tool that allows users to step through a high level script - // via buttons and keyboard shortcuts in RViz - // visual_tools.loadRemoteControl(); + /* Remote control is an introspection tool that allows users to step through a high level script */ + /* via buttons and keyboard shortcuts in RViz */ + /* visual_tools.loadRemoteControl(); */ // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); @@ -135,8 +135,8 @@ int main(int argc, char** argv) // Start the demo // ^^^^^^^^^^^^^^^^^^^^^^^^^ - /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); */ prompt("Press 'Enter' to start the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); */ // .. _move_group_interface-planning-to-pose-goal: // @@ -163,13 +163,13 @@ int main(int argc, char** argv) // Visualizing plans // ^^^^^^^^^^^^^^^^^ // We can also visualize the plan as a line with markers in RViz. - RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line"); + RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line"); visual_tools.publishAxisLabeled(target_pose1, "pose1"); visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE); /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); - /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ prompt("Press 'Enter' to continue the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ // Moving to a pose goal // ^^^^^^^^^^^^^^^^^^^^^ @@ -218,8 +218,8 @@ int main(int argc, char** argv) visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE); /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); - /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ prompt("Press 'Enter' to continue the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ // Planning with Path Constraints // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -281,15 +281,15 @@ int main(int argc, char** argv) success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED"); - // Visualize the plan in RViz + // Visualize the plan in RViz visual_tools.deleteAllMarkers(); visual_tools.publishAxisLabeled(start_pose2, "start"); visual_tools.publishAxisLabeled(target_pose1, "goal"); visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE); /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); - /* visual_tools.prompt("next step"); */ prompt("Press 'Enter' to continue the demo"); + /* visual_tools.prompt("next step"); */ // When done with the path constraint be sure to clear it. move_group.clearPathConstraints(); @@ -327,15 +327,15 @@ int main(int argc, char** argv) double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); RCLCPP_INFO(LOGGER, "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0); - // Visualize the plan in RViz + // Visualize the plan in RViz visual_tools.deleteAllMarkers(); visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE); visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL); for (std::size_t i = 0; i < waypoints.size(); ++i) visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL); visual_tools.trigger(); - /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ prompt("Press 'Enter' to continue the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ // Cartesian motions should often be slow, e.g. when approaching objects. The speed of cartesian // plans cannot currently be set through the maxVelocityScalingFactor, but requires you to time @@ -359,13 +359,13 @@ int main(int argc, char** argv) success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); RCLCPP_INFO(LOGGER, "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED"); - + visual_tools.deleteAllMarkers(); visual_tools.publishText(text_pose, "Clear Goal", rvt::WHITE, rvt::XLARGE); /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); - /* visual_tools.prompt("next step"); */ prompt("Press 'Enter' to continue the demo"); + /* visual_tools.prompt("next step"); */ // The result may look like this: // @@ -406,11 +406,11 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Add an object into the world"); planning_scene_interface.addCollisionObjects(collision_objects); - // Show text in RViz of status and wait for MoveGroup to receive and process the collision object message + // Show text in RViz of status and wait for MoveGroup to receive and process the collision object message visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); - /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); */ prompt("Press 'Enter' to continue once the collision object appears in RViz"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); */ // Now when we plan a trajectory it will avoid the obstacle success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); @@ -418,8 +418,8 @@ int main(int argc, char** argv) visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE); /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); - /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); */ prompt("Press 'Enter' to continue once the plan is complete"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); */ // The result may look like this: // @@ -462,8 +462,8 @@ int main(int argc, char** argv) visual_tools.trigger(); /* Wait for MoveGroup to receive and process the attached collision object message */ - /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot"); */ prompt("Press 'Enter' once the collision object attaches to the robot"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot"); */ // Replan, but now with the object in hand. move_group.setStartStateToCurrentState(); @@ -471,8 +471,8 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED"); /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); - /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); */ prompt("Press 'Enter' once the plan is complete"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); */ // The result may look something like this: // @@ -486,14 +486,14 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Detach the object from the robot"); move_group.detachObject(object_to_attach.id); - // Show text in RViz of status + // Show text in RViz of status visual_tools.deleteAllMarkers(); visual_tools.publishText(text_pose, "Object detached from robot", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); /* Wait for MoveGroup to receive and process the attached collision object message */ - /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot"); */ prompt("Press 'Enter' once the collision object detaches from the robot"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot"); */ // Now, let's remove the objects from the world. RCLCPP_INFO(LOGGER, "Remove the objects from the world"); @@ -507,8 +507,8 @@ int main(int argc, char** argv) visual_tools.trigger(); /* Wait for MoveGroup to receive and process the attached collision object message */ - /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears"); */ prompt("Press 'Enter' once the collision object disappears"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears"); */ // END_TUTORIAL From db06f2d07b59a0c8547f16bf99330d3bac0710d8 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Wed, 31 Mar 2021 07:08:36 -0600 Subject: [PATCH 7/8] fixed demo errors --- .../src/move_group_interface_tutorial.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index bf4f4559f8..e82eeb2dfc 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -343,7 +343,7 @@ int main(int argc, char** argv) // Pull requests are welcome. // // You can execute a trajectory like this. - move_group.execute(trajectory); + /* move_group.execute(trajectory); */ // Adding objects to the environment // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -351,7 +351,8 @@ int main(int argc, char** argv) // First let's plan to another simple goal with no objects in the way. move_group.setStartState(*move_group.getCurrentState()); geometry_msgs::msg::Pose another_pose; - another_pose.orientation.x = 1.0; + another_pose.orientation.w = 0; + another_pose.orientation.x = -1.0; another_pose.position.x = 0.7; another_pose.position.y = 0.0; another_pose.position.z = 0.59; @@ -362,6 +363,7 @@ int main(int argc, char** argv) visual_tools.deleteAllMarkers(); visual_tools.publishText(text_pose, "Clear Goal", rvt::WHITE, rvt::XLARGE); + visual_tools.publishAxisLabeled(another_pose, "goal"); /* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */ visual_tools.trigger(); prompt("Press 'Enter' to continue the demo"); @@ -390,7 +392,7 @@ int main(int argc, char** argv) // Define a pose for the box (specified relative to frame_id) geometry_msgs::msg::Pose box_pose; box_pose.orientation.w = 1.0; - box_pose.position.x = 0.5; + box_pose.position.x = 0.48; box_pose.position.y = 0.0; box_pose.position.z = 0.25; @@ -453,10 +455,14 @@ int main(int argc, char** argv) object_to_attach.operation = object_to_attach.ADD; planning_scene_interface.applyCollisionObject(object_to_attach); - // Then, we "attach" the object to the robot. It uses the frame_id to determine which robot link it is attached to. + // Then, we "attach" the object to the robot. It uses the frame_id to determine which robot link it is attached to + // and we also need to tell MoveIt that the object is allowed to be in collision with the finger links of the gripper. // You could also use applyAttachedCollisionObject to attach an object to the robot directly. RCLCPP_INFO(LOGGER, "Attach the object to the robot"); - move_group.attachObject(object_to_attach.id, "panda_hand"); + std::vector touch_links; + touch_links.push_back("panda_rightfinger"); + touch_links.push_back("panda_leftfinger"); + move_group.attachObject(object_to_attach.id, "panda_hand", touch_links); visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); From 5d916f849cc3733ea1e5d27116473397f2305f0d Mon Sep 17 00:00:00 2001 From: marqrazz Date: Wed, 31 Mar 2021 11:19:03 -0600 Subject: [PATCH 8/8] applied clang format --- doc/move_group_interface/src/move_group_interface_tutorial.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index e82eeb2dfc..f7044c0db1 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -455,7 +455,7 @@ int main(int argc, char** argv) object_to_attach.operation = object_to_attach.ADD; planning_scene_interface.applyCollisionObject(object_to_attach); - // Then, we "attach" the object to the robot. It uses the frame_id to determine which robot link it is attached to + // Then, we "attach" the object to the robot. It uses the frame_id to determine which robot link it is attached to // and we also need to tell MoveIt that the object is allowed to be in collision with the finger links of the gripper. // You could also use applyAttachedCollisionObject to attach an object to the robot directly. RCLCPP_INFO(LOGGER, "Attach the object to the robot");