From af64efc0a1fe1ab8a46a4411b7ffd4b0f4bcde30 Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Mon, 10 Mar 2025 14:52:35 +0300
Subject: [PATCH] update launch files

---
 rbs_mill_assist/launch/moveit.launch.py     | 30 ++++++++++++++-------
 rbs_mill_assist/launch/simulation.launch.py | 11 ++++++++
 2 files changed, 32 insertions(+), 9 deletions(-)

diff --git a/rbs_mill_assist/launch/moveit.launch.py b/rbs_mill_assist/launch/moveit.launch.py
index 720d77e..93da59f 100644
--- a/rbs_mill_assist/launch/moveit.launch.py
+++ b/rbs_mill_assist/launch/moveit.launch.py
@@ -1,6 +1,7 @@
 from launch_ros.actions import Node
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from ament_index_python.packages import get_package_share_directory
 from launch_ros.substitutions import FindPackage, FindPackageShare
 from launch.substitutions import (
     Command,
@@ -8,6 +9,7 @@ from launch.substitutions import (
     LaunchConfiguration,
     PathJoinSubstitution,
 )
+import yaml
 from rbs_launch_utils.launch_common import (
     get_package_share_directory,
     load_yaml,
@@ -90,9 +92,9 @@ def launch_setup(context, *args, **kwargs):
     robot_description_semantic_content = LaunchConfiguration(
         "robot_description_semantic"
     ).perform(context)
-    robot_description_kinematics_filepath = LaunchConfiguration(
-        "robot_description_kinematics"
-    ).perform(context)
+    # robot_description_kinematics_filepath = LaunchConfiguration(
+    #     "robot_description_kinematics"
+    # ).perform(context)
     namespace = LaunchConfiguration("namespace").perform(context)
     moveit_config_package = LaunchConfiguration("moveit_config_package").perform(
         context
@@ -102,11 +104,11 @@ def launch_setup(context, *args, **kwargs):
     robot_description_semantic = {
         "robot_description_semantic": robot_description_semantic_content
     }
-    robot_description_kinematics = {
-        "robot_description_kinematics": load_yaml_abs(
-            robot_description_kinematics_filepath
-        )
-    }
+    # robot_description_kinematics = {
+    #     "robot_description_kinematics": load_yaml_abs(
+    #         robot_description_kinematics_filepath
+    #     )
+    # }
     use_sim_time = {"use_sim_time": use_sim_time}
 
     # Planning Configuration
@@ -133,6 +135,7 @@ def launch_setup(context, *args, **kwargs):
     #     "moveit_simple_controller_manager/MoveItSimpleControllerManager",
     # }
 
+    xacro_mappings = yaml.safe_load(os.path.join(get_package_share_directory("rbs_mill_assist"), "urdf", "xacro_args.yaml"))
     moveit_config = (
         MoveItConfigsBuilder(robot_name="rbs_arm", package_name="rbs_mill_assist")
         .robot_description(file_path="urdf/rbs_arm.xacro")
@@ -142,6 +145,7 @@ def launch_setup(context, *args, **kwargs):
         .robot_description_kinematics("moveit/kinematics.yaml")
         .robot_description_semantic("srdf/rbs_arm.srdf")
         .moveit_cpp("moveit/moveit_cpp.yaml")
+        .planning_pipelines(pipelines=["ompl", "chomp"], default_planning_pipeline="chomp")
         .planning_scene_monitor(
             publish_geometry_updates=True,
             publish_planning_scene=True,
@@ -180,7 +184,7 @@ def launch_setup(context, *args, **kwargs):
         parameters=[
             robot_description,
             robot_description_semantic,
-            robot_description_kinematics,
+            # robot_description_kinematics,
             ompl_planning_pipeline_config,
             # trajectory_execution,
             moveit_controllers,
@@ -188,6 +192,14 @@ def launch_setup(context, *args, **kwargs):
             use_sim_time,
             robot_description_planning,
             # moveit_config.to_dict()
+
+            # moveit_config.robot_description,
+            # moveit_config.robot_description_semantic,
+            # moveit_config.planning_pipelines,
+            # moveit_config.planning_scene_monitor,
+            moveit_config.robot_description_kinematics,
+            # moveit_config.joint_limits,
+            # moveit_config.trajectory_execution,
         ],
     )
     planning_scene_publisher = Node(
diff --git a/rbs_mill_assist/launch/simulation.launch.py b/rbs_mill_assist/launch/simulation.launch.py
index bcfc6c2..603a350 100644
--- a/rbs_mill_assist/launch/simulation.launch.py
+++ b/rbs_mill_assist/launch/simulation.launch.py
@@ -186,6 +186,16 @@ def launch_setup(context, *args, **kwargs):
         ]
     )
 
+    publish_ee_pose = Node(
+        package="rbs_mill_assist",
+        executable="publish_ee_pose_node",
+        parameters=[
+            {"use_sim_time": use_sim_time},
+            {"base_link": base_link_name},
+            {"ee_link": ee_link_name}
+        ]
+    )
+
     nodes_to_start = [
         rbs_robot_setup,
         # rviz_node,
@@ -194,6 +204,7 @@ def launch_setup(context, *args, **kwargs):
         gz_bridge,
         grasping_service,
         get_named_pose_service,
+        # publish_ee_pose
         # get_workspace
     ]
     return nodes_to_start