From 49956deb24e9c078599337f29ce7980b3b41daf5 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Wed, 14 Apr 2021 01:53:16 +0300 Subject: [PATCH 1/8] Remove move group prefixes --- .../lerp_motion_planner/src/lerp_example.cpp | 2 +- .../src/motion_planning_api_tutorial.cpp | 2 +- .../src/motion_planning_pipeline_tutorial.cpp | 2 +- .../move_group_python_interface_tutorial.py | 2 +- doc/quickstart_in_rviz/launch/demo.launch.py | 252 +++++++++++------- .../launch/panda_moveit_config_demo.rviz | 2 +- doc/trajopt_planner/src/trajopt_example.cpp | 2 +- 7 files changed, 159 insertions(+), 105 deletions(-) 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 ad123336d0..2db2ceac54 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 d205e3bc01..55237e0ca8 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_python_interface/scripts/move_group_python_interface_tutorial.py b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py index 9e7c7d0d74..1878cca870 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 @@ -110,7 +110,7 @@ def __init__(self): ## Create a `DisplayTrajectory`_ ROS publisher which is used to display ## trajectories in Rviz: - display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', + display_trajectory_publisher = rospy.Publisher('/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) diff --git a/doc/quickstart_in_rviz/launch/demo.launch.py b/doc/quickstart_in_rviz/launch/demo.launch.py index 9d51fff59b..3c9aa01dde 100644 --- a/doc/quickstart_in_rviz/launch/demo.launch.py +++ b/doc/quickstart_in_rviz/launch/demo.launch.py @@ -1,139 +1,193 @@ import os import yaml from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -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): 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: + with open(absolute_file_path, "r") as file: return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + 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: + with open(absolute_file_path, "r") as file: return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available return None def generate_launch_description(): - # Command-line arguments - tutorial_arg = DeclareLaunchArgument( - 'rviz_tutorial', default_value='False', description='Tutorial flag') - # 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 } + 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) + 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('run_move_group', '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} + 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]) + 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 - tutorial_mode = LaunchConfiguration('rviz_tutorial') - rviz_base = os.path.join(get_package_share_directory('moveit2_tutorials'), 'launch') - rviz_full_config = os.path.join(rviz_base, 'panda_moveit_config_demo.rviz') - rviz_empty_config = os.path.join(rviz_base, 'panda_moveit_config_demo_empty.rviz') - rviz_node_tutorial = Node(package='rviz2', - executable='rviz2', - name='rviz2', - output='log', - arguments= ['-d', rviz_empty_config], - parameters=[robot_description, - robot_description_semantic, - ompl_planning_pipeline_config, - kinematics_yaml], - condition=IfCondition(tutorial_mode)) - rviz_node = Node(package='rviz2', - executable='rviz2', - name='rviz2', - output='log', - arguments= ['-d', rviz_full_config], - parameters=[robot_description, - robot_description_semantic, - ompl_planning_pipeline_config, - kinematics_yaml], - condition=UnlessCondition(tutorial_mode)) + rviz_config_file = ( + get_package_share_directory("moveit2_tutorials") + "/launch/panda_moveit_config_demo_empty.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']) + 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("run_move_group"), "config", "panda_controllers.yaml"), - os.path.join(get_package_share_directory("run_move_group"), "config", "start_positions.yaml"), - robot_description] - ) + 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([ tutorial_arg, rviz_node, rviz_node_tutorial, static_tf, robot_state_publisher, run_move_group_node, fake_joint_driver_node, mongodb_server_node ]) + 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/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 */ From 4fc7b5574889cc4c86ce2c26d9ed67890261d2b6 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Wed, 14 Apr 2021 10:27:37 +0000 Subject: [PATCH 2/8] Fix quickstart rviz tutorial --- doc/quickstart_in_rviz/launch/demo.launch.py | 49 +++++++++++++------- 1 file changed, 33 insertions(+), 16 deletions(-) diff --git a/doc/quickstart_in_rviz/launch/demo.launch.py b/doc/quickstart_in_rviz/launch/demo.launch.py index 3c9aa01dde..944223a37c 100644 --- a/doc/quickstart_in_rviz/launch/demo.launch.py +++ b/doc/quickstart_in_rviz/launch/demo.launch.py @@ -1,6 +1,9 @@ import os import yaml from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +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 @@ -31,6 +34,11 @@ def load_yaml(package_name, file_path): def generate_launch_description(): + + # Command-line arguments + tutorial_arg = DeclareLaunchArgument( + 'rviz_tutorial', default_value='False', description='Tutorial flag') + # planning_context robot_description_config = xacro.process_file( os.path.join( @@ -106,22 +114,30 @@ def generate_launch_description(): ) # RViz - rviz_config_file = ( - get_package_share_directory("moveit2_tutorials") + "/launch/panda_moveit_config_demo_empty.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, - ], - ) + tutorial_mode = LaunchConfiguration('rviz_tutorial') + rviz_base = os.path.join(get_package_share_directory('moveit2_tutorials'), 'launch') + rviz_full_config = os.path.join(rviz_base, 'panda_moveit_config_demo.rviz') + rviz_empty_config = os.path.join(rviz_base, 'panda_moveit_config_demo_empty.rviz') + rviz_node_tutorial = Node(package='rviz2', + executable='rviz2', + name='rviz2', + output='log', + arguments= ['-d', rviz_empty_config], + parameters=[robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + kinematics_yaml], + condition=IfCondition(tutorial_mode)) + rviz_node = Node(package='rviz2', + executable='rviz2', + name='rviz2', + output='log', + arguments= ['-d', rviz_full_config], + parameters=[robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + kinematics_yaml], + condition=UnlessCondition(tutorial_mode)) # Static TF static_tf = Node( @@ -183,6 +199,7 @@ def generate_launch_description(): return LaunchDescription( [ rviz_node, + rviz_node_tutorial, static_tf, robot_state_publisher, run_move_group_node, From 36315907d605d8672e20dec7ed592a390ae447a9 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Wed, 14 Apr 2021 13:59:45 +0000 Subject: [PATCH 3/8] Fix move_group_interface and move_it_cpp tutorials --- .gitignore | 4 +- .pre-commit-config.yaml | 43 ++ CMakeLists.txt | 4 +- 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 ++++--- doc/moveit_cpp/CMakeLists.txt | 14 +- doc/moveit_cpp/config/moveit_cpp.yaml | 34 +- doc/moveit_cpp/launch/moveit_cpp_node.launch | 4 - .../launch/moveit_cpp_tutorial.launch | 39 -- .../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 | 98 ++-- 18 files changed, 1352 insertions(+), 290 deletions(-) create mode 100644 .pre-commit-config.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 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 ebf3322155..112fa6b2f8 100644 --- a/.gitignore +++ b/.gitignore @@ -5,4 +5,6 @@ doc/html build Gemfile native_build -*.gdb \ No newline at end of file +*.gdb +# For local rst builds +_build/ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000000..b380f58c88 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,43 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v3.4.0 + hooks: + - id: check-added-large-files + - id: check-case-conflict + - id: check-merge-conflict + - id: check-symlinks + - id: check-yaml + - id: debug-statements + - id: end-of-file-fixer + - id: mixed-line-ending + - id: trailing-whitespace + + - repo: https://github.com/psf/black + rev: 20.8b1 + hooks: + - id: black + + - repo: local + hooks: + - id: clang-format + name: clang-format + description: Format files with ClangFormat. + entry: clang-format-10 + language: system + files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ + args: ['-fallback-style=none', '-i'] diff --git a/CMakeLists.txt b/CMakeLists.txt index 1bb11f1726..bc56e3cc41 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/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 7257b21b4c..47e403ad4b 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 **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: :: - 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/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 1f2cee7d8f..0000000000 --- a/doc/moveit_cpp/launch/moveit_cpp_node.launch +++ /dev/null @@ -1,4 +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 74618311b0..0000000000 --- a/doc/moveit_cpp/launch/moveit_cpp_tutorial.launch +++ /dev/null @@ -1,39 +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 381676ccc1..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..b13803322f 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,11 +120,11 @@ 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 */ - /* planning_components->execute(); // Execute the plan */ + // planning_components->execute(); // Execute the plan */ } // Plan #1 visualization: @@ -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,11 +165,11 @@ 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 */ - /* planning_components->execute(); // Execute the plan */ + // planning_components->execute(); // Execute the plan */ } // Plan #2 visualization: @@ -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,11 +210,11 @@ 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 */ - /* planning_components->execute(); // Execute the plan */ + // planning_components->execute(); // Execute the plan */ } // Plan #3 visualization: @@ -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,11 +253,11 @@ 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 */ - /* planning_components->execute(); // Execute the plan */ + // planning_components->execute(); // Execute the plan */ } // Plan #4 visualization: @@ -238,11 +266,11 @@ 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."); + return 0; } From bbed081460cc5e5f349ab498306b9e872e147ca6 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Wed, 14 Apr 2021 18:13:15 +0000 Subject: [PATCH 4/8] Add default value for rviz_tutorial argument --- doc/quickstart_in_rviz/launch/demo.launch.py | 61 +++++++++++--------- 1 file changed, 35 insertions(+), 26 deletions(-) diff --git a/doc/quickstart_in_rviz/launch/demo.launch.py b/doc/quickstart_in_rviz/launch/demo.launch.py index 944223a37c..55090b6790 100644 --- a/doc/quickstart_in_rviz/launch/demo.launch.py +++ b/doc/quickstart_in_rviz/launch/demo.launch.py @@ -34,10 +34,10 @@ def load_yaml(package_name, file_path): def generate_launch_description(): - # Command-line arguments tutorial_arg = DeclareLaunchArgument( - 'rviz_tutorial', default_value='False', description='Tutorial flag') + "rviz_tutorial", default_value="False", description="Tutorial flag" + ) # planning_context robot_description_config = xacro.process_file( @@ -114,30 +114,38 @@ def generate_launch_description(): ) # RViz - tutorial_mode = LaunchConfiguration('rviz_tutorial') - rviz_base = os.path.join(get_package_share_directory('moveit2_tutorials'), 'launch') - rviz_full_config = os.path.join(rviz_base, 'panda_moveit_config_demo.rviz') - rviz_empty_config = os.path.join(rviz_base, 'panda_moveit_config_demo_empty.rviz') - rviz_node_tutorial = Node(package='rviz2', - executable='rviz2', - name='rviz2', - output='log', - arguments= ['-d', rviz_empty_config], - parameters=[robot_description, - robot_description_semantic, - ompl_planning_pipeline_config, - kinematics_yaml], - condition=IfCondition(tutorial_mode)) - rviz_node = Node(package='rviz2', - executable='rviz2', - name='rviz2', - output='log', - arguments= ['-d', rviz_full_config], - parameters=[robot_description, - robot_description_semantic, - ompl_planning_pipeline_config, - kinematics_yaml], - condition=UnlessCondition(tutorial_mode)) + tutorial_mode = LaunchConfiguration("rviz_tutorial") + rviz_base = os.path.join(get_package_share_directory("moveit2_tutorials"), "launch") + rviz_full_config = os.path.join(rviz_base, "panda_moveit_config_demo.rviz") + rviz_empty_config = os.path.join(rviz_base, "panda_moveit_config_demo_empty.rviz") + rviz_node_tutorial = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_empty_config], + parameters=[ + robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + kinematics_yaml, + ], + condition=IfCondition(tutorial_mode), + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_full_config], + parameters=[ + robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + kinematics_yaml, + ], + condition=UnlessCondition(tutorial_mode), + ) # Static TF static_tf = Node( @@ -198,6 +206,7 @@ def generate_launch_description(): return LaunchDescription( [ + tutorial_arg, rviz_node, rviz_node_tutorial, static_tf, From 2f21bbe4a9c1d5b685465750c0418141ac71d8f8 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Wed, 14 Apr 2021 22:49:40 +0300 Subject: [PATCH 5/8] Update doc/moveit_cpp/src/moveit_cpp_tutorial.cpp --- doc/moveit_cpp/src/moveit_cpp_tutorial.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp b/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp index b13803322f..ad0d52a010 100644 --- a/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp +++ b/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp @@ -124,7 +124,7 @@ int main(int argc, char** argv) visual_tools.trigger(); /* Uncomment if you want to execute the plan */ - // planning_components->execute(); // Execute the plan */ + /* planning_components->execute(); // Execute the plan */ } // Plan #1 visualization: From 08ad0d173e1eadb63d59fbf8e8896202f235b31f Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Wed, 14 Apr 2021 19:52:18 +0000 Subject: [PATCH 6/8] Auto stash before checking out "origin/fix/move_group" --- doc/moveit_cpp/src/moveit_cpp_tutorial.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp b/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp index ad0d52a010..c0ef0a1c4f 100644 --- a/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp +++ b/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp @@ -169,7 +169,7 @@ int main(int argc, char** argv) visual_tools.trigger(); /* Uncomment if you want to execute the plan */ - // planning_components->execute(); // Execute the plan */ + /* planning_components->execute(); // Execute the plan */ } // Plan #2 visualization: @@ -214,7 +214,7 @@ int main(int argc, char** argv) visual_tools.trigger(); /* Uncomment if you want to execute the plan */ - // planning_components->execute(); // Execute the plan */ + /* planning_components->execute(); // Execute the plan */ } // Plan #3 visualization: @@ -257,7 +257,7 @@ int main(int argc, char** argv) visual_tools.trigger(); /* Uncomment if you want to execute the plan */ - // planning_components->execute(); // Execute the plan */ + /* planning_components->execute(); // Execute the plan */ } // Plan #4 visualization: From c3dfd6695b4c78e0313d0760901ba1270082851d Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Thu, 15 Apr 2021 09:32:16 +0000 Subject: [PATCH 7/8] Add ros shutdown to end of tutorial --- doc/moveit_cpp/src/moveit_cpp_tutorial.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp b/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp index c0ef0a1c4f..712dc43fbd 100644 --- a/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp +++ b/doc/moveit_cpp/src/moveit_cpp_tutorial.cpp @@ -272,5 +272,6 @@ int main(int argc, char** argv) /* visual_tools.prompt("Press 'next' to end the demo"); */ RCLCPP_INFO(LOGGER, "Shutting down."); + rclcpp::shutdown(); return 0; } From f74e0aa5cd79562faa22cabd4e62a7a736503ed1 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Thu, 15 Apr 2021 17:13:32 +0300 Subject: [PATCH 8/8] Update move_group_interface_tutorial.rst --- doc/move_group_interface/move_group_interface_tutorial.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/move_group_interface/move_group_interface_tutorial.rst b/doc/move_group_interface/move_group_interface_tutorial.rst index 47e403ad4b..8b4c25471c 100644 --- a/doc/move_group_interface/move_group_interface_tutorial.rst +++ b/doc/move_group_interface/move_group_interface_tutorial.rst @@ -12,7 +12,7 @@ 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. +**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