enhance skill server parameter handling and assembly service

- Unified `base_link` and `ee_link` parameter usage across launch files and skill server components.
- Improved synchronization in `mtjs_jtc` and `mtp_jtc` with a timeout mechanism for joint position updates, adding error handling when data is unavailable.
- Extended `AssemblyConfigService` to broadcast TF transforms for relative parts during initialization.
- Enhanced YAML parsing for `AssemblyConfigService` to handle missing orientations with default values.
- Updated `mtp_jtc_cart` to align parameter names with other skill server components.
- Added single-threaded executor to `AssemblyConfigService` for better lifecycle management.

rbs_skill_servers/launch/skills.launch.py

@@ -55,6 +55,8 @@ def launch_setup(context, *args, **kwargs):
                 parameters=[
                     {"use_sim_time": use_sim_time},
                     {"robot_name": namespace},
+                    {"base_link": base_link_name},
+                    {"ee_link": ee_link_name},
                     robot_description,
                 ],
             ),
@@ -66,7 +68,7 @@ def launch_setup(context, *args, **kwargs):
                     {"use_sim_time": use_sim_time},
                     {"robot_name": namespace},
                     {"base_link": base_link_name},
-                    {"robot_ee_link": ee_link_name},
+                    {"ee_link": ee_link_name},
                     robot_description,
                 ],
             ),
@@ -101,6 +103,7 @@ def launch_setup(context, *args, **kwargs):
         package="rbs_utils",
         executable="assembly_config_service.py",
         namespace=namespace,
+        parameters=[{"use_sim_time": use_sim_time}],
         output="screen",
     )
 

rbs_skill_servers/src/mtjs_jtc.cpp

@@ -60,8 +60,16 @@ protected:
 
     trajectory_goal.trajectory.joint_names = m_joint_names;
 
-    // TODO: Better sync solution
-    while (m_current_joint_positions.empty()) {
+    const int max_wait_iterations = 100;
+    int wait_count = 0;
+    while (m_current_joint_positions.empty() &&
+           wait_count++ < max_wait_iterations) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+    if (m_current_joint_positions.empty()) {
+      RCLCPP_ERROR(this->get_logger(),
+                   "Joint positions were not received in time");
+      return;
     }
 
     trajectory_goal.trajectory.points = generate_trajectory(

rbs_skill_servers/src/mtp_jtc.cpp

@@ -57,6 +57,12 @@ public:
                       std::placeholders::_1),
             s_options);
     this->declare_parameter("robot_description", "");
+    this->declare_parameter("base_link", "");
+    this->declare_parameter("ee_link", "");
+
+    m_robot_description = this->get_parameter("robot_description").as_string();
+    m_base_link = this->get_parameter("base_link").as_string();
+    m_ee_link = this->get_parameter("ee_link").as_string();
   }
 
 protected:
@@ -75,8 +81,16 @@ protected:
 
     trajectory_goal.trajectory.joint_names = m_joint_names;
 
