From 4039a0689f7f99dcaafdcb99552f82992d85d993 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 12 Jan 2021 14:22:32 +0100 Subject: [PATCH] Added moveit2 demo Signed-off-by: ahcorde --- rrbot_description/CMakeLists.txt | 7 + rrbot_description/package.xml | 18 ++ rrbot_description/urdf/rrbot.urdf | 214 +++++++++++++ rrbot_moveit_config/CMakeLists.txt | 7 + rrbot_moveit_config/config/kinematics.yaml | 4 + rrbot_moveit_config/config/moveit_cpp.yaml | 20 ++ rrbot_moveit_config/config/ompl_planning.yaml | 155 ++++++++++ .../config/ros_controllers.yaml | 10 + rrbot_moveit_config/config/rrbot.srdf | 30 ++ rrbot_moveit_config/config/rviz_rrbot.rviz | 292 ++++++++++++++++++ .../launch/rrbot_demo.launch.py | 128 ++++++++ rrbot_moveit_config/package.xml | 18 ++ 12 files changed, 903 insertions(+) create mode 100644 rrbot_description/CMakeLists.txt create mode 100644 rrbot_description/package.xml create mode 100644 rrbot_description/urdf/rrbot.urdf create mode 100644 rrbot_moveit_config/CMakeLists.txt create mode 100644 rrbot_moveit_config/config/kinematics.yaml create mode 100644 rrbot_moveit_config/config/moveit_cpp.yaml create mode 100644 rrbot_moveit_config/config/ompl_planning.yaml create mode 100644 rrbot_moveit_config/config/ros_controllers.yaml create mode 100644 rrbot_moveit_config/config/rrbot.srdf create mode 100644 rrbot_moveit_config/config/rviz_rrbot.rviz create mode 100644 rrbot_moveit_config/launch/rrbot_demo.launch.py create mode 100644 rrbot_moveit_config/package.xml diff --git a/rrbot_description/CMakeLists.txt b/rrbot_description/CMakeLists.txt new file mode 100644 index 00000000..b4a927ad --- /dev/null +++ b/rrbot_description/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.10.2) +project(rrbot_description) +find_package(ament_cmake REQUIRED) + +ament_package() + +install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) diff --git a/rrbot_description/package.xml b/rrbot_description/package.xml new file mode 100644 index 00000000..4fc0e2bf --- /dev/null +++ b/rrbot_description/package.xml @@ -0,0 +1,18 @@ + + + rrbot_description + 0.0.0 + Rrbot Resources used for MoveIt testing + + Alejandro + + Alejandro + + BSD + + ament_cmake + + + ament_cmake + + diff --git a/rrbot_description/urdf/rrbot.urdf b/rrbot_description/urdf/rrbot.urdf new file mode 100644 index 00000000..95654211 --- /dev/null +++ b/rrbot_description/urdf/rrbot.urdf @@ -0,0 +1,214 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gazebo_ros2_control/GazeboSystem + /home/ahcorde/ros2_control_ws_foxy/rrbot.yaml + 0.001 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Black + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Red + + diff --git a/rrbot_moveit_config/CMakeLists.txt b/rrbot_moveit_config/CMakeLists.txt new file mode 100644 index 00000000..0bee5592 --- /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 launch 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 00000000..114327b5 --- /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/moveit_cpp.yaml b/rrbot_moveit_config/config/moveit_cpp.yaml new file mode 100644 index 00000000..830e389a --- /dev/null +++ b/rrbot_moveit_config/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_config/config/ompl_planning.yaml b/rrbot_moveit_config/config/ompl_planning.yaml new file mode 100644 index 00000000..de238609 --- /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/ros_controllers.yaml b/rrbot_moveit_config/config/ros_controllers.yaml new file mode 100644 index 00000000..072eacf3 --- /dev/null +++ b/rrbot_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,10 @@ +controller_names: + - rrbot_arm_controller + +rrbot_arm_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - single_rrbot_joint1 + - single_rrbot_joint2 diff --git a/rrbot_moveit_config/config/rrbot.srdf b/rrbot_moveit_config/config/rrbot.srdf new file mode 100644 index 00000000..a32a612d --- /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 00000000..199fbea0 --- /dev/null +++ b/rrbot_moveit_config/config/rviz_rrbot.rviz @@ -0,0 +1,292 @@ +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 + 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 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: single_rrbot_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: 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: + " - Trajectory Slider": + collapsed: false + Displays: + collapsed: false + Height: 812 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000216000002d2fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002d2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100ffffff000000010000010f000002d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002cc000002d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1533 + X: 255 + Y: 1107 diff --git a/rrbot_moveit_config/launch/rrbot_demo.launch.py b/rrbot_moveit_config/launch/rrbot_demo.launch.py new file mode 100644 index 00000000..f72a1b34 --- /dev/null +++ b/rrbot_moveit_config/launch/rrbot_demo.launch.py @@ -0,0 +1,128 @@ +# Copyright 2020 ROS2-Control Development Team (2020) +# +# 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 = get_package_share_directory('rrbot_moveit_config') + '/config/moveit_cpp.yaml' + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']), + ) + + robot_description_config = load_file('rrbot_description', 'urdf/rrbot.urdf') + robot_description = {'robot_description': robot_description_config} + + robot_description_semantic_config = load_file('rrbot_moveit_config', 'config/rrbot.srdf') + robot_description_semantic = {'robot_description_semantic': robot_description_semantic_config} + + kinematics_yaml = load_yaml('rrbot_moveit_config', 'config/kinematics.yaml') + + controllers_yaml = load_yaml('rrbot_moveit_config', 'config/ros_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', '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='run_moveit_cpp', + # 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]) + + 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') + + # RViz + rviz_config_file = 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]) + # 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', 'single_rrbot_link1']) + + return LaunchDescription([ + gazebo, + node_robot_state_publisher, + run_moveit_cpp_node, + rviz_node, + static_tf, + spawn_entity + ]) diff --git a/rrbot_moveit_config/package.xml b/rrbot_moveit_config/package.xml new file mode 100644 index 00000000..124c8d84 --- /dev/null +++ b/rrbot_moveit_config/package.xml @@ -0,0 +1,18 @@ + + + rrbot_moveit_config + 0.0.0 + Rrbot files used for MoveIt config + + Alejandro + + Alejandro + + BSD + + ament_cmake + + + ament_cmake + +