From c4c13d55b1e7bf7b8b30e604860ae579c4b37984 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Sat, 30 Nov 2024 19:32:37 +0300 Subject: [PATCH] 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 | 5 +- rbs_skill_servers/src/mtjs_jtc.cpp | 12 +- rbs_skill_servers/src/mtp_jtc.cpp | 36 ++++-- rbs_skill_servers/src/mtp_jtc_cart.cpp | 4 +- .../scripts/assembly_config_service.py | 118 +++++++++++++----- 5 files changed, 127 insertions(+), 48 deletions(-) diff --git a/rbs_skill_servers/launch/skills.launch.py b/rbs_skill_servers/launch/skills.launch.py index aae19fa..d971900 100644 --- a/rbs_skill_servers/launch/skills.launch.py +++ b/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", ) diff --git a/rbs_skill_servers/src/mtjs_jtc.cpp b/rbs_skill_servers/src/mtjs_jtc.cpp index 6fba882..f36ee49 100644 --- a/rbs_skill_servers/src/mtjs_jtc.cpp +++ b/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( diff --git a/rbs_skill_servers/src/mtp_jtc.cpp b/rbs_skill_servers/src/mtp_jtc.cpp index c88f637..1b6aba6 100644 --- a/rbs_skill_servers/src/mtp_jtc.cpp +++ b/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 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::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 &target_joint_values, const double duration) { + RCLCPP_INFO(this->get_logger(), "Starting generate_trajectory"); const int num_points = 100; std::vector points; for (int i = 0; i <= num_points; ++i) { trajectory_msgs::msg::JointTrajectoryPoint point; + double t = static_cast(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 &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 m_current_joint_positions; std::unordered_map m_joint_mame_to_index; std::vector m_joint_names; + std::string m_base_link; + std::string m_ee_link; + std::string m_robot_description; }; } // namespace rbs_skill_actions diff --git a/rbs_skill_servers/src/mtp_jtc_cart.cpp b/rbs_skill_servers/src/mtp_jtc_cart.cpp index 4fdcc98..c2b17d5 100644 --- a/rbs_skill_servers/src/mtp_jtc_cart.cpp +++ b/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"); diff --git a/rbs_utils/rbs_utils/scripts/assembly_config_service.py b/rbs_utils/rbs_utils/scripts/assembly_config_service.py index eaa3b3e..e932949 100755 --- a/rbs_utils/rbs_utils/scripts/assembly_config_service.py +++ b/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()