From 26b775df0ee57bf66844e3b36fa995a11db3c664 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Wed, 21 Apr 2021 18:50:38 +0300 Subject: [PATCH] Fix move group namespace and demo.launch.py (#69) * Remove move group prefixes * Fix quickstart rviz tutorial * Fix move_group_interface and move_it_cpp tutorials * Update doc/moveit_cpp/src/moveit_cpp_tutorial.cpp * Add ros shutdown to end of tutorial * Update move_group_interface_tutorial.rst Co-authored-by: Jafar Abdi --- .gitignore | 2 + CMakeLists.txt | 4 +- .../lerp_motion_planner/src/lerp_example.cpp | 2 +- .../src/motion_planning_api_tutorial.cpp | 2 +- .../src/motion_planning_pipeline_tutorial.cpp | 2 +- doc/move_group_interface/CMakeLists.txt | 15 +- .../launch/move_group.launch.py | 193 +++++++ .../launch/move_group.rviz | 505 ++++++++++++++++++ .../move_group_interface_tutorial.launch | 6 - .../move_group_interface_tutorial.launch.py | 58 ++ .../move_group_interface_tutorial.rst | 17 +- .../src/move_group_interface_tutorial.cpp | 184 ++++--- .../move_group_python_interface_tutorial.py | 5 +- doc/moveit_cpp/CMakeLists.txt | 14 +- doc/moveit_cpp/config/moveit_cpp.yaml | 34 +- doc/moveit_cpp/launch/moveit_cpp_node.launch | 3 - .../launch/moveit_cpp_tutorial.launch | 38 -- .../launch/moveit_cpp_tutorial.launch.py | 165 ++++++ .../launch/moveit_cpp_tutorial.rviz | 240 ++++++--- doc/moveit_cpp/moveitcpp_tutorial.rst | 19 +- doc/moveit_cpp/src/moveit_cpp_tutorial.cpp | 91 ++-- doc/quickstart_in_rviz/launch/demo.launch.py | 64 ++- .../launch/panda_moveit_config_demo.rviz | 2 +- doc/trajopt_planner/src/trajopt_example.cpp | 2 +- 24 files changed, 1351 insertions(+), 316 deletions(-) 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 delete mode 100644 doc/moveit_cpp/launch/moveit_cpp_node.launch delete mode 100644 doc/moveit_cpp/launch/moveit_cpp_tutorial.launch create mode 100644 doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py diff --git a/.gitignore b/.gitignore index d7dbdd1258..112fa6b2f8 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,5 @@ build Gemfile native_build *.gdb +# For local rst builds +_build/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 787d530747..11d051818f 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) @@ -69,7 +69,7 @@ add_subdirectory(doc/quickstart_in_rviz) # add_subdirectory(doc/tests) # add_subdirectory(doc/trajopt_planner) # add_subdirectory(doc/creating_moveit_plugins/lerp_motion_planner) -# add_subdirectory(doc/moveit_cpp) +add_subdirectory(doc/moveit_cpp) # add_subdirectory(doc/collision_environments) # add_subdirectory(doc/visualizing_collisions) # add_subdirectory(doc/bullet_collision_checker) diff --git a/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_example.cpp b/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_example.cpp index c28546e861..b6ab7112d8 100644 --- a/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_example.cpp +++ b/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_example.cpp @@ -166,7 +166,7 @@ int main(int argc, char** argv) // visual_tools.trigger(); ros::Publisher display_publisher = - node_handle.advertise("/move_group/display_planned_path", 1, true); + node_handle.advertise("/display_planned_path", 1, true); moveit_msgs::DisplayTrajectory display_trajectory; /* Visualize the trajectory */ diff --git a/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp b/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp index 5f82134b21..83d93168d6 100644 --- a/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp +++ b/doc/motion_planning_api/src/motion_planning_api_tutorial.cpp @@ -195,7 +195,7 @@ int main(int argc, char** argv) // Visualize the result // ^^^^^^^^^^^^^^^^^^^^ ros::Publisher display_publisher = - node_handle.advertise("/move_group/display_planned_path", 1, true); + node_handle.advertise("/display_planned_path", 1, true); moveit_msgs::DisplayTrajectory display_trajectory; /* Visualize the trajectory */ diff --git a/doc/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp b/doc/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp index 05d994048a..0885086e0f 100644 --- a/doc/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp +++ b/doc/motion_planning_pipeline/src/motion_planning_pipeline_tutorial.cpp @@ -175,7 +175,7 @@ int main(int argc, char** argv) // Visualize the result // ^^^^^^^^^^^^^^^^^^^^ ros::Publisher display_publisher = - node_handle.advertise("/move_group/display_planned_path", 1, true); + node_handle.advertise("/display_planned_path", 1, true); moveit_msgs::DisplayTrajectory display_trajectory; /* Visualize the trajectory */ diff --git a/doc/move_group_interface/CMakeLists.txt b/doc/move_group_interface/CMakeLists.txt index 03fcfd4976..b4e2c9128f 100644 --- a/doc/move_group_interface/CMakeLists.txt +++ b/doc/move_group_interface/CMakeLists.txt @@ -1,8 +1,13 @@ 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} +) 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..242d9f229d --- /dev/null +++ b/doc/move_group_interface/launch/move_group.launch.py @@ -0,0 +1,193 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ExecuteProcess +from ament_index_python.packages import get_package_share_directory +import xacro + + +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 = xacro.process_file( + os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "panda.urdf.xacro", + ) + ) + robot_description = {"robot_description": robot_description_config.toxml()} + + 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 + moveit_simple_controllers_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/panda_controllers.yaml" + ) + moveit_controllers = { + "moveit_simple_controller_manager": moveit_simple_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 + run_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], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "panda_ros_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, ros2_controllers_path], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + # Load controllers + load_controllers = [] + for controller in ["panda_arm_controller", "joint_state_controller"]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + shell=True, + output="screen", + ) + ] + + # 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, + run_move_group_node, + ros2_control_node, + mongodb_server_node, + ] + + load_controllers + ) 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..d3af128c14 --- /dev/null +++ b/doc/move_group_interface/launch/move_group.rviz @@ -0,0 +1,505 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 203 + - 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_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: false + 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 + - 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 + 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: + Displays: + collapsed: false + Height: 983 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 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..6f9b638558 --- /dev/null +++ b/doc/move_group_interface/launch/move_group_interface_tutorial.launch.py @@ -0,0 +1,58 @@ +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/move_group_interface_tutorial.rst b/doc/move_group_interface/move_group_interface_tutorial.rst index d2673f25f8..8b4c25471c 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,17 +12,23 @@ 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 **MoveitVisualTools** has 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: :: - 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_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 2407850603..f7044c0db1 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 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()); + 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,15 +98,18 @@ int main(int argc, char** argv) // Visualization // ^^^^^^^^^^^^^ // + // 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; - 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 Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); @@ -103,19 +123,20 @@ int main(int argc, char** argv) // ^^^^^^^^^^^^^^^^^^^^^^^^^ // // 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"); + 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: // @@ -123,7 +144,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 +158,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"); + 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"); + 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 // ^^^^^^^^^^^^^^^^^^^^^ @@ -171,7 +193,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 +211,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 + // 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"); + prompt("Press 'Enter' to continue the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ // Planning with Path Constraints // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -204,7 +227,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 +237,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 +262,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 +279,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.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(); @@ -276,10 +300,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,11 +321,11 @@ 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(); @@ -310,7 +334,8 @@ int main(int argc, char** argv) 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 @@ -318,28 +343,31 @@ 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 // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // // 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; - another_pose.orientation.x = 1.0; + geometry_msgs::msg::Pose another_pose; + 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; 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.publishAxisLabeled(another_pose, "goal"); + /* 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: // @@ -347,14 +375,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,9 +390,9 @@ 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.x = 0.48; box_pose.position.y = 0.0; box_pose.position.z = 0.25; @@ -372,26 +400,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"); + 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); - ROS_INFO_NAMED("tutorial", "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED"); + 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"); + 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: // @@ -404,10 +434,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 +445,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; @@ -425,24 +455,30 @@ 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. - ROS_INFO_NAMED("tutorial", "Attach the object to the robot"); - move_group.attachObject(object_to_attach.id, "panda_hand"); + RCLCPP_INFO(LOGGER, "Attach the object to the robot"); + 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(); /* 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(); 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); + 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: // @@ -453,7 +489,7 @@ 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 @@ -462,10 +498,11 @@ 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 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. - 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); @@ -476,10 +513,11 @@ 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 - ros::shutdown(); + rclcpp::shutdown(); return 0; } diff --git a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py index 001cfc5abb..fb8bb76885 100755 --- a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py +++ b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py @@ -109,13 +109,10 @@ def __init__(self): ## This interface can be used to plan and execute motions: group_name = "panda_arm" move_group = moveit_commander.MoveGroupCommander(group_name) - ## Create a `DisplayTrajectory`_ ROS publisher which is used to display ## trajectories in Rviz: display_trajectory_publisher = rospy.Publisher( - "/move_group/display_planned_path", - moveit_msgs.msg.DisplayTrajectory, - queue_size=20, + "/display_planned_path", moveit_msgs.msg.DisplayTrajectory, queue_size=20 ) ## END_SUB_TUTORIAL diff --git a/doc/moveit_cpp/CMakeLists.txt b/doc/moveit_cpp/CMakeLists.txt index fe0867d6f7..7f317918eb 100644 --- a/doc/moveit_cpp/CMakeLists.txt +++ b/doc/moveit_cpp/CMakeLists.txt @@ -1,5 +1,13 @@ add_executable(moveit_cpp_tutorial src/moveit_cpp_tutorial.cpp) -target_link_libraries(moveit_cpp_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -install(TARGETS moveit_cpp_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +target_include_directories(moveit_cpp_tutorial PUBLIC include) +ament_target_dependencies(moveit_cpp_tutorial ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(TARGETS moveit_cpp_tutorial + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) diff --git a/doc/moveit_cpp/config/moveit_cpp.yaml b/doc/moveit_cpp/config/moveit_cpp.yaml index 9a69f0dd1d..866a63d18e 100644 --- a/doc/moveit_cpp/config/moveit_cpp.yaml +++ b/doc/moveit_cpp/config/moveit_cpp.yaml @@ -1,18 +1,20 @@ -planning_scene_monitor_options: - name: "planning_scene_monitor" - robot_description: "robot_description" - joint_state_topic: "/joint_states" - attached_collision_object_topic: "/planning_scene_monitor" - publish_planning_scene_topic: "/publish_planning_scene" - monitored_planning_scene_topic: "/monitored_planning_scene" - wait_for_initial_state_timeout: 10.0 +moveit_cpp_tutorial: + ros__parameters: + planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" + publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 -planning_pipelines: - pipeline_names: - - ompl + planning_pipelines: + #namespace: "moveit_cpp" # optional, default is ~ + pipeline_names: ["ompl"] -plan_request_params: - planning_attempts: 1 - planning_pipeline: ompl - max_velocity_scaling_factor: 1.0 - max_acceleration_scaling_factor: 1.0 + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 diff --git a/doc/moveit_cpp/launch/moveit_cpp_node.launch b/doc/moveit_cpp/launch/moveit_cpp_node.launch deleted file mode 100644 index 65b053b022..0000000000 --- a/doc/moveit_cpp/launch/moveit_cpp_node.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/doc/moveit_cpp/launch/moveit_cpp_tutorial.launch b/doc/moveit_cpp/launch/moveit_cpp_tutorial.launch deleted file mode 100644 index 362df5eb11..0000000000 --- a/doc/moveit_cpp/launch/moveit_cpp_tutorial.launch +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - ["/moveit_cpp_tutorial/fake_controller_joint_states"] - - - - - - - - - - - - - diff --git a/doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py b/doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py new file mode 100644 index 0000000000..d9a66ca26b --- /dev/null +++ b/doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py @@ -0,0 +1,165 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ExecuteProcess +from ament_index_python.packages import get_package_share_directory +import xacro + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + # moveit_cpp.yaml is passed by filename for now since it's node specific + moveit_cpp_yaml_file_name = ( + get_package_share_directory("moveit2_tutorials") + "/config/moveit_cpp.yaml" + ) + + # Component yaml files are grouped in separate namespaces + robot_description_config = xacro.process_file( + os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "panda.urdf.xacro", + ) + ) + robot_description = {"robot_description": robot_description_config.toxml()} + + 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} + + moveit_simple_controllers_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/panda_controllers.yaml" + ) + moveit_controllers = { + "moveit_simple_controller_manager": moveit_simple_controllers_yaml, + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + } + + ompl_planning_pipeline_config = { + "ompl": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + "start_state_max_bounds_error": 0.1, + } + } + ompl_planning_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" + ) + ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml) + + # MoveItCpp demo executable + moveit_cpp_node = Node( + name="moveit_cpp_tutorial", + package="moveit2_tutorials", + prefix="xterm -e", + executable="moveit_cpp_tutorial", + output="screen", + parameters=[ + moveit_cpp_yaml_file_name, + robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_pipeline_config, + moveit_controllers, + ], + ) + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit2_tutorials") + + "/launch/moveit_cpp_tutorial.rviz" + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[robot_description, robot_description_semantic], + ) + + # Static TF + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + ) + + # Publish TF + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "panda_ros_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, ros2_controllers_path], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + # Load controllers + load_controllers = [] + for controller in ["panda_arm_controller", "joint_state_controller"]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + shell=True, + output="screen", + ) + ] + + return LaunchDescription( + [ + static_tf, + robot_state_publisher, + rviz_node, + moveit_cpp_node, + ros2_control_node, + ] + + load_controllers + ) diff --git a/doc/moveit_cpp/launch/moveit_cpp_tutorial.rviz b/doc/moveit_cpp/launch/moveit_cpp_tutorial.rviz index 9bd99e197f..ae1cef7629 100644 --- a/doc/moveit_cpp/launch/moveit_cpp_tutorial.rviz +++ b/doc/moveit_cpp/launch/moveit_cpp_tutorial.rviz @@ -1,5 +1,5 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: @@ -7,41 +7,32 @@ Panels: - /Global Options1 - /Status1 - /PlanningScene1 + - /Trajectory1 Splitter Ratio: 0.5 - Tree Height: 505 - - Class: rviz/Selection + Tree Height: 583 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 - - Class: rviz/Views + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" - - Class: rviz_visual_tools/RvizVisualToolsGui - Name: RvizVisualToolsGui -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -53,34 +44,16 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Attached Body Color: 150; 50; 150 - Class: moveit_rviz_plugin/RobotState - Collision Enabled: false - Enabled: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - Name: RobotState - Robot Alpha: 1 - Robot Description: robot_description - Robot State Topic: display_robot_state - Show All Links: true - Show Highlights: true - Value: false - Visual Enabled: true - Class: moveit_rviz_plugin/PlanningScene Enabled: true Move Group Namespace: "" Name: PlanningScene - Planning Scene Topic: /publish_planning_scene + Planning Scene Topic: /moveit_cpp/publish_planning_scene Robot Description: robot_description Scene Geometry: - Scene Alpha: 0.899999976 + Scene Alpha: 0.8999999761581421 Scene Color: 50; 230; 50 - Scene Display Time: 0.200000003 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels @@ -155,90 +128,179 @@ Visualization Manager: Show Robot Collision: false Show Robot Visual: true Value: true - - Class: rviz/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Class: rviz/MarkerArray + - 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 + - Class: rviz_default_plugins/MarkerArray Enabled: true - Marker Topic: /rviz_visual_tools Name: MarkerArray Namespaces: Text: true - Queue Size: 100 + start_pose: true + target_pose: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /moveit_cpp_tutorial Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: world + Fixed Frame: panda_link0 Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - 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: /clicked_point + 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/Orbit - Distance: 4.72797632 + Class: rviz_default_plugins/Orbit + Distance: 2.943387746810913 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.103620194 - Y: -0.190347269 - Z: 0.189688578 + X: -0.3850630223751068 + Y: -0.21364367008209229 + Z: 0.6843962073326111 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.415397704 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.45039814710617065 Target Frame: Value: Orbit (rviz) - Yaw: 0.575399756 + Yaw: 0.4703981876373291 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 876 + Height: 812 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000178000002dbfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000028d000000dc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000024005200760069007a00560069007300750061006c0054006f006f006c007300470075006901000002bb000000480000004800ffffff000000010000010f000002dbfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002db000000b500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000064000000042fc0100000002fb0000000800540069006d00650100000000000006400000027000fffffffb0000000800540069006d00650100000000000004500000000000000000000003ad000002db00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - RvizVisualToolsGui: - collapsed: false + QMainWindow State: 000000ff00000000fd000000040000000000000216000002d2fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002d2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100ffffff000000010000010f000002d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002cc000002d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false - Time: - collapsed: false Tool Properties: collapsed: false + Trajectory - Trajectory Slider: + collapsed: false Views: collapsed: false - Width: 1600 - X: 0 - Y: 24 + Width: 1533 + X: 255 + Y: 198 diff --git a/doc/moveit_cpp/moveitcpp_tutorial.rst b/doc/moveit_cpp/moveitcpp_tutorial.rst index 606c5d3726..d6b46e37b7 100644 --- a/doc/moveit_cpp/moveitcpp_tutorial.rst +++ b/doc/moveit_cpp/moveitcpp_tutorial.rst @@ -1,8 +1,3 @@ -:moveit1: - -.. - Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag) - MoveItCpp Tutorial ================================== @@ -18,14 +13,20 @@ 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 a shell, run the launch file: :: - roslaunch moveit_tutorials moveit_cpp_tutorial.launch + ros2 launch moveit2_tutorials moveit_cpp_tutorial.launch.py + +**Note:** RvizVisualToolsGui panel has not been ported to ROS2 yet! -**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>`_. +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. @@ -37,4 +38,4 @@ The entire code can be seen :codedir:`here in the MoveIt GitHub project` on GitHub. All the code in this tutorial can be run from the **moveit_tutorials** package that you have as part of your MoveIt setup. +The entire launch file is :codedir:`here` on GitHub. All the code in this tutorial can be run from the **moveit2_tutorials** package that you have as part of your MoveIt setup. diff --git a/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp b/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp index bdd0982de4..712dc43fbd 100644 --- a/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp +++ b/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp @@ -1,21 +1,47 @@ -#include +#include #include // MoveitCpp #include #include -#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()); + fflush(stdout); + while (std::cin.get() != '\n' && rclcpp::ok()) + ; +} namespace rvt = rviz_visual_tools; +// 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("moveit_cpp_tutorial"); + int main(int argc, char** argv) { - ros::init(argc, argv, "moveit_cpp_tutorial"); - ros::NodeHandle nh("/moveit_cpp_tutorial"); - ros::AsyncSpinner spinner(4); - spinner.start(); + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + RCLCPP_INFO(LOGGER, "Initialize node"); + + // This enables loading undeclared parameters + // best practice would be to declare parameters in the corresponding classes + // and provide descriptions about expected use + node_options.automatically_declare_parameters_from_overrides(true); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("run_moveit_cpp", "", node_options); + + // 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(node); + std::thread([&executor]() { executor.spin(); }).detach(); // BEGIN_TUTORIAL // @@ -26,11 +52,11 @@ int main(int argc, char** argv) static const std::string LOGNAME = "moveit_cpp_tutorial"; /* Otherwise robot with zeros joint_states */ - ros::Duration(1.0).sleep(); + rclcpp::sleep_for(std::chrono::seconds(1)); - ROS_INFO_STREAM_NAMED(LOGNAME, "Starting MoveIt Tutorials..."); + RCLCPP_INFO(LOGGER, "Starting MoveIt Tutorials..."); - auto moveit_cpp_ptr = std::make_shared(nh); + auto moveit_cpp_ptr = std::make_shared(node); moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService(); auto planning_components = @@ -44,10 +70,11 @@ int main(int argc, char** argv) // // The package MoveItVisualTools provides many capabilties for visualizing objects, robots, // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script - moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0", rvt::RVIZ_MARKER_TOPIC, - moveit_cpp_ptr->getPlanningSceneMonitor()); + rviz_visual_tools::RvizVisualTools visual_tools("panda_link0", "moveit_cpp_tutorial", node); + /* moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0", rvt::RVIZ_MARKER_TOPIC, + moveit_cpp_ptr->getPlanningSceneMonitor()); */ visual_tools.deleteAllMarkers(); - visual_tools.loadRemoteControl(); + /* visual_tools.loadRemoteControl(); */ Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); text_pose.translation().z() = 1.75; @@ -56,7 +83,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"); */ // Planning with MoveItCpp // ^^^^^^^^^^^^^^^^^^^^^^^ @@ -70,7 +98,7 @@ int main(int argc, char** argv) planning_components->setStartStateToCurrentState(); // The first way to set the goal of the plan is by using geometry_msgs::PoseStamped ROS message type as follow - geometry_msgs::PoseStamped target_pose1; + geometry_msgs::msg::PoseStamped target_pose1; target_pose1.header.frame_id = "panda_link0"; target_pose1.pose.orientation.w = 1.0; target_pose1.pose.position.x = 0.28; @@ -92,7 +120,7 @@ int main(int argc, char** argv) visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose"); visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE); // Visualize the trajectory in Rviz - visual_tools.publishTrajectoryLine(plan_solution1.trajectory, joint_model_group_ptr); + /* visual_tools.publishTrajectoryLine(plan_solution1.trajectory, joint_model_group_ptr); */ visual_tools.trigger(); /* Uncomment if you want to execute the plan */ @@ -105,10 +133,10 @@ int main(int argc, char** argv) // :width: 250pt // :align: center // - // Start the next plan visual_tools.deleteAllMarkers(); - 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"); */ // Plan #2 // ^^^^^^^ @@ -116,7 +144,7 @@ int main(int argc, char** argv) // Here we will set the current state of the plan using // moveit::core::RobotState auto start_state = *(moveit_cpp_ptr->getCurrentState()); - geometry_msgs::Pose start_pose; + geometry_msgs::msg::Pose start_pose; start_pose.orientation.w = 1.0; start_pose.position.x = 0.55; start_pose.position.y = 0.0; @@ -137,7 +165,7 @@ int main(int argc, char** argv) visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose"); visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE); visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose"); - visual_tools.publishTrajectoryLine(plan_solution2.trajectory, joint_model_group_ptr); + /* visual_tools.publishTrajectoryLine(plan_solution2.trajectory, joint_model_group_ptr); */ visual_tools.trigger(); /* Uncomment if you want to execute the plan */ @@ -150,10 +178,10 @@ int main(int argc, char** argv) // :width: 250pt // :align: center // - // Start the next plan visual_tools.deleteAllMarkers(); - 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"); */ // Plan #3 // ^^^^^^^ @@ -161,7 +189,7 @@ int main(int argc, char** argv) // We can also set the goal of the plan using // moveit::core::RobotState auto target_state = *robot_start_state; - geometry_msgs::Pose target_pose2; + geometry_msgs::msg::Pose target_pose2; target_pose2.orientation.w = 1.0; target_pose2.position.x = 0.55; target_pose2.position.y = -0.05; @@ -182,7 +210,7 @@ int main(int argc, char** argv) visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose"); visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE); visual_tools.publishAxisLabeled(target_pose2, "target_pose"); - visual_tools.publishTrajectoryLine(plan_solution3.trajectory, joint_model_group_ptr); + /* visual_tools.publishTrajectoryLine(plan_solution3.trajectory, joint_model_group_ptr); */ visual_tools.trigger(); /* Uncomment if you want to execute the plan */ @@ -195,10 +223,10 @@ int main(int argc, char** argv) // :width: 250pt // :align: center // - // Start the next plan visual_tools.deleteAllMarkers(); - 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"); */ // Plan #4 // ^^^^^^^ @@ -225,7 +253,7 @@ int main(int argc, char** argv) visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose"); visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE); visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("panda_link8"), "target_pose"); - visual_tools.publishTrajectoryLine(plan_solution4.trajectory, joint_model_group_ptr); + /* visual_tools.publishTrajectoryLine(plan_solution4.trajectory, joint_model_group_ptr); */ visual_tools.trigger(); /* Uncomment if you want to execute the plan */ @@ -238,11 +266,12 @@ int main(int argc, char** argv) // :width: 250pt // :align: center // - // END_TUTORIAL visual_tools.deleteAllMarkers(); - visual_tools.prompt("Press 'next' to end the demo"); + prompt("Press 'Enter' to exit the demo"); + /* visual_tools.prompt("Press 'next' to end the demo"); */ - ROS_INFO_STREAM_NAMED(LOGNAME, "Shutting down."); - ros::waitForShutdown(); + RCLCPP_INFO(LOGGER, "Shutting down."); + rclcpp::shutdown(); + return 0; } diff --git a/doc/quickstart_in_rviz/launch/demo.launch.py b/doc/quickstart_in_rviz/launch/demo.launch.py index 1848fcab2c..55090b6790 100644 --- a/doc/quickstart_in_rviz/launch/demo.launch.py +++ b/doc/quickstart_in_rviz/launch/demo.launch.py @@ -5,7 +5,9 @@ from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition, UnlessCondition from launch_ros.actions import Node +from launch.actions import ExecuteProcess from ament_index_python.packages import get_package_share_directory +import xacro def load_file(package_name, file_path): @@ -38,10 +40,14 @@ def generate_launch_description(): ) # planning_context - robot_description_config = load_file( - "moveit_resources_panda_description", "urdf/panda.urdf" + robot_description_config = xacro.process_file( + os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "panda.urdf.xacro", + ) ) - robot_description = {"robot_description": robot_description_config} + robot_description = {"robot_description": robot_description_config.toxml()} robot_description_semantic_config = load_file( "moveit_resources_panda_moveit_config", "config/panda.srdf" @@ -69,9 +75,11 @@ def generate_launch_description(): ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) # Trajectory Execution Functionality - controllers_yaml = load_yaml("run_move_group", "config/controllers.yaml") + moveit_simple_controllers_yaml = load_yaml( + "moveit_resources_panda_moveit_config", "config/panda_controllers.yaml" + ) moveit_controllers = { - "moveit_simple_controller_manager": controllers_yaml, + "moveit_simple_controller_manager": moveit_simple_controllers_yaml, "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", } @@ -157,25 +165,32 @@ def generate_launch_description(): 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("run_move_group"), - "config", - "panda_controllers.yaml", - ), - os.path.join( - get_package_share_directory("run_move_group"), - "config", - "start_positions.yaml", - ), - robot_description, - ], + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "panda_ros_controllers.yaml", ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, ros2_controllers_path], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + # Load controllers + load_controllers = [] + for controller in ["panda_arm_controller", "joint_state_controller"]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + shell=True, + output="screen", + ) + ] # Warehouse mongodb server mongodb_server_node = Node( @@ -197,7 +212,8 @@ def generate_launch_description(): static_tf, robot_state_publisher, run_move_group_node, - fake_joint_driver_node, + ros2_control_node, mongodb_server_node, ] + + load_controllers ) diff --git a/doc/quickstart_in_rviz/launch/panda_moveit_config_demo.rviz b/doc/quickstart_in_rviz/launch/panda_moveit_config_demo.rviz index aac81fa150..c66fdd9f83 100644 --- a/doc/quickstart_in_rviz/launch/panda_moveit_config_demo.rviz +++ b/doc/quickstart_in_rviz/launch/panda_moveit_config_demo.rviz @@ -309,7 +309,7 @@ Visualization Manager: Show Trail: false State Display Time: 0.05 s Trail Step Size: 1 - Trajectory Topic: /move_group/display_planned_path + Trajectory Topic: /display_planned_path Planning Metrics: Payload: 1 Show Joint Torques: false diff --git a/doc/trajopt_planner/src/trajopt_example.cpp b/doc/trajopt_planner/src/trajopt_example.cpp index f026a7646a..98bfbdf6c2 100644 --- a/doc/trajopt_planner/src/trajopt_example.cpp +++ b/doc/trajopt_planner/src/trajopt_example.cpp @@ -220,7 +220,7 @@ int main(int argc, char** argv) // Visualize the result // ======================================================================================== ros::Publisher display_publisher = - node_handle.advertise("/move_group/display_planned_path", 1, true); + node_handle.advertise("/display_planned_path", 1, true); moveit_msgs::DisplayTrajectory display_trajectory; /* Visualize the trajectory */