From 6c5fb63602bd9d5a59c134047cc6203d12249378 Mon Sep 17 00:00:00 2001
From: Marcus Grieger <marcus@grieger.xyz>
Date: Sun, 4 Feb 2024 19:11:07 +0100
Subject: [PATCH] Updated non-gui launch file

---
 launch/hexapod_robot.launch.py | 74 +++++++++++++---------------------
 1 file changed, 27 insertions(+), 47 deletions(-)

diff --git a/launch/hexapod_robot.launch.py b/launch/hexapod_robot.launch.py
index e416425..7ca0180 100644
--- a/launch/hexapod_robot.launch.py
+++ b/launch/hexapod_robot.launch.py
@@ -3,7 +3,6 @@ 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
 
@@ -12,77 +11,59 @@ from moveit_configs_utils import MoveItConfigsBuilder
 
 def generate_launch_description():
 
-    robot_description_content = Command([
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
-            PathJoinSubstitution(
-                [
-                    FindPackageShare("hexapod_robot"),
-                    "config",
-                    "urdf",
-                    "hexapod_robot.urdf.xacro"
-                ]
-            )
-        ]
+    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()]
     )
-    robot_description = {"robot_description": robot_description_content}
 
     ros2_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "ros2_controllers.yaml"])
 
     robot_state_publisher_node = Node(
         package = "robot_state_publisher",
         executable = "robot_state_publisher",
-        parameters = [robot_description],
+        parameters = [moveit_config.robot_description],
         output = "both"
     )            
 
     ros2_control_node = Node(
         package = "controller_manager",
         executable = "ros2_control_node",
-        parameters = [robot_description, ros2_controllers],
+        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",
-    # )
-
     joint_state_broadcaster_spawner = Node (
         package = "controller_manager",
         executable = "spawner",
         arguments = ["joint_state_broadcaster", "--controller-manager", "/controller_manager"]
     )
-    
-    # forward_position_controller_spawner = Node(
-    #     package = "controller_manager",
-    #     executable = "spawner",
-    #     arguments = ["forward_position_controller", "--controller-manager", "/controller_manager"]
-    # )
 
     joint_trajectory_controller_spawner = Node(
         package = "controller_manager",
         executable = "spawner",
-        arguments = ["e1_group_controller", "--controller-manager", "/controller_manager"]
+        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_forward_position_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
-    #     event_handler = OnProcessExit(
-    #         target_action = joint_state_broadcaster_spawner,
-    #         on_exit = [forward_position_controller_spawner]
-    #     )
-    # )
+    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(
@@ -93,11 +74,10 @@ def generate_launch_description():
 
 
     nodes = [
-        ros2_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
     ]