From f1420f15727795ed60bf19369b3ce81f86278a06 Mon Sep 17 00:00:00 2001 From: Kevin Jiang <127545683+ANonABento@users.noreply.github.com> Date: Mon, 30 Jun 2025 17:37:21 +0900 Subject: [PATCH] Kirigirisu merge feature added (#23) Added script to cmake install path Added merge script that communicates with kirigirisu Added another launch exe for kirigirisu visualization Adjusted log content in test/hardware --- openarm_bimanual_description/CMakeLists.txt | 5 ++ .../launch/display.launch.py | 2 +- .../launch/kirigirisu.launch.py | 70 ++++++++++++++++ .../scripts/joint_state_merger.py | 81 +++++++++++++++++++ .../test/test_openarm_hardware.cpp | 8 +- 5 files changed, 164 insertions(+), 2 deletions(-) mode change 100644 => 100755 openarm_bimanual_description/launch/display.launch.py create mode 100644 openarm_bimanual_description/launch/kirigirisu.launch.py create mode 100755 openarm_bimanual_description/scripts/joint_state_merger.py diff --git a/openarm_bimanual_description/CMakeLists.txt b/openarm_bimanual_description/CMakeLists.txt index 8ffbad0..89256be 100644 --- a/openarm_bimanual_description/CMakeLists.txt +++ b/openarm_bimanual_description/CMakeLists.txt @@ -33,4 +33,9 @@ install(DIRECTORY launch meshes rviz urdf worlds DESTINATION share/${PROJECT_NAME} ) +install(PROGRAMS + scripts/joint_state_merger.py + DESTINATION lib/${PROJECT_NAME} +) + ament_package() diff --git a/openarm_bimanual_description/launch/display.launch.py b/openarm_bimanual_description/launch/display.launch.py old mode 100644 new mode 100755 index ad3fd5a..3a59257 --- a/openarm_bimanual_description/launch/display.launch.py +++ b/openarm_bimanual_description/launch/display.launch.py @@ -94,7 +94,7 @@ def generate_launch_description(): ), launch.actions.DeclareLaunchArgument( name="gui", - default_value="True", + default_value="true", description="Flag to enable joint_state_publisher_gui", ), launch.actions.DeclareLaunchArgument( diff --git a/openarm_bimanual_description/launch/kirigirisu.launch.py b/openarm_bimanual_description/launch/kirigirisu.launch.py new file mode 100644 index 0000000..1edfb45 --- /dev/null +++ b/openarm_bimanual_description/launch/kirigirisu.launch.py @@ -0,0 +1,70 @@ +import xacro +from pathlib import Path + +from launch import LaunchDescription +import launch_ros +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + pkg_share = Path( + launch_ros.substitutions.FindPackageShare(package="openarm_bimanual_description").find( + "openarm_bimanual_description" + ) + ) + default_model_path = pkg_share / "urdf/openarm_bimanual.urdf.xacro" + default_rviz_config_path = pkg_share / "rviz/robot_description.rviz" + + # Parse xacro to get robot_description XML + doc = xacro.process_file(str(default_model_path)) + robot_description_xml = doc.toxml() + + # Directly read udrf file (no xacro) + # robot_description_xml = Path(default_model_path).read_text() + + # Node to publish joint states from /joint_states topic or GUI + joint_state_publisher_node = Node( + package="joint_state_publisher", + executable="joint_state_publisher", + name="joint_state_publisher", + output="screen", + parameters=[{"use_sim_time": False}], + ) + + # Node to publish robot state TFs based on robot_description and /joint_states + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[ + {"robot_description": robot_description_xml, + "use_sim_time": False} + ], + ) + + # RViz node to visualize the robot model and TF + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", + arguments=["-d", str(default_rviz_config_path)], + parameters=[{"use_sim_time": False}], + ) + + joint_state_merger_node = Node( + package="openarm_bimanual_description", + executable="joint_state_merger.py", + name="joint_state_merger", + output="screen", + parameters=[{"urdf_path": str(default_model_path)}], + ) + + + return LaunchDescription([ + #joint_state_publisher_node, + robot_state_publisher_node, + joint_state_merger_node, + rviz_node, + ]) diff --git a/openarm_bimanual_description/scripts/joint_state_merger.py b/openarm_bimanual_description/scripts/joint_state_merger.py new file mode 100755 index 0000000..7894133 --- /dev/null +++ b/openarm_bimanual_description/scripts/joint_state_merger.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import JointState +from urdf_parser_py.urdf import URDF +from pathlib import Path +import xacro + +class JointStateMerger(Node): + def __init__(self): + super().__init__('joint_state_merger') + + # Get URDF file parameter + urdf_path = self.declare_parameter('urdf_path', '').get_parameter_value().string_value + if not urdf_path or not Path(urdf_path).exists(): + self.get_logger().error(f"Invalid or missing URDF path: {urdf_path}") + rclpy.shutdown() + return + + # Parse URDF directly from file (not xacro) + # self.robot = URDF.from_xml_file(urdf_path) + + # Process xacro and parse URDF + doc = xacro.process_file(urdf_path) + robot_description_xml = doc.toxml() + self.robot = URDF.from_xml_string(robot_description_xml) + + # Initialize full joint position dict with zeros + self.joint_positions = { + j.name: 0.0 for j in self.robot.joints if j.type != 'fixed' + } + self.joint_names = list(self.joint_positions.keys()) + + # Store joint limits (min/max) for normalization + self.joint_limits = { + j.name: (j.limit.lower, j.limit.upper) if j.limit else (-3.14, 3.14) + for j in self.robot.joints if j.type != 'fixed' + } + + # Publisher for merged joint states + self.joint_state_pub = self.create_publisher(JointState, '/joint_states', 10) + + # Subscription to your partial updates — change topic & msg type as needed + self.partial_update_sub = self.create_subscription( + JointState, '/joint_states_partial', self.partial_update_callback, 10 + ) + + # Optionally subscribe to existing /joint_states to keep current positions up to date + self.existing_joint_states_sub = self.create_subscription( + JointState, '/joint_states', self.existing_joint_states_callback, 10 + ) + + # Timer to publish merged joint states regularly + self.timer = self.create_timer(0.05, self.publish_merged_joint_states) + + def partial_update_callback(self, msg: JointState): + for name, pos in zip(msg.name, msg.position): + if name in self.joint_positions: + self.joint_positions[name] = pos + + def existing_joint_states_callback(self, msg: JointState): + for name, pos in zip(msg.name, msg.position): + if name in self.joint_positions: + self.joint_positions[name] = pos + + def publish_merged_joint_states(self): + msg = JointState() + msg.header.stamp = self.get_clock().now().to_msg() + msg.name = self.joint_names + msg.position = [self.joint_positions[n] for n in self.joint_names] + self.joint_state_pub.publish(msg) + +def main(args=None): + rclpy.init(args=args) + node = JointStateMerger() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/openarm_hardware/test/test_openarm_hardware.cpp b/openarm_hardware/test/test_openarm_hardware.cpp index 1f64b27..e033a57 100644 --- a/openarm_hardware/test/test_openarm_hardware.cpp +++ b/openarm_hardware/test/test_openarm_hardware.cpp @@ -17,7 +17,9 @@ #include +#include "rclcpp/rclcpp.hpp" #include "hardware_interface/resource_manager.hpp" + #include "ros2_control_test_assets/components_urdfs.hpp" #include "ros2_control_test_assets/descriptions.hpp" @@ -124,5 +126,9 @@ class TestOpenArmHW : public ::testing::Test { TEST_F(TestOpenArmHW, load_openarm_hardware_7dof) { auto urdf = ros2_control_test_assets::urdf_head + openarm_hardware_7dof_ + ros2_control_test_assets::urdf_tail; - ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); + ASSERT_NO_THROW({ + auto logger = rclcpp::get_logger("test_logger"); + auto clock = std::make_shared(RCL_ROS_TIME); + hardware_interface::ResourceManager rm(urdf, clock, logger, true, 0); + }); }