From bec3583343f2ca56bb7fbd87eed8a77f8c215255 Mon Sep 17 00:00:00 2001 From: Marcus Grieger Date: Mon, 5 Feb 2024 21:21:01 +0100 Subject: [PATCH] moved moveit related files to hexapod_moveit package --- .setup_assistant | 10 - config/hexapod_robot.rviz | 669 ------------------------------ config/hexapod_robot.srdf | 21 - config/joint_limits.yaml | 7 - config/kinematics.yaml | 77 ---- config/moveit_controllers.yaml | 81 ---- config/pilz_cartesian_limits.yaml | 6 - launch/move_group.launch.py | 85 ---- launch/move_group_gui.launch.py | 96 ----- launch/rviz.launch.py | 37 -- 10 files changed, 1089 deletions(-) delete mode 100644 .setup_assistant delete mode 100644 config/hexapod_robot.rviz delete mode 100644 config/hexapod_robot.srdf delete mode 100644 config/joint_limits.yaml delete mode 100644 config/kinematics.yaml delete mode 100644 config/moveit_controllers.yaml delete mode 100644 config/pilz_cartesian_limits.yaml delete mode 100644 launch/move_group.launch.py delete mode 100644 launch/move_group_gui.launch.py delete mode 100644 launch/rviz.launch.py diff --git a/.setup_assistant b/.setup_assistant deleted file mode 100644 index b450ed8..0000000 --- a/.setup_assistant +++ /dev/null @@ -1,10 +0,0 @@ -moveit_setup_assistant_config: - urdf: - package: hexapod_robot - relative_path: config/urdf/hexapod_robot.urdf.xacro - srdf: - relative_path: config/hexapod_robot.srdf - package_settings: - author_name: Marcus Grieger - author_email: marcus@grieger.xyz - generated_timestamp: 1707046355 \ No newline at end of file diff --git a/config/hexapod_robot.rviz b/config/hexapod_robot.rviz deleted file mode 100644 index 1ec916d..0000000 --- a/config/hexapod_robot.rviz +++ /dev/null @@ -1,669 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 70 - Name: Displays - Property Tree Widget: - Expanded: - - /MotionPlanning1 - Splitter Ratio: 0.5 - Tree Height: 378 - - Class: rviz_common/Help - Name: Help - - 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 - - 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: false - 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_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c4_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c5_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c6_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e4_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e5_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e6_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f4_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f5_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f6_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t4_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t5_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t6_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Loop Animation: true - 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: display_planned_path - Use Sim Time: false - 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.05000000074505806 - Joint Violation Color: 255; 0; 255 - Planning Group: e1_group - Query Goal State: true - 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: 1 - 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_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c4_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c5_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c6_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e4_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e5_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e6_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f4_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f5_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f6_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t4_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t5_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t6_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Robot Alpha: 0.5 - Show Robot Collision: false - Show Robot Visual: true - Value: true - Velocity_Scaling_Factor: 0.1 - - 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_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c4_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c5_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - c6_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e4_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e5_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - e6_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f4_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f5_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - f6_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t4_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t5_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - t6_link: - 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: 3x - Trail Step Size: 1 - Trajectory Topic: /display_planned_path - Use Sim Time: false - Value: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - base_link: - Value: true - c1_link: - Value: true - c2_link: - Value: true - c3_link: - Value: true - c4_link: - Value: true - c5_link: - Value: true - c6_link: - Value: true - e1_link: - Value: true - e2_link: - Value: true - e3_link: - Value: true - e4_link: - Value: true - e5_link: - Value: true - e6_link: - Value: true - f1_link: - Value: true - f2_link: - Value: true - f3_link: - Value: true - f4_link: - Value: true - f5_link: - Value: true - f6_link: - Value: true - t1_link: - Value: true - t2_link: - Value: true - t3_link: - Value: true - t4_link: - Value: true - t5_link: - Value: true - t6_link: - Value: true - Marker Scale: 0.10000000149011612 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - base_link: - c1_link: - f1_link: - t1_link: - e1_link: - {} - c2_link: - f2_link: - t2_link: - e2_link: - {} - c3_link: - f3_link: - t3_link: - e3_link: - {} - c4_link: - f4_link: - t4_link: - e4_link: - {} - c5_link: - f5_link: - t5_link: - e5_link: - {} - c6_link: - f6_link: - t6_link: - e6_link: - {} - Update Interval: 0 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: base_link - 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 - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 0.6329568028450012 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.07758110761642456 - Y: 0.08254023641347885 - Z: 0.09184139221906662 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5400001406669617 - Target Frame: base_link - Value: Orbit (rviz_default_plugins) - Yaw: 2.3151493072509766 - Saved: ~ -Window Geometry: - " - Trajectory Slider": - collapsed: false - Displays: - collapsed: false - Height: 1287 - Help: - collapsed: false - Hide Left Dock: false - Hide Right Dock: false - MotionPlanning: - collapsed: false - MotionPlanning - Trajectory Slider: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002b4000004a9fc0200000006fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004400fffffffb000000100044006900730070006c006100790073010000003f00000200000000cc00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000245000001a10000019600fffffffb0000000800480065006c0070000000029a0000006e0000006f00fffffffb0000000a0056006900650077007301000003ec000000fc000000a900fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004400ffffff0000063e000004a900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Views: - collapsed: false - Width: 2296 - X: 0 - Y: 32 diff --git a/config/hexapod_robot.srdf b/config/hexapod_robot.srdf deleted file mode 100644 index df01027..0000000 --- a/config/hexapod_robot.srdf +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/config/joint_limits.yaml b/config/joint_limits.yaml deleted file mode 100644 index 33387dc..0000000 --- a/config/joint_limits.yaml +++ /dev/null @@ -1,7 +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 - diff --git a/config/kinematics.yaml b/config/kinematics.yaml deleted file mode 100644 index aa4f5c6..0000000 --- a/config/kinematics.yaml +++ /dev/null @@ -1,77 +0,0 @@ -e1_group: - kinematics_solver: pick_ik/PickIkPlugin - kinematics_solver_timeout: 0.05 - kinematics_solver_attempts: 3 - mode: global - position_scale: 1.0 - rotation_scale: 0.0 - position_threshold: 0.001 - orientation_threshold: 0.01 - cost_threshold: 0.001 - minimal_displacement_weight: 0.0 - gd_step_size: 0.0001 - -e2_group: - kinematics_solver: pick_ik/PickIkPlugin - kinematics_solver_timeout: 0.05 - kinematics_solver_attempts: 3 - mode: global - position_scale: 1.0 - rotation_scale: 0.0 - position_threshold: 0.001 - orientation_threshold: 0.01 - cost_threshold: 0.001 - minimal_displacement_weight: 0.0 - gd_step_size: 0.0001 - -e3_group: - kinematics_solver: pick_ik/PickIkPlugin - kinematics_solver_timeout: 0.05 - kinematics_solver_attempts: 3 - mode: global - position_scale: 1.0 - rotation_scale: 0.0 - position_threshold: 0.001 - orientation_threshold: 0.01 - cost_threshold: 0.001 - minimal_displacement_weight: 0.0 - gd_step_size: 0.0001 - -e4_group: - kinematics_solver: pick_ik/PickIkPlugin - kinematics_solver_timeout: 0.05 - kinematics_solver_attempts: 3 - mode: global - position_scale: 1.0 - rotation_scale: 0.0 - position_threshold: 0.001 - orientation_threshold: 0.01 - cost_threshold: 0.001 - minimal_displacement_weight: 0.0 - gd_step_size: 0.0001 - -e5_group: - kinematics_solver: pick_ik/PickIkPlugin - kinematics_solver_timeout: 0.05 - kinematics_solver_attempts: 3 - mode: global - position_scale: 1.0 - rotation_scale: 0.0 - position_threshold: 0.001 - orientation_threshold: 0.01 - cost_threshold: 0.001 - minimal_displacement_weight: 0.0 - gd_step_size: 0.0001 - -e6_group: - kinematics_solver: pick_ik/PickIkPlugin - kinematics_solver_timeout: 0.05 - kinematics_solver_attempts: 3 - mode: global - position_scale: 1.0 - rotation_scale: 0.0 - position_threshold: 0.001 - orientation_threshold: 0.01 - cost_threshold: 0.001 - minimal_displacement_weight: 0.0 - gd_step_size: 0.0001 diff --git a/config/moveit_controllers.yaml b/config/moveit_controllers.yaml deleted file mode 100644 index 1f4dfde..0000000 --- a/config/moveit_controllers.yaml +++ /dev/null @@ -1,81 +0,0 @@ -# MoveIt uses this configuration for controller management - -moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager - -moveit_simple_controller_manager: - controller_names: - - e1_group_controller - - e2_group_controller - - e3_group_controller - - e4_group_controller - - e5_group_controller - - e6_group_controller - - e1_group_controller: - type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true - joints: - - c1_joint - - f1_joint - - t1_joint - action_ns: follow_joint_trajectory - default: true - - e2_group_controller: - type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true - joints: - - c2_joint - - f2_joint - - t2_joint - action_ns: follow_joint_trajectory - default: true - - e3_group_controller: - type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true - joints: - - c3_joint - - f3_joint - - t3_joint - action_ns: follow_joint_trajectory - default: true - - e4_group_controller: - type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true - joints: - - c4_joint - - f4_joint - - t4_joint - action_ns: follow_joint_trajectory - default: true - - e5_group_controller: - type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true - joints: - - c5_joint - - f5_joint - - t5_joint - action_ns: follow_joint_trajectory - default: true - - e6_group_controller: - type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true - joints: - - c6_joint - - f6_joint - - t6_joint - action_ns: follow_joint_trajectory - default: true - - - \ No newline at end of file diff --git a/config/pilz_cartesian_limits.yaml b/config/pilz_cartesian_limits.yaml deleted file mode 100644 index b2997ca..0000000 --- a/config/pilz_cartesian_limits.yaml +++ /dev/null @@ -1,6 +0,0 @@ -# Limits for the Pilz planner -cartesian_limits: - max_trans_vel: 1.0 - max_trans_acc: 2.25 - max_trans_dec: -5.0 - max_rot_vel: 1.57 diff --git a/launch/move_group.launch.py b/launch/move_group.launch.py deleted file mode 100644 index 7ca0180..0000000 --- a/launch/move_group.launch.py +++ /dev/null @@ -1,85 +0,0 @@ -from launch import LaunchDescription -from launch.actions import RegisterEventHandler -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution -from launch.event_handlers import OnProcessExit - -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -from moveit_configs_utils import MoveItConfigsBuilder - - -def generate_launch_description(): - - moveit_config = ( - MoveItConfigsBuilder("hexapod_robot", package_name = "hexapod_robot") - - .planning_scene_monitor( - publish_robot_description=True, publish_robot_description_semantic=True - ) - .planning_pipelines(pipelines = ["ompl"]) - .to_moveit_configs() - ) - - move_group_node = Node( - package = "moveit_ros_move_group", - executable = "move_group", - output = "screen", - parameters = [moveit_config.to_dict()] - ) - - ros2_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "ros2_controllers.yaml"]) - - robot_state_publisher_node = Node( - package = "robot_state_publisher", - executable = "robot_state_publisher", - parameters = [moveit_config.robot_description], - output = "both" - ) - - ros2_control_node = Node( - package = "controller_manager", - executable = "ros2_control_node", - parameters = [moveit_config.robot_description, ros2_controllers], - output="both" - ) - - joint_state_broadcaster_spawner = Node ( - package = "controller_manager", - executable = "spawner", - arguments = ["joint_state_broadcaster", "--controller-manager", "/controller_manager"] - ) - - joint_trajectory_controller_spawner = Node( - package = "controller_manager", - executable = "spawner", - arguments = ["e1_group_controller", "e2_group_controller", "e3_group_controller", "e4_group_controller", "e5_group_controller", "e6_group_controller", "--controller-manager", "/controller_manager"] - ) - - - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler = OnProcessExit( - target_action = joint_state_broadcaster_spawner, - on_exit = [rviz_node] - ) - ) - - - delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler = OnProcessExit( - target_action = joint_state_broadcaster_spawner, - on_exit = [joint_trajectory_controller_spawner] - ) - ) - - - nodes = [ - robot_state_publisher_node, - move_group_node, - ros2_control_node, - joint_state_broadcaster_spawner, - delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner - ] - - return LaunchDescription(nodes) - \ No newline at end of file diff --git a/launch/move_group_gui.launch.py b/launch/move_group_gui.launch.py deleted file mode 100644 index 7bd9990..0000000 --- a/launch/move_group_gui.launch.py +++ /dev/null @@ -1,96 +0,0 @@ -from launch import LaunchDescription -from launch.actions import RegisterEventHandler -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution -from launch.event_handlers import OnProcessExit - -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -from moveit_configs_utils import MoveItConfigsBuilder - - -def generate_launch_description(): - - moveit_config = ( - MoveItConfigsBuilder("hexapod_robot", package_name = "hexapod_robot") - - .planning_scene_monitor( - publish_robot_description=True, publish_robot_description_semantic=True - ) - .planning_pipelines(pipelines = ["ompl"]) - .to_moveit_configs() - ) - - move_group_node = Node( - package = "moveit_ros_move_group", - executable = "move_group", - output = "screen", - parameters = [moveit_config.to_dict()] - ) - - ros2_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "ros2_controllers.yaml"]) - - robot_state_publisher_node = Node( - package = "robot_state_publisher", - executable = "robot_state_publisher", - parameters = [moveit_config.robot_description], - output = "both" - ) - - ros2_control_node = Node( - package = "controller_manager", - executable = "ros2_control_node", - parameters = [moveit_config.robot_description, ros2_controllers], - output="both" - ) - - rviz_node = Node( - package = "rviz2", - executable = "rviz2", - arguments = ['-d', PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "hexapod_robot.rviz"])], - name = "rviz2", - output = "screen", - parameters = [moveit_config.planning_pipelines, moveit_config.robot_description_kinematics] - ) - - joint_state_broadcaster_spawner = Node ( - package = "controller_manager", - executable = "spawner", - arguments = ["joint_state_broadcaster", "--controller-manager", "/controller_manager"] - ) - - joint_trajectory_controller_spawner = Node( - package = "controller_manager", - executable = "spawner", - arguments = ["e1_group_controller", "e2_group_controller", "e3_group_controller", "e4_group_controller", "e5_group_controller", "e6_group_controller", "--controller-manager", "/controller_manager"] - ) - - - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler = OnProcessExit( - target_action = joint_state_broadcaster_spawner, - on_exit = [rviz_node] - ) - ) - - - delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler = OnProcessExit( - target_action = joint_state_broadcaster_spawner, - on_exit = [joint_trajectory_controller_spawner] - ) - ) - - - nodes = [ - robot_state_publisher_node, - move_group_node, - ros2_control_node, - joint_state_broadcaster_spawner, - delay_rviz_after_joint_state_broadcaster_spawner, - # delay_forward_position_controller_spawner_after_joint_state_broadcaster_spawner, - delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner - ] - - return LaunchDescription(nodes) - \ No newline at end of file diff --git a/launch/rviz.launch.py b/launch/rviz.launch.py deleted file mode 100644 index e629c08..0000000 --- a/launch/rviz.launch.py +++ /dev/null @@ -1,37 +0,0 @@ -from launch import LaunchDescription -from launch.actions import RegisterEventHandler -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution -from launch.event_handlers import OnProcessExit - -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -from moveit_configs_utils import MoveItConfigsBuilder - - -def generate_launch_description(): - - moveit_config = ( - MoveItConfigsBuilder("hexapod_robot", package_name = "hexapod_robot") - - .planning_scene_monitor( - publish_robot_description=True, publish_robot_description_semantic=True - ) - .planning_pipelines(pipelines = ["ompl"]) - .to_moveit_configs() - ) - - rviz_node = Node( - package = "rviz2", - executable = "rviz2", - arguments = ['-d', PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "hexapod_robot.rviz"])], - name = "rviz2", - output = "screen", - parameters = [moveit_config.planning_pipelines, moveit_config.robot_description_kinematics] - ) - nodes = [ - rviz_node - ] - - return LaunchDescription(nodes) - \ No newline at end of file