-    // TODO: Better sync solution
-    while (m_current_joint_positions.empty()) {
+    const int max_wait_iterations = 100;
+    int wait_count = 0;
+    while (m_current_joint_positions.empty() &&
+           wait_count++ < max_wait_iterations) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+    if (m_current_joint_positions.empty()) {
+      RCLCPP_ERROR(this->get_logger(),
+                   "Joint positions were not received in time");
+      return;
     }
 
     std::vector<double> target_joint_values;
@@ -86,6 +100,7 @@ protected:
         generate_trajectory(m_current_joint_positions, target_joint_values,
                             m_current_goal->duration);
 
+    // RCLCPP_INFO(this->get_logger(), "Starting executing action goal");
     auto send_goal_options =
         rclcpp_action::Client<FollowJointTrajectory>::SendGoalOptions();
     send_goal_options.result_callback =
@@ -109,7 +124,7 @@ private:
     if (m_joint_names.empty()) {
       return;
     }
-    RCLCPP_WARN(this->get_logger(), "Called joint positions");
+    // RCLCPP_WARN(this->get_logger(), "Called joint positions");
 
     if (m_joint_mame_to_index.empty()) {
       m_joint_mame_to_index.reserve(m_joint_names.size());
@@ -140,10 +155,12 @@ private:
                       const std::vector<double> &target_joint_values,
                       const double duration) {
 
+    RCLCPP_INFO(this->get_logger(), "Starting generate_trajectory");
     const int num_points = 100;
     std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
     for (int i = 0; i <= num_points; ++i) {
       trajectory_msgs::msg::JointTrajectoryPoint point;
+
       double t = static_cast<double>(i) / num_points;
       for (size_t j = 0; j < target_joint_values.size(); ++j) {
         double position = start_joint_values[j] +
@@ -157,6 +174,8 @@ private:
   }
 
   void solveIK(std::vector<double> &out) {
+
+    RCLCPP_INFO(this->get_logger(), "Starting solveIK");
     KDL::JntArray q_in(m_joint_names.size());
     for (size_t i = 0; i < m_joint_names.size(); ++i) {
       q_in(i) = m_current_joint_positions[i];
@@ -175,14 +194,8 @@ private:
                     target_pose.translation().y(),
                     target_pose.translation().z()));
 
-    const std::string m_base_link = "base_link";
-    const std::string m_ee_link = "gripper_grasp_point";
-
-    auto robot_description =
-        this->get_parameter("robot_description").as_string();
-
     KDL::Tree kdl_tree;
-    if (!kdl_parser::treeFromString(robot_description, kdl_tree)) {
+    if (!kdl_parser::treeFromString(m_robot_description, kdl_tree)) {
       RCLCPP_ERROR(this->get_logger(),
                    "Failed to parse KDL tree from robot description.");
       return;
@@ -216,6 +229,9 @@ private:
   std::vector<double> m_current_joint_positions;
   std::unordered_map<std::string, size_t> m_joint_mame_to_index;
   std::vector<std::string> m_joint_names;
+  std::string m_base_link;
+  std::string m_ee_link;
+  std::string m_robot_description;
 };
 
 } // namespace rbs_skill_actions

rbs_skill_servers/src/mtp_jtc_cart.cpp

@@ -60,7 +60,7 @@ public:
             s_options);
     this->declare_parameter("robot_description", "");
     this->declare_parameter("base_link", "");
-    this->declare_parameter("robot_ee_link", "");
+    this->declare_parameter("ee_link", "");
 
     auto robot_description =
         this->get_parameter("robot_description").as_string();
@@ -73,7 +73,7 @@ public:
     }
 
     auto base_link = this->get_parameter("base_link").as_string();
-    auto robot_ee_link = this->get_parameter("robot_ee_link").as_string();
+    auto robot_ee_link = this->get_parameter("ee_link").as_string();
     if (base_link.empty() or robot_ee_link.empty()) {
       RCLCPP_ERROR(this->get_logger(),
                    "Describe robot end-effector link and base link to continue");

rbs_utils/rbs_utils/scripts/assembly_config_service.py

@@ -4,24 +4,55 @@ import os
 import rclpy
 from rclpy.node import Node
 import yaml
-from geometry_msgs.msg import Point, Pose, Quaternion, PoseArray
+from geometry_msgs.msg import Point, Pose, Quaternion
+from visualization_msgs.msg import Marker, MarkerArray
 from rbs_utils_interfaces.msg import NamedPose, RelativeNamedPose, AssemblyConfig
 from rbs_utils_interfaces.srv import GetGraspPose, GetWorkspace
+from tf2_ros import TransformListener, Buffer, TransformException
+from rbs_assets_library import get_model_meshes_info
+from env_manager.utils import Tf2Broadcaster
+
 
 class AssemblyConfigService(Node):
     def __init__(self, node_name="assembly_config"):
         super().__init__(node_name)
 
+        # Initialize parameters
         self.declare_parameter("db_folder", "asp-example")
         db_folder = self.get_parameter("db_folder").get_parameter_value().string_value
-        
+
         # Parse the YAML file
-        yaml_file = os.path.join(os.getenv('RBS_ASSEMBLY_DIR', ''), db_folder,'rbs_db.yaml')
+        yaml_file = os.path.join(
+            os.getenv("RBS_ASSEMBLY_DIR", ""), db_folder, "rbs_db.yaml"
+        )
         self.assembly_config = parse_yaml(yaml_file)
-        
-        # Create services
-        self.workspace_service = self.create_service(GetWorkspace, "get_workspace", self.get_workspace_callback)
-        self.grasp_pose_service = self.create_service(GetGraspPose, "get_grasp_pose", self.get_grasp_pose_callback)
+
+        tf2_broadcaster = Tf2Broadcaster(self)
+        for relative_part in self.assembly_config.relative_part:
+            tf2_broadcaster.broadcast_tf(
+                relative_part.relative_at,
+                relative_part.name,
+                translation=(
+                    relative_part.pose.position.x,
+                    relative_part.pose.position.y,
+                    relative_part.pose.position.z,
+                ),
+                rotation=(
+                    relative_part.pose.orientation.x,
+                    relative_part.pose.orientation.y,
+                    relative_part.pose.orientation.z,
+                    relative_part.pose.orientation.w
+                )
+            )
+
+
+        # Services
+        self.workspace_service = self.create_service(
+            GetWorkspace, "get_workspace", self.get_workspace_callback
+        )
+        self.grasp_pose_service = self.create_service(
+            GetGraspPose, "get_grasp_pose", self.get_grasp_pose_callback
+        )
 
     def get_workspace_callback(self, request, response):
         self.get_logger().info("Workspace request received")
@@ -35,52 +66,73 @@ class AssemblyConfigService(Node):
         return response
 
 def parse_yaml(yaml_file):
-    with open(yaml_file, 'r') as file:
+    with open(yaml_file, "r") as file:
         data = yaml.safe_load(file)
-    
-    assets_db = data.get('assets_db', '')
 
-    workspace = [Point(**point) for point in data.get('workspace', [])]
+    assets_db = data.get("assets_db", "")
+
+    workspace = [Point(**point) for point in data.get("workspace", [])]
 
     absolute_part = []
-    for part in data.get('absolute_part', []):
-        position = part['pose']['position']
-        orientation = part['pose']['orientation']
+    for part in data.get("absolute_part", []):
+        position = part["pose"]["position"]
+        orientation = part["pose"].get(
+            "orientation", {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
+        )
         pose = Pose(position=Point(**position), orientation=Quaternion(**orientation))
-        absolute_part.append(NamedPose(name=part['name'], pose=pose))
+        absolute_part.append(NamedPose(name=part["name"], pose=pose))
 
     relative_part = []
-    for part in data.get('relative_part', []):
-        position = part['pose']['position']
-        orientation = part['pose']['orientation']
+    for part in data.get("relative_part", []):
+        position = part["pose"]["position"]
+        orientation = part["pose"].get(
+            "orientation", {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
+        )
         pose = Pose(position=Point(**position), orientation=Quaternion(**orientation))
-        relative_part.append(RelativeNamedPose(name=part['name'], relative_at=part['relative_at'], pose=pose))
+        relative_part.append(
+            RelativeNamedPose(
+                name=part["name"], relative_at=part["relative_at"], pose=pose
+            )
+        )
 
     grasp_pose = []
-    for pose in data.get('grasp_pose', []):
-        position = pose['pose']['position']
-        orientation = pose['pose']['orientation']
-        pose_obj = Pose(position=Point(**position), orientation=Quaternion(**orientation))
-        grasp_pose.append(RelativeNamedPose(name=pose['name'], relative_at=pose['relative_at'], pose=pose_obj))
+    for pose in data.get("grasp_pose", []):
+        position = pose["pose"]["position"]
+        orientation = pose["pose"].get(
+            "orientation", {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
+        )
+        pose_obj = Pose(
+            position=Point(**position), orientation=Quaternion(**orientation)
+        )
+        grasp_pose.append(
+            RelativeNamedPose(
+                name=pose["name"], relative_at=pose["relative_at"], pose=pose_obj
+            )
+        )
 
     extra_poses = []
-    
+
     assembly_config = AssemblyConfig(
         assets_db=assets_db,
         workspace=workspace,
         absolute_part=absolute_part,
         relative_part=relative_part,
         grasp_pose=grasp_pose,
-        extra_poses=extra_poses
+        extra_poses=extra_poses,
     )
-    
+
     return assembly_config
 
-def main(args=None):
-    rclpy.init(args=args)
-    node = AssemblyConfigService()
-    rclpy.spin(node)
-    rclpy.shutdown()
+def main():
+    rclpy.init()
 
-if __name__ == '__main__':
+    executor = rclpy.executors.SingleThreadedExecutor()
+    node = AssemblyConfigService()
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
+        node.destroy_node()
+
+if __name__ == "__main__":
     main()