diff --git a/README.md b/README.md index dae3da7..83f6c24 100644 --- a/README.md +++ b/README.md @@ -1,32 +1,20 @@ # Gazebo ROS Demos -* Author: Dave Coleman * License: GNU General Public License, version 3 (GPL-3.0) -Example robots and code for interfacing Gazebo with ROS - -## Tutorials - -[ROS URDF](http://gazebosim.org/tutorials/?tut=ros_urdf) +Example robots and code for interfacing Gazebo, ROS2 and Moveit2 ## Quick Start -Rviz: - - roslaunch rrbot_description rrbot_rviz.launch - Gazebo: - roslaunch rrbot_gazebo rrbot_world.launch - -ROS Control: + ros2 launch rrbot_moveit_demo_nodes rrbot_demo.launch.py - roslaunch rrbot_control rrbot_control.launch +## Develop and Contribute -Example of Moving Joints: +We welcome any contributions to this repo and encourage you to fork the project then send pull requests back to this parent repo. Thanks for your help! - rostopic pub /rrbot/joint2_position_controller/command std_msgs/Float64 "data: -0.9" -## Develop and Contribute +## Notes -We welcome any contributions to this repo and encourage you to fork the project then send pull requests back to this parent repo. Thanks for your help! +`gazebo_tutorials` and `rrbot_control` have not yet been ported to ROS2, but need to be. diff --git a/gazebo_tutorials/COLCON_IGNORE b/gazebo_tutorials/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/rrbot_control/COLCON_IGNORE b/rrbot_control/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/rrbot_description/CMakeLists.txt b/rrbot_description/CMakeLists.txt index d1aaebb..2b6d152 100644 --- a/rrbot_description/CMakeLists.txt +++ b/rrbot_description/CMakeLists.txt @@ -1,15 +1,7 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.10.2) project(rrbot_description) +find_package(ament_cmake REQUIRED) -find_package(catkin REQUIRED) +ament_package() -catkin_package() - -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -install(DIRECTORY meshes - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -install(DIRECTORY urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY urdf meshes DESTINATION share/${PROJECT_NAME}) diff --git a/rrbot_description/launch/rrbot.rviz b/rrbot_description/launch/rrbot.rviz deleted file mode 100644 index 372b748..0000000 --- a/rrbot_description/launch/rrbot.rviz +++ /dev/null @@ -1,202 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.5 - Tree Height: 257 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679016 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: LaserScan -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.0299999993 - 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 - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - hokuyo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/LaserScan - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: -999999 - Min Color: 0; 0; 0 - Min Intensity: 999999 - Name: LaserScan - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.00999999978 - Style: Flat Squares - Topic: /rrbot/laser/scan - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Camera - Enabled: true - Image Rendering: background and overlay - Image Topic: /rrbot/camera1/image_raw - Name: Camera - Overlay Alpha: 0.5 - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - Visibility: - Grid: true - LaserScan: true - RobotModel: true - Value: true - Zoom Factor: 1 - Enabled: true - Global Options: - Background Color: 238; 238; 238 - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/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 - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 10.3891287 - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.543884933 - Y: -0.380490273 - Z: 0.564803362 - Focal Shape Fixed Size: false - Focal Shape Size: 0.0500000007 - Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.845398188 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.89539808 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 876 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000028a00000326fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000190000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000001600ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d00650000000000000006400000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003b00000032600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1600 - X: 0 - Y: 24 diff --git a/rrbot_description/launch/rrbot_rviz.launch b/rrbot_description/launch/rrbot_rviz.launch deleted file mode 100644 index 3b65916..0000000 --- a/rrbot_description/launch/rrbot_rviz.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/rrbot_description/package.xml b/rrbot_description/package.xml index b7a16ce..540bb1c 100644 --- a/rrbot_description/package.xml +++ b/rrbot_description/package.xml @@ -1,7 +1,7 @@ rrbot_description - 0.0.0 + 0.2.0 The rrbot_description package Dave Coleman @@ -14,12 +14,10 @@ Dave Coleman - catkin - - joint_state_publisher - robot_state_publisher - rviz + ament_cmake + ament_cmake + diff --git a/rrbot_description/urdf/rrbot.gazebo b/rrbot_description/urdf/rrbot.gazebo index 4d3dd58..8bb953a 100644 --- a/rrbot_description/urdf/rrbot.gazebo +++ b/rrbot_description/urdf/rrbot.gazebo @@ -3,9 +3,9 @@ - - /rrbot - gazebo_ros_control/DefaultRobotHWSim + + gazebo_ros2_control/GazeboSystem + $(find rrbot_moveit_demo_nodes)/config/gazebo_controllers.yaml @@ -65,9 +65,12 @@ 0.01 - - /rrbot/laser/scan - hokuyo_link + + + /rrbot + ~/out:=laser/scan + + sensor_msgs/LaserScan diff --git a/rrbot_description/urdf/rrbot.xacro b/rrbot_description/urdf/rrbot.xacro index dadb935..9753c54 100644 --- a/rrbot_description/urdf/rrbot.xacro +++ b/rrbot_description/urdf/rrbot.xacro @@ -4,18 +4,25 @@ - - - - - - - + + + + + + + + + + + + + + - + - + @@ -30,14 +37,14 @@ - + - + @@ -45,10 +52,7 @@ - + @@ -65,14 +69,14 @@ - + - + @@ -80,10 +84,7 @@ - + @@ -100,14 +101,14 @@ - + - + @@ -115,15 +116,12 @@ - + - + @@ -134,26 +132,26 @@ - + - + - + - + - + @@ -164,58 +162,58 @@ - + - + - + - + + so that ros and opencv can operate on the camera frame correctly --> - + - - + - - transmission_interface/SimpleTransmission + + + gazebo_ros2_control/GazeboSystem + - hardware_interface/EffortJointInterface + + -6.28 + 6.28 + + + + - - hardware_interface/EffortJointInterface - 1 - - - - - transmission_interface/SimpleTransmission - hardware_interface/EffortJointInterface + + -6.28 + 6.28 + + + + - - hardware_interface/EffortJointInterface - 1 - - - + diff --git a/rrbot_gazebo/CMakeLists.txt b/rrbot_gazebo/CMakeLists.txt index 83e9fa8..2d94d86 100644 --- a/rrbot_gazebo/CMakeLists.txt +++ b/rrbot_gazebo/CMakeLists.txt @@ -1,12 +1,12 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(rrbot_gazebo) -find_package(catkin REQUIRED) - -catkin_package() +find_package(ament_cmake REQUIRED) install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - + DESTINATION share/${PROJECT_NAME} +) install(DIRECTORY worlds - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + DESTINATION share/${PROJECT_NAME} +) +ament_package() diff --git a/rrbot_gazebo/launch/rrbot_world.launch b/rrbot_gazebo/launch/rrbot_world.launch deleted file mode 100644 index ef53859..0000000 --- a/rrbot_gazebo/launch/rrbot_world.launch +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/rrbot_gazebo/launch/rrbot_world.launch.py b/rrbot_gazebo/launch/rrbot_world.launch.py new file mode 100644 index 0000000..994cc95 --- /dev/null +++ b/rrbot_gazebo/launch/rrbot_world.launch.py @@ -0,0 +1,124 @@ +# Copyright 2021 Open Robotics (2021) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource + +import xacro +import yaml + + +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 + rrbot_gazebo = os.path.join( + get_package_share_directory('rrbot_gazebo'), + 'worlds', + 'rrbot.world') + + print(rrbot_gazebo) + + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']), + launch_arguments={'world': rrbot_gazebo}.items(), + ) + + rrbot_description_path = os.path.join( + get_package_share_directory('rrbot_description')) + + xacro_file = os.path.join(rrbot_description_path, + 'urdf', + 'rrbot.xacro') + + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + robot_description_config = doc.toxml() + robot_description = {'robot_description': robot_description_config} + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[robot_description] + ) + + spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', + arguments=['-topic', 'robot_description', + '-entity', 'rrbot'], + output='screen') + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_start_controller', 'joint_state_controller'], + output='screen' + ) + + load_joint_trajectory_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_start_controller', 'joint_trajectory_controller'], + output='screen' + ) + + # 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', 'link1']) + + return LaunchDescription([ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[load_joint_state_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_joint_trajectory_controller], + ) + ), + gazebo, + node_robot_state_publisher, + static_tf, + spawn_entity + ]) diff --git a/rrbot_gazebo/package.xml b/rrbot_gazebo/package.xml index 83ef638..927163a 100644 --- a/rrbot_gazebo/package.xml +++ b/rrbot_gazebo/package.xml @@ -1,7 +1,11 @@ - + + + rrbot_gazebo - 0.1.0 - rrbot_gazebo + 0.2.0 + + Launch files and worlds to launch the rrbot in Gazebo, this will publish the joint states in ROS 2. + Dave Coleman BSD @@ -12,15 +16,16 @@ Dave Coleman - catkin + ament_cmake - gazebo_plugins - gazebo_ros - gazebo_ros_control - rrbot_control - rrbot_description - xacro + gazebo_plugins + gazebo_ros + gazebo_ros2_control + robot_state_publisher + rrbot_description + xacro + ament_cmake diff --git a/rrbot_moveit_config/CMakeLists.txt b/rrbot_moveit_config/CMakeLists.txt new file mode 100644 index 0000000..116b4b2 --- /dev/null +++ b/rrbot_moveit_config/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.10.2) +project(rrbot_moveit_config) +find_package(ament_cmake REQUIRED) + +ament_package() + +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) diff --git a/rrbot_moveit_config/config/kinematics.yaml b/rrbot_moveit_config/config/kinematics.yaml new file mode 100644 index 0000000..114327b --- /dev/null +++ b/rrbot_moveit_config/config/kinematics.yaml @@ -0,0 +1,4 @@ +rrbot_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 diff --git a/rrbot_moveit_config/config/ompl_planning.yaml b/rrbot_moveit_config/config/ompl_planning.yaml new file mode 100644 index 0000000..de23860 --- /dev/null +++ b/rrbot_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,155 @@ +planner_configs: + AnytimePathShortening: + type: geometric::AnytimePathShortening + shortcut: true # Attempt to shortcut all new solution paths + hybridize: true # Compute hybrid solution trajectories + max_hybrid_paths: 24 # Number of hybrid paths generated per iteration + num_planners: 4 # The number of default planners to use for planning + planners: "" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 +rrbot_arm: + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo diff --git a/rrbot_moveit_config/config/rrbot.srdf b/rrbot_moveit_config/config/rrbot.srdf new file mode 100644 index 0000000..5646baa --- /dev/null +++ b/rrbot_moveit_config/config/rrbot.srdf @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rrbot_moveit_config/config/rviz_rrbot.rviz b/rrbot_moveit_config/config/rviz_rrbot.rviz new file mode 100644 index 0000000..c0a1fef --- /dev/null +++ b/rrbot_moveit_config/config/rviz_rrbot.rviz @@ -0,0 +1,240 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PlanningScene1 + - /Trajectory1 + Splitter Ratio: 0.5 + Tree Height: 583 + - 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: /moveit_cpp/publish_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 + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + 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 + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + 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 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: link1 + 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: 10.540088653564453 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.10164402425289154 + Y: -0.05426773428916931 + Z: 0.09421030431985855 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8853975534439087 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.49039822816848755 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 812 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000216000002d2fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002d2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100ffffff000000010000010f000002d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002cc000002d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1533 + X: 1920 + Y: 207 diff --git a/rrbot_moveit_config/package.xml b/rrbot_moveit_config/package.xml new file mode 100644 index 0000000..8833043 --- /dev/null +++ b/rrbot_moveit_config/package.xml @@ -0,0 +1,20 @@ + + + rrbot_moveit_config + 0.2.0 + Rrbot files used for MoveIt config + + Alejandro + + Alejandro + + BSD + + ament_cmake + + rviz2 + + + ament_cmake + + diff --git a/rrbot_moveit_demo_nodes/CMakeLists.txt b/rrbot_moveit_demo_nodes/CMakeLists.txt new file mode 100644 index 0000000..4cd7b08 --- /dev/null +++ b/rrbot_moveit_demo_nodes/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 3.5) +project(rrbot_moveit_demo_nodes) + +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(moveit_common REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +# This shouldn't be necessary (required by moveit_simple_controller_manager) +find_package(rclcpp REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(trajectory_msgs REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_executable(run_moveit_cpp src/run_moveit_cpp.cpp) +ament_target_dependencies(run_moveit_cpp + moveit_msgs + moveit_ros_planning_interface + rclcpp + trajectory_msgs +) + +install(TARGETS run_moveit_cpp + EXPORT export_${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/rrbot_moveit_demo_nodes/config/controllers.yaml b/rrbot_moveit_demo_nodes/config/controllers.yaml new file mode 100644 index 0000000..74f9960 --- /dev/null +++ b/rrbot_moveit_demo_nodes/config/controllers.yaml @@ -0,0 +1,10 @@ +controller_names: + - joint_trajectory_controller + +joint_trajectory_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - joint1 + - joint2 diff --git a/rrbot_moveit_demo_nodes/config/gazebo_controllers.yaml b/rrbot_moveit_demo_nodes/config/gazebo_controllers.yaml new file mode 100644 index 0000000..cc8e3d4 --- /dev/null +++ b/rrbot_moveit_demo_nodes/config/gazebo_controllers.yaml @@ -0,0 +1,16 @@ +controller_manager: + ros__parameters: + update_rate: 1000 # Hz + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_controller: + type: joint_state_controller/JointStateController + +joint_trajectory_controller: + ros__parameters: + joints: + - joint1 + - joint2 + interface_name: position diff --git a/rrbot_moveit_demo_nodes/config/moveit_cpp.yaml b/rrbot_moveit_demo_nodes/config/moveit_cpp.yaml new file mode 100644 index 0000000..830e389 --- /dev/null +++ b/rrbot_moveit_demo_nodes/config/moveit_cpp.yaml @@ -0,0 +1,20 @@ +run_moveit_cpp: + 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: 10 + planning_pipeline: ompl + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 diff --git a/rrbot_moveit_demo_nodes/launch/rrbot_demo.launch.py b/rrbot_moveit_demo_nodes/launch/rrbot_demo.launch.py new file mode 100644 index 0000000..8e6edd9 --- /dev/null +++ b/rrbot_moveit_demo_nodes/launch/rrbot_demo.launch.py @@ -0,0 +1,124 @@ +# Copyright 2021 Open Robotics (2021) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + +import xacro +import yaml + + +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 = os.path.join( + get_package_share_directory('rrbot_moveit_demo_nodes'), + 'config', + 'moveit_cpp.yaml') + + rrbot_description_path = os.path.join( + get_package_share_directory('rrbot_description')) + + xacro_file = os.path.join(rrbot_description_path, + 'urdf', + 'rrbot.xacro') + + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + robot_description_config = doc.toxml() + robot_description = {'robot_description': robot_description_config} + + robot_description_semantic_config = load_file('rrbot_moveit_config', + os.path.join('config', 'rrbot.srdf')) + robot_description_semantic = {'robot_description_semantic': robot_description_semantic_config} + + kinematics_yaml = load_yaml('rrbot_moveit_config', + os.path.join('config', 'kinematics.yaml')) + + controllers_yaml = load_yaml('rrbot_moveit_demo_nodes', + os.path.join('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('rrbot_moveit_config', + os.path.join('config', 'ompl_planning.yaml')) + ompl_planning_pipeline_config['ompl'].update(ompl_planning_yaml) + + # MoveItCpp demo executable + run_moveit_cpp_node = Node(name='run_moveit_cpp', + package='rrbot_moveit_demo_nodes', + # TODO(henningkayser): add debug argument + # prefix='xterm -e gdb --args', + executable='run_moveit_cpp', + 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 = os.path.join( + get_package_share_directory('rrbot_moveit_config'), 'config', 'rviz_rrbot.rviz') + rviz_node = Node(package='rviz2', + executable='rviz2', + name='rviz2', + output='log', + arguments=['-d', rviz_config_file], + parameters=[robot_description, + robot_description_semantic]) + + return LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + os.path.join(get_package_share_directory('rrbot_gazebo'), 'launch'), '/rrbot_world.launch.py' + ]), + ), + run_moveit_cpp_node, + rviz_node, + ]) diff --git a/rrbot_moveit_demo_nodes/package.xml b/rrbot_moveit_demo_nodes/package.xml new file mode 100644 index 0000000..84c0cac --- /dev/null +++ b/rrbot_moveit_demo_nodes/package.xml @@ -0,0 +1,34 @@ + + + + rrbot_moveit_demo_nodes + 0.2.0 + + Config and launch files to run a MoveIt2 demo with the rrbot. + This example will use Gazebo classic to simulate the robot and ROS 2 to communicate the rrbot with MoveIt2. + + Alejandro Hernández + Apache 2 + BSD + + ament_cmake + + moveit_common + moveit_msgs + moveit_ros_planning_interface + rclcpp + trajectory_msgs + + rrbot_description + rrbot_gazebo + rrbot_moveit_config + rviz2 + xacro + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/rrbot_moveit_demo_nodes/src/run_moveit_cpp.cpp b/rrbot_moveit_demo_nodes/src/run_moveit_cpp.cpp new file mode 100644 index 0000000..08709ec --- /dev/null +++ b/rrbot_moveit_demo_nodes/src/run_moveit_cpp.cpp @@ -0,0 +1,117 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Henning Kayser + This code was taken from the moveit 2 repository and mofied to work with rrbot_arm + https://github.com/ros-planning/moveit2/tree/main/moveit_demo_nodes/run_moveit_cpp/src + Desc: A simple demo node running MoveItCpp for planning and execution +*/ + +#include +#include +#include +#include +#include +#include +#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_cpp_demo"); + +class MoveItCppDemo +{ +public: + MoveItCppDemo(const rclcpp::Node::SharedPtr& node) + : node_(node) + , robot_state_publisher_(node_->create_publisher("display_robot_state", 1)) + { + } + + void run() + { + RCLCPP_INFO(LOGGER, "Initialize MoveItCpp"); + moveit_cpp_ = std::make_shared(node_); + moveit_cpp_->getPlanningSceneMonitor()->providePlanningSceneService(); // let RViz display query PlanningScene + moveit_cpp_->getPlanningSceneMonitor()->setPlanningScenePublishingFrequency(100); + + RCLCPP_INFO(LOGGER, "Initialize PlanningComponent"); + moveit::planning_interface::PlanningComponent arm("rrbot_arm", moveit_cpp_); + + + // A little delay before running the plan + rclcpp::sleep_for(std::chrono::seconds(3)); + + // Set joint state goal + RCLCPP_INFO(LOGGER, "Set goal"); + arm.setGoal("extended"); + + // Run actual plan + RCLCPP_INFO(LOGGER, "Plan to goal"); + const auto plan_solution = arm.plan(); + if (plan_solution) + { + RCLCPP_INFO(LOGGER, "arm.execute()"); + arm.execute(); + } + } + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr robot_state_publisher_; + moveit::planning_interface::MoveItCppPtr moveit_cpp_; +}; + +int main(int argc, char** argv) +{ + RCLCPP_INFO(LOGGER, "Initialize node"); + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + // 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); + + MoveItCppDemo demo(node); + std::thread run_demo([&demo]() { + // Let RViz initialize before running demo + // TODO(henningkayser): use lifecycle events to launch node + rclcpp::sleep_for(std::chrono::seconds(5)); + demo.run(); + }); + + rclcpp::spin(node); + run_demo.join(); + + return 0; +}