diff --git a/launch/joint_trajectory_control.launch.py b/launch/joint_trajectory_control.launch.py index 7bd9990..6dd9b9c 100644 --- a/launch/joint_trajectory_control.launch.py +++ b/launch/joint_trajectory_control.launch.py @@ -3,56 +3,43 @@ 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() + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("hexapod_robot"), + "config", + "urdf", + "hexapod_robot.urdf.xacro" + ] + ) + ] ) + robot_description = {"robot_description": robot_description_content} - 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_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], + parameters = [robot_description], output = "both" ) - ros2_control_node = Node( + control_node = Node( package = "controller_manager", executable = "ros2_control_node", - parameters = [moveit_config.robot_description, ros2_controllers], + parameters = [robot_description, robot_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", @@ -66,14 +53,6 @@ def generate_launch_description(): ) - 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, @@ -81,14 +60,10 @@ def generate_launch_description(): ) ) - nodes = [ + control_node, 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 ]