diff --git a/README.md b/README.md index f0f4255..06024ef 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,3 @@ # Robossembler ROS2 -Repo for ROS2 packages related to Robossembler - -## Known issues - -Controller manager can not loaded with `gazebo_ros2_control/GazeboSystem` but MoveitCpp and controller manager completely loaded with `fake_components/GenericSystem` in `ros2_control` tag in file `rasms_descroption/urdf/rasms.control.xacro` \ No newline at end of file +Repo for ROS2 packages related to Robossembler \ No newline at end of file diff --git a/config/joint_limits.yml b/config/joint_limits.yml deleted file mode 100644 index 8227e0a..0000000 --- a/config/joint_limits.yml +++ /dev/null @@ -1,35 +0,0 @@ -# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed - -# For beginners, we downscale velocity and acceleration limits. -# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 0.1 -default_acceleration_scaling_factor: 0.1 - -# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] -# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] -joint_limits: - joint1: - has_velocity_limits: false - max_velocity: 0.0 - has_acceleration_limits: false - max_acceleration: 0 - joint3: - has_velocity_limits: false - max_velocity: 0.0 - has_acceleration_limits: false - max_acceleration: 0 - joint4: - has_velocity_limits: false - max_velocity: 0.0 - has_acceleration_limits: false - max_acceleration: 0 - joint5: - has_velocity_limits: false - max_velocity: 0.0 - has_acceleration_limits: false - max_acceleration: 0 - joint6: - has_velocity_limits: false - max_velocity: 0.0 - has_acceleration_limits: false - max_acceleration: 0 \ No newline at end of file diff --git a/config/kinematics.yml b/config/kinematics.yml deleted file mode 100644 index 29496db..0000000 --- a/config/kinematics.yml +++ /dev/null @@ -1,4 +0,0 @@ -rasms_arm_group: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.05 diff --git a/config/moveit_cpp.yml b/config/moveit_cpp.yml deleted file mode 100644 index d1f6682..0000000 --- a/config/moveit_cpp.yml +++ /dev/null @@ -1,20 +0,0 @@ -rasms_moveit: - ros__parameters: - 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: - #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 \ No newline at end of file diff --git a/config/ompl_planning.yml b/config/ompl_planning.yml deleted file mode 100644 index 661cf4c..0000000 --- a/config/ompl_planning.yml +++ /dev/null @@ -1,151 +0,0 @@ -planner_configs: - SBLkConfigDefault: - type: geometric::SBL - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ESTkConfigDefault: - 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 - LBKPIECEkConfigDefault: - 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 - BKPIECEkConfigDefault: - 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 - KPIECEkConfigDefault: - 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 - RRTkConfigDefault: - 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 - RRTConnectkConfigDefault: - type: geometric::RRTConnect - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - RRTstarkConfigDefault: - 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 - TRRTkConfigDefault: - 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() - PRMkConfigDefault: - type: geometric::PRM - max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 - PRMstarkConfigDefault: - type: geometric::PRMstar - FMTkConfigDefault: - 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 - BFMTkConfigDefault: - 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 - PDSTkConfigDefault: - type: geometric::PDST - STRIDEkConfigDefault: - 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 - BiTRRTkConfigDefault: - 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 - LBTRRTkConfigDefault: - 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 - BiESTkConfigDefault: - type: geometric::BiEST - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ProjESTkConfigDefault: - 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 - LazyPRMkConfigDefault: - type: geometric::LazyPRM - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - LazyPRMstarkConfigDefault: - type: geometric::LazyPRMstar - SPARSkConfigDefault: - 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 - SPARStwokConfigDefault: - 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 - TrajOptDefault: - type: geometric::TrajOpt - -rasms_arm_group: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault - - FMTkConfigDefault - - BFMTkConfigDefault - - PDSTkConfigDefault - - STRIDEkConfigDefault - - BiTRRTkConfigDefault - - LBTRRTkConfigDefault - - BiESTkConfigDefault - - ProjESTkConfigDefault - - LazyPRMkConfigDefault - - LazyPRMstarkConfigDefault - - SPARSkConfigDefault - - SPARStwokConfigDefault - - TrajOptDefault diff --git a/config/rasms.rviz b/config/rasms.rviz deleted file mode 100644 index 05aa213..0000000 --- a/config/rasms.rviz +++ /dev/null @@ -1,431 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - Splitter Ratio: 0.5 - Tree Height: 356 - - 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 - - Class: rviz_visual_tools/RvizVisualToolsGui - Name: RvizVisualToolsGui -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: moveit_rviz_plugin/PlanningScene - Enabled: true - Move Group Namespace: "" - Name: PlanningScene - Planning Scene Topic: /monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 0.8999999761581421 - Scene Color: 50; 230; 50 - Scene Display Time: 0.20000000298023224 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - 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 - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - 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 - base: - 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 - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - 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 - - Acceleration_Scaling_Factor: 0.1 - Class: moveit_rviz_plugin/MotionPlanning - Enabled: true - Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false - MoveIt_Allow_External_Program: false - MoveIt_Allow_Replanning: false - MoveIt_Allow_Sensor_Positioning: false - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Cartesian_Path: false - MoveIt_Use_Constraint_Aware_IK: true - MoveIt_Warehouse_Host: 127.0.0.1 - MoveIt_Warehouse_Port: 33829 - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 - Name: MotionPlanning - Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - 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 - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Loop Animation: false - Robot Alpha: 0.5 - Robot Color: 150; 50; 150 - Show Robot Collision: false - Show Robot Visual: true - Show Trail: false - State Display Time: 0.05 s - Trail Step Size: 1 - Trajectory Topic: /move_group/display_planned_path - Planning Metrics: - Payload: 1 - Show Joint Torques: false - Show Manipulability: false - Show Manipulability Index: false - Show Weight Limit: false - TextHeight: 0.07999999821186066 - Planning Request: - Colliding Link Color: 255; 0; 0 - Goal State Alpha: 1 - Goal State Color: 250; 128; 0 - Interactive Marker Size: 0 - Joint Violation Color: 255; 0; 255 - Planning Group: rasms_arm_group - Query Goal State: false - Query Start State: false - Show Workspace: false - Start State Alpha: 1 - Start State Color: 0; 255; 0 - Planning Scene Topic: /monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 0.8999999761581421 - Scene Color: 50; 230; 50 - Scene Display Time: 0.009999999776482582 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - 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 - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - 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 - Velocity_Scaling_Factor: 0.1 - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MarkerArray - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /move_group_tutorial - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: base - 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: 2.561343193054199 - 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.6653979420661926 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.26539817452430725 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1043 - Hide Left Dock: false - Hide Right Dock: false - MotionPlanning: - collapsed: false - MotionPlanning - Trajectory Slider: - collapsed: false - QMainWindow State: 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 - RvizVisualToolsGui: - collapsed: false - Selection: - collapsed: false - Tool Properties: - collapsed: false - Trajectory - Trajectory Slider: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 2560 - Y: 360 diff --git a/config/rasms_controllers.yml b/config/rasms_controllers.yml deleted file mode 100644 index 7e1950b..0000000 --- a/config/rasms_controllers.yml +++ /dev/null @@ -1,26 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 500 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - - rasms_arm_controller: - type: joint_trajectory_controller/JointTrajectoryController - -rasms_arm_controller: - ros__parameters: - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - command_interfaces: - - position - state_interfaces: - - position - state_publish_rate: 50.0 - action_monitor_rate: 20.0 - \ No newline at end of file diff --git a/config/rasms_moveit_controllers.yml b/config/rasms_moveit_controllers.yml deleted file mode 100644 index 7f360d2..0000000 --- a/config/rasms_moveit_controllers.yml +++ /dev/null @@ -1,15 +0,0 @@ -controller_names: - - rasms_arm_controller - -# http://control.ros.org/ros2_controllers/joint_trajectory_controller/doc/userdoc.html#ros2-interface-of-the-controller -rasms_arm_controller: - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: true - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 \ No newline at end of file diff --git a/config/robasm_sgonov.rviz b/config/robasm_sgonov.rviz deleted file mode 100644 index 3f075e9..0000000 --- a/config/robasm_sgonov.rviz +++ /dev/null @@ -1,182 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /RobotModel1 - Splitter Ratio: 0.5 - Tree Height: 787 - - 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 - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - 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 - base: - 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 - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: base - 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: 1.4318417310714722 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.01542382501065731 - Y: -0.006365143693983555 - Z: 0.15774454176425934 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5703980326652527 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.5603898763656616 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c70000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 diff --git a/rasms_moveit_actions/src/run_moveit.cpp b/rasms_moveit_actions/src/run_moveit.cpp index 1ade3a7..d2ad87d 100644 --- a/rasms_moveit_actions/src/run_moveit.cpp +++ b/rasms_moveit_actions/src/run_moveit.cpp @@ -1,319 +1,100 @@ +#include #include -#include -// MoveitCpp #include #include +#include +#include +#include -#include +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_cpp_demo"); -#include -//#include +class MoveItCppDemo +{ +public: + MoveItCppDemo(const rclcpp::Node::SharedPtr& node) + : node_(node) + , robot_state_publisher_(node_->create_publisher("display_robot_state", 1)) + { + } -namespace rvt = rviz_visual_tools; + 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); -// 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"); + RCLCPP_INFO(LOGGER, "Initialize PlanningComponent"); + moveit_cpp::PlanningComponent arm("panda_arm", moveit_cpp_); + + // A little delay before running the plan + rclcpp::sleep_for(std::chrono::seconds(3)); + + // Create collision object, planning shouldn't be too easy + moveit_msgs::msg::CollisionObject collision_object; + collision_object.header.frame_id = "panda_link0"; + collision_object.id = "box"; + + shape_msgs::msg::SolidPrimitive box; + box.type = box.BOX; + box.dimensions = { 0.1, 0.4, 0.1 }; + + geometry_msgs::msg::Pose box_pose; + box_pose.position.x = 0.4; + box_pose.position.y = 0.0; + box_pose.position.z = 1.0; + + collision_object.primitives.push_back(box); + collision_object.primitive_poses.push_back(box_pose); + collision_object.operation = collision_object.ADD; + + // Add object to planning scene + { // Lock PlanningScene + planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_->getPlanningSceneMonitor()); + scene->processCollisionObjectMsg(collision_object); + } // Unlock PlanningScene + + // Set joint state goal + RCLCPP_INFO(LOGGER, "Set goal"); + arm.setGoal("pose1"); + + // 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_cpp::MoveItCppPtr moveit_cpp_; +}; int main(int argc, char** argv) { + RCLCPP_INFO(LOGGER, "Initialize node"); 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(); + 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(); + }); - // BEGIN_TUTORIAL - // - // Setup - // ^^^^^ - // - static const std::string PLANNING_GROUP = "rasms_arm_group"; - static const std::string LOGNAME = "moveit_interface"; + rclcpp::spin(node); + run_demo.join(); - /* Otherwise robot with zeros joint_states */ - rclcpp::sleep_for(std::chrono::seconds(1)); - - RCLCPP_INFO(LOGGER, "Starting MoveIt Tutorials..."); - - auto moveit_cpp_ptr = std::make_shared(node); - // get error in thes line - moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService(); - RCLCPP_INFO(LOGGER, "Success getPlanningSceneMonitor()"); - - auto planning_components = std::make_shared(PLANNING_GROUP, moveit_cpp_ptr); - auto robot_model_ptr = moveit_cpp_ptr->getRobotModel(); - RCLCPP_INFO(LOGGER, "Success getRobotModel()"); - auto robot_start_state = planning_components->getStartState(); - RCLCPP_INFO(LOGGER, "Success getStartState()"); - auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP); - RCLCPP_INFO(LOGGER, "Success getJointModelGroup()"); - - // Visualization - // ^^^^^^^^^^^^^ - // - // 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(node, "base", "moveit_cpp_tutorial", - moveit_cpp_ptr->getPlanningSceneMonitor()); - visual_tools.deleteAllMarkers(); - visual_tools.loadRemoteControl(); - - Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); - text_pose.translation().z() = 1.75; - visual_tools.publishText(text_pose, "MoveItCpp_Demo", rvt::WHITE, rvt::XLARGE); - visual_tools.trigger(); - - // Start the demo - // ^^^^^^^^^^^^^^^^^^^^^^^^^ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); - - // Planning with MoveItCpp - // ^^^^^^^^^^^^^^^^^^^^^^^ - // There are multiple ways to set the start and the goal states of the plan - // they are illustrated in the following plan examples - // - // Plan #1 - // ^^^^^^^ - // - // We can set the start state of the plan to the current state of the robot - 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::msg::PoseStamped target_pose1; - target_pose1.header.frame_id = "base"; - target_pose1.pose.orientation.w = 1.0; - target_pose1.pose.position.x = 0.28; - target_pose1.pose.position.y = -0.2; - target_pose1.pose.position.z = 0.5; - planning_components->setGoal(target_pose1, "link6"); - - // Now, we call the PlanningComponents to compute the plan and visualize it. - // Note that we are just planning - auto plan_solution1 = planning_components->plan(); - - // Check if PlanningComponents succeeded in finding the plan - if (plan_solution1) - { - // Visualize the start pose in Rviz - visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("base"), "zero_pose"); - // Visualize the goal pose in Rviz - visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose"); - visual_tools.publishText(text_pose, "setStartStateToCurrentState", rvt::WHITE, rvt::XLARGE); - // Visualize the trajectory in Rviz - visual_tools.publishTrajectoryLine(plan_solution1.trajectory, joint_model_group_ptr); - visual_tools.trigger(); - - /* Uncomment if you want to execute the plan */ - /* planning_components->execute(); // Execute the plan */ - } - - // Plan #1 visualization: - // - // .. image:: images/moveitcpp_plan1.png - // :width: 250pt - // :align: center - // - // Start the next plan - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - visual_tools.deleteAllMarkers(); - visual_tools.trigger(); - - // Plan #2 - // ^^^^^^^ - // - // Here we will set the current state of the plan using - // moveit::core::RobotState - auto start_state = *(moveit_cpp_ptr->getCurrentState()); - geometry_msgs::msg::Pose start_pose; - start_pose.orientation.w = 1.0; - start_pose.position.x = 0.55; - start_pose.position.y = 0.0; - start_pose.position.z = 0.6; - - start_state.setFromIK(joint_model_group_ptr, start_pose); - - planning_components->setStartState(start_state); - - // We will reuse the old goal that we had and plan to it. - auto plan_solution2 = planning_components->plan(); - if (plan_solution2) - { - moveit::core::RobotState robot_state(robot_model_ptr); - moveit::core::robotStateMsgToRobotState(plan_solution2.start_state, robot_state); - - visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("link6"), "start_pose"); - visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose"); - visual_tools.publishText(text_pose, "moveit::core::RobotState_Start_State", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(plan_solution2.trajectory, joint_model_group_ptr); - visual_tools.trigger(); - - /* Uncomment if you want to execute the plan */ - /* planning_components->execute(); // Execute the plan */ - } - - // Plan #2 visualization: - // - // .. image:: images/moveitcpp_plan2.png - // :width: 250pt - // :align: center - // - // Start the next plan - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - visual_tools.deleteAllMarkers(); - visual_tools.trigger(); - - // Plan #3 - // ^^^^^^^ - // - // We can also set the goal of the plan using - // moveit::core::RobotState - auto target_state = *robot_start_state; - geometry_msgs::msg::Pose target_pose2; - target_pose2.orientation.w = 1.0; - target_pose2.position.x = 0.55; - target_pose2.position.y = -0.05; - target_pose2.position.z = 0.8; - - target_state.setFromIK(joint_model_group_ptr, target_pose2); - - planning_components->setGoal(target_state); - - // We will reuse the old start that we had and plan from it. - auto plan_solution3 = planning_components->plan(); - if (plan_solution3) - { - moveit::core::RobotState robot_state(robot_model_ptr); - moveit::core::robotStateMsgToRobotState(plan_solution3.start_state, robot_state); - - visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("link6"), "start_pose"); - visual_tools.publishAxisLabeled(target_pose2, "target_pose"); - visual_tools.publishText(text_pose, "moveit::core::RobotState_Goal_Pose", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(plan_solution3.trajectory, joint_model_group_ptr); - visual_tools.trigger(); - - /* Uncomment if you want to execute the plan */ - /* planning_components->execute(); // Execute the plan */ - } - - // Plan #3 visualization: - // - // .. image:: images/moveitcpp_plan3.png - // :width: 250pt - // :align: center - // - // Start the next plan - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - visual_tools.deleteAllMarkers(); - visual_tools.trigger(); - - // Plan #4 - // ^^^^^^^ - // - // We can set the start state of the plan to the current state of the robot - // We can set the goal of the plan using the name of a group states - // for panda robot we have one named robot state for "panda_arm" planning group called "ready" - // see `panda_arm.xacro - // `_ - - /* // Set the start state of the plan from a named robot state */ - /* planning_components->setStartState("ready"); // Not implemented yet */ - // Set the goal state of the plan from a named robot state - planning_components->setGoal("ready"); - - // Again we will reuse the old start that we had and plan from it. - auto plan_solution4 = planning_components->plan(); - if (plan_solution4) - { - moveit::core::RobotState robot_state(robot_model_ptr); - moveit::core::robotStateMsgToRobotState(plan_solution4.start_state, robot_state); - - visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("link6"), "start_pose"); - visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("link6"), "target_pose"); - visual_tools.publishText(text_pose, "Goal_Pose_From_Named_State", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(plan_solution4.trajectory, joint_model_group_ptr); - visual_tools.trigger(); - - /* Uncomment if you want to execute the plan */ - /* planning_components->execute(); // Execute the plan */ - } - - // Plan #4 visualization: - // - // .. image:: images/moveitcpp_plan4.png - // :width: 250pt - // :align: center - // - // Start the next plan - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - visual_tools.deleteAllMarkers(); - visual_tools.trigger(); - - // Plan #5 - // ^^^^^^^ - // - // We can also generate motion plans around objects in the collision scene. - // - // First we create the collision object - moveit_msgs::msg::CollisionObject collision_object; - collision_object.header.frame_id = "base"; - collision_object.id = "box"; - - shape_msgs::msg::SolidPrimitive box; - box.type = box.BOX; - box.dimensions = { 0.1, 0.4, 0.1 }; - - geometry_msgs::msg::Pose box_pose; - box_pose.position.x = 0.4; - box_pose.position.y = 0.0; - box_pose.position.z = 1.0; - - collision_object.primitives.push_back(box); - collision_object.primitive_poses.push_back(box_pose); - collision_object.operation = collision_object.ADD; - - // Add object to planning scene - { // Lock PlanningScene - planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_ptr->getPlanningSceneMonitor()); - scene->processCollisionObjectMsg(collision_object); - } // Unlock PlanningScene - planning_components->setStartStateToCurrentState(); - planning_components->setGoal("extended"); - - auto plan_solution5 = planning_components->plan(); - if (plan_solution5) - { - visual_tools.publishText(text_pose, "Planning_Around_Collision_Object", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(plan_solution5.trajectory, joint_model_group_ptr); - visual_tools.trigger(); - - /* Uncomment if you want to execute the plan */ - /* planning_components->execute(); // Execute the plan */ - } - - // Plan #5 visualization: - // - // .. image:: images/moveitcpp_plan5.png - // :width: 250pt - // :align: center - // - // END_TUTORIAL - visual_tools.prompt("Press 'next' to end the demo"); - visual_tools.deleteAllMarkers(); - visual_tools.trigger(); - - RCLCPP_INFO(LOGGER, "Shutting down."); - rclcpp::shutdown(); return 0; -} +} \ No newline at end of file diff --git a/rasms_moveit_config/config/.vscode/c_cpp_properties.json b/rasms_moveit_config/config/.vscode/c_cpp_properties.json deleted file mode 100644 index cc44bf2..0000000 --- a/rasms_moveit_config/config/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - "configurations": [ - { - "browse": { - "databaseFilename": "", - "limitSymbolsToIncludedHeaders": true - }, - "includePath": [ - "/home/splinter1984/robas_ws/install/moveit_visual_tools/include/**", - "/home/splinter1984/robas_ws/install/rviz_visual_tools/include/**", - "/opt/ros/foxy/include/**", - "/home/splinter1984/robas_ws/src/moveit_visual_tools/include/**", - "/home/splinter1984/robas_ws/src/robossembler-ros2/rasms_manipulator/include/**", - "/home/splinter1984/robas_ws/src/rviz_visual_tools/include/**", - "/usr/include/**" - ], - "name": "ROS" - } - ], - "version": 4 -} \ No newline at end of file diff --git a/rasms_moveit_config/config/.vscode/settings.json b/rasms_moveit_config/config/.vscode/settings.json deleted file mode 100644 index 6b40311..0000000 --- a/rasms_moveit_config/config/.vscode/settings.json +++ /dev/null @@ -1,5 +0,0 @@ -{ - "python.autoComplete.extraPaths": [ - "/opt/ros/foxy/lib/python3.8/site-packages" - ] -} \ No newline at end of file diff --git a/rasms_moveit_config/launch/rasms_control.launch.py b/rasms_moveit_config/launch/rasms_control.launch.py index 3003009..bb19678 100644 --- a/rasms_moveit_config/launch/rasms_control.launch.py +++ b/rasms_moveit_config/launch/rasms_control.launch.py @@ -23,14 +23,6 @@ def generate_launch_description(): ) ) - launch_args.append( - DeclareLaunchArgument( - name="controller", - default_value="rasms_arm_controller", - description="Robot controller." - ) - ) - # Configure robot_description robot_description = {"robot_description": LaunchConfiguration("robot_description")} @@ -86,7 +78,7 @@ def generate_launch_description(): arguments=["rasms_arm_controller", "--controller-manager", "/controller_manager"], ) - load_controllers = [] + """load_controllers = [] for controller in [ "rasms_arm_controller", "joint_state_broadcaster", @@ -97,17 +89,17 @@ def generate_launch_description(): shell=True, output="screen", ) - ] + ]""" return LaunchDescription( launch_args + - load_controllers + + #load_controllers + [ controller_manager, robot_state_publisher, - static_tf, + #static_tf, joint_state_broadcaster, rasms_arm_controller, - joint_state_publisher + #joint_state_publisher ] ) diff --git a/rasms_moveit_config/launch/rasms_moveit_interface.launch.py b/rasms_moveit_config/launch/rasms_moveit_interface.launch.py index b1ce3ea..ff5b615 100644 --- a/rasms_moveit_config/launch/rasms_moveit_interface.launch.py +++ b/rasms_moveit_config/launch/rasms_moveit_interface.launch.py @@ -34,10 +34,6 @@ def load_file(package_name: str, file_path: str) -> str: def generate_launch_description(): - #============= - #================================================================= Start initialize launch args ========================================================== - #============= - # Launch arguments launch_args = [] @@ -54,10 +50,6 @@ def generate_launch_description(): description="Launch robot in simulation or on real setup." )) - #============= - #================================================================= Start launching nodes ========================================================== - #============= - # Configure robot_description robot_description = {"robot_description": LaunchConfiguration("robot_description")} @@ -68,10 +60,6 @@ def generate_launch_description(): # Kinematics kinematics_yaml = load_yaml("rasms_moveit_config", "config/kinematics.yml") - - # Update group name - #kinematics_yaml["rasms_arm_group"] = kinematics_yaml["group_name"] - #del kinematics_yaml["group_name"] # Joint limits robot_description_planning = { @@ -83,9 +71,6 @@ def generate_launch_description(): ) } - # Planning - #ompl_yaml = load_yaml("rasms_moveit_config", "config/ompl_planning.yml") - planning = { "move_group": { "planning_plugin": "ompl_interface/OMPLPlanner", @@ -107,6 +92,7 @@ def generate_launch_description(): moveit_controllers = {"moveit_simple_controller_manager": moveit_simple_controller_yml, "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager"} + # Planning ompl_planning_pipeline_config = { "ompl": { "planning_plugin": "ompl_interface/OMPLPlanner", @@ -185,8 +171,8 @@ def generate_launch_description(): launch_nodes = [] launch_nodes.append(rviz) - launch_nodes.append(move_group_node) - #launch_nodes.append(moveit_cpp_node) + #launch_nodes.append(move_group_node) + launch_nodes.append(moveit_cpp_node) return LaunchDescription(