diff --git a/CMakeLists.txt b/CMakeLists.txt index d678bc9f0d..c0e57a94ee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,7 +68,7 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) # add_subdirectory(doc/tests) # add_subdirectory(doc/trajopt_planner) # add_subdirectory(doc/creating_moveit_plugins/lerp_motion_planner) -# add_subdirectory(doc/moveit_cpp) +add_subdirectory(doc/moveit_cpp) # add_subdirectory(doc/collision_environments) # add_subdirectory(doc/visualizing_collisions) # add_subdirectory(doc/bullet_collision_checker) 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/controllers.yaml b/doc/moveit_cpp/config/controllers.yaml new file mode 100644 index 0000000000..58ea42051d --- /dev/null +++ b/doc/moveit_cpp/config/controllers.yaml @@ -0,0 +1,15 @@ +controller_names: + - panda_arm_controller + +panda_arm_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 diff --git a/doc/moveit_cpp/config/moveit_cpp.yaml b/doc/moveit_cpp/config/moveit_cpp.yaml index 9a69f0dd1d..43bcf4d3bb 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 - -planning_pipelines: - pipeline_names: - - ompl - -plan_request_params: - planning_attempts: 1 - planning_pipeline: ompl - max_velocity_scaling_factor: 1.0 - max_acceleration_scaling_factor: 1.0 +moveit_cpp_tutorial: + ros__parameters: + planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" + publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + + planning_pipelines: + #namespace: "moveit_cpp" # optional, default is ~ + pipeline_names: ["ompl"] + + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 \ No newline at end of file diff --git a/doc/moveit_cpp/config/panda_controllers.yaml b/doc/moveit_cpp/config/panda_controllers.yaml new file mode 100644 index 0000000000..bc6e45d277 --- /dev/null +++ b/doc/moveit_cpp/config/panda_controllers.yaml @@ -0,0 +1,18 @@ +panda_arm_controller: + ros__parameters: + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + write_op_modes: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 \ No newline at end of file diff --git a/doc/moveit_cpp/config/start_positions.yaml b/doc/moveit_cpp/config/start_positions.yaml new file mode 100644 index 0000000000..f4553b3e33 --- /dev/null +++ b/doc/moveit_cpp/config/start_positions.yaml @@ -0,0 +1,19 @@ +fake_joint_driver_node: + ros__parameters: + start_position: + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + values: + - 0.0 + - -0.785 + - 0.0 + - -2.356 + - 0.0 + - 1.571 + - 0.785 \ No newline at end of file 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..c78f250648 --- /dev/null +++ b/doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py @@ -0,0 +1,101 @@ +import os +import yaml +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + # moveit_cpp.yaml is passed by filename for now since it's node specific + moveit_cpp_yaml_file_name = get_package_share_directory('moveit2_tutorials') + "/config/moveit_cpp.yaml" + + # Component yaml files are grouped in separate namespaces + robot_description_config = load_file('moveit_resources_panda_description', 'urdf/panda.urdf') + robot_description = {'robot_description' : robot_description_config} + + robot_description_semantic_config = load_file('moveit_resources_panda_moveit_config', 'config/panda.srdf') + robot_description_semantic = {'robot_description_semantic' : robot_description_semantic_config} + + kinematics_yaml = load_yaml('moveit_resources_panda_moveit_config', 'config/kinematics.yaml') + robot_description_kinematics = { 'robot_description_kinematics' : kinematics_yaml } + + controllers_yaml = load_yaml('moveit2_tutorials', 'config/controllers.yaml') + moveit_controllers = { 'moveit_simple_controller_manager' : controllers_yaml, + 'moveit_controller_manager': 'moveit_simple_controller_manager/MoveItSimpleControllerManager'} + + ompl_planning_pipeline_config = { 'ompl' : { + 'planning_plugin' : 'ompl_interface/OMPLPlanner', + 'request_adapters' : """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""" , + 'start_state_max_bounds_error' : 0.1 } } + ompl_planning_yaml = load_yaml('moveit_resources_panda_moveit_config', 'config/ompl_planning.yaml') + ompl_planning_pipeline_config['ompl'].update(ompl_planning_yaml) + + # MoveItCpp demo executable + moveit_cpp_node = Node(name='moveit_cpp_tutorial', + package='moveit2_tutorials', + prefix='xterm -e', + executable='moveit_cpp_tutorial', + output='screen', + parameters=[moveit_cpp_yaml_file_name, + robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_pipeline_config, + moveit_controllers]) + + # RViz + rviz_config_file = get_package_share_directory('moveit2_tutorials') + "/launch/moveit_cpp_tutorial.rviz" + rviz_node = Node(package='rviz2', + executable='rviz2', + name='rviz2', + output='log', + arguments=['-d', rviz_config_file], + parameters=[robot_description, + robot_description_semantic]) + + # Static TF + static_tf = Node(package='tf2_ros', + executable='static_transform_publisher', + name='static_transform_publisher', + output='log', + arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'panda_link0']) + + # Publish TF + robot_state_publisher = Node(package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='both', + parameters=[robot_description]) + + # Fake joint driver + fake_joint_driver_node = Node(package='fake_joint_driver', + executable='fake_joint_driver_node', + # TODO(JafarAbdi): Why this launch the two nodes (controller manager and the fake joint driver) with the same name! + # name='fake_joint_driver_node', + parameters=[{'controller_name': 'panda_arm_controller'}, + os.path.join(get_package_share_directory("moveit2_tutorials"), "config", "panda_controllers.yaml"), + os.path.join(get_package_share_directory("moveit2_tutorials"), "config", "start_positions.yaml"), + robot_description] + ) + + return LaunchDescription([ static_tf, robot_state_publisher, rviz_node, moveit_cpp_node, fake_joint_driver_node ]) diff --git a/doc/moveit_cpp/launch/moveit_cpp_tutorial.rviz b/doc/moveit_cpp/launch/moveit_cpp_tutorial.rviz index 9bd99e197f..d92043fd4d 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: /moeit_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..665923651a 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", "moeit_cpp_tutorial", node); + /* moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0", rvt::RVIZ_MARKER_TOPIC, + moveit_cpp_ptr->getPlanningSceneMonitor()); */ visual_tools.deleteAllMarkers(); - visual_tools.loadRemoteControl(); + /* visual_tools.loadRemoteControl(); */ Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); text_pose.translation().z() = 1.75; @@ -56,7 +83,8 @@ int main(int argc, char** argv) // Start the demo // ^^^^^^^^^^^^^^^^^^^^^^^^^ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); + prompt("Press 'Enter' to start the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); */ // Planning with MoveItCpp // ^^^^^^^^^^^^^^^^^^^^^^^ @@ -70,7 +98,7 @@ int main(int argc, char** argv) planning_components->setStartStateToCurrentState(); // The first way to set the goal of the plan is by using geometry_msgs::PoseStamped ROS message type as follow - geometry_msgs::PoseStamped target_pose1; + geometry_msgs::msg::PoseStamped target_pose1; target_pose1.header.frame_id = "panda_link0"; target_pose1.pose.orientation.w = 1.0; target_pose1.pose.position.x = 0.28; @@ -92,7 +120,7 @@ int main(int argc, char** argv) visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose"); visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE); // Visualize the trajectory in Rviz - visual_tools.publishTrajectoryLine(plan_solution1.trajectory, joint_model_group_ptr); + /* visual_tools.publishTrajectoryLine(plan_solution1.trajectory, joint_model_group_ptr); */ visual_tools.trigger(); /* Uncomment if you want to execute the plan */ @@ -105,10 +133,10 @@ int main(int argc, char** argv) // :width: 250pt // :align: center // - // Start the next plan visual_tools.deleteAllMarkers(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + prompt("Press 'Enter' to continue the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ // Plan #2 // ^^^^^^^ @@ -116,7 +144,7 @@ int main(int argc, char** argv) // Here we will set the current state of the plan using // moveit::core::RobotState auto start_state = *(moveit_cpp_ptr->getCurrentState()); - geometry_msgs::Pose start_pose; + geometry_msgs::msg::Pose start_pose; start_pose.orientation.w = 1.0; start_pose.position.x = 0.55; start_pose.position.y = 0.0; @@ -137,7 +165,7 @@ int main(int argc, char** argv) visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose"); visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE); visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose"); - visual_tools.publishTrajectoryLine(plan_solution2.trajectory, joint_model_group_ptr); + /* visual_tools.publishTrajectoryLine(plan_solution2.trajectory, joint_model_group_ptr); */ visual_tools.trigger(); /* Uncomment if you want to execute the plan */ @@ -150,10 +178,10 @@ int main(int argc, char** argv) // :width: 250pt // :align: center // - // Start the next plan visual_tools.deleteAllMarkers(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + prompt("Press 'Enter' to continue the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ // Plan #3 // ^^^^^^^ @@ -161,7 +189,7 @@ int main(int argc, char** argv) // We can also set the goal of the plan using // moveit::core::RobotState auto target_state = *robot_start_state; - geometry_msgs::Pose target_pose2; + geometry_msgs::msg::Pose target_pose2; target_pose2.orientation.w = 1.0; target_pose2.position.x = 0.55; target_pose2.position.y = -0.05; @@ -182,7 +210,7 @@ int main(int argc, char** argv) visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose"); visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE); visual_tools.publishAxisLabeled(target_pose2, "target_pose"); - visual_tools.publishTrajectoryLine(plan_solution3.trajectory, joint_model_group_ptr); + /* visual_tools.publishTrajectoryLine(plan_solution3.trajectory, joint_model_group_ptr); */ visual_tools.trigger(); /* Uncomment if you want to execute the plan */ @@ -195,10 +223,10 @@ int main(int argc, char** argv) // :width: 250pt // :align: center // - // Start the next plan visual_tools.deleteAllMarkers(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + prompt("Press 'Enter' to continue the demo"); + /* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */ // Plan #4 // ^^^^^^^ @@ -225,7 +253,7 @@ int main(int argc, char** argv) visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose"); visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE); visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("panda_link8"), "target_pose"); - visual_tools.publishTrajectoryLine(plan_solution4.trajectory, joint_model_group_ptr); + /* visual_tools.publishTrajectoryLine(plan_solution4.trajectory, joint_model_group_ptr); */ visual_tools.trigger(); /* Uncomment if you want to execute the plan */ @@ -238,11 +266,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 continue 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; }