feat(dual_arm_action_server): publish joint_states from config

- add joint_names and joint start indices to config
- publish joint_states only when names configured
- add joint index parameters and member storage
- pass robot_description to action server launch
- add sensor_msgs dependency

src/dual_arm_action_server/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(interfaces REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 
 include_directories(
@@ -27,6 +28,7 @@ ament_target_dependencies(dual_arm_action_server_node
   rclcpp_action
   interfaces
   geometry_msgs
+  sensor_msgs
   tf2
 )
 

src/dual_arm_action_server/config/dual_arm_action_server.yaml

@@ -7,4 +7,36 @@ dual_arm_action_server:
     right_push_ip: "192.168.2.19"
     push_port: 8089
     push_cycle: 20
+    left_arm_joint_start_index: 9     # start index of left arm: left_joint1
+    right_arm_joint_start_index: 15   # start index of right arm: right_joint1
+    joint_names:
+      - left_behind_hip_joint
+      - left_behind_leg_joint
+      - left_behind_wheel_joint
+      - right_behind_hip_joint
+      - right_behind_leg_joint
+      - right_behind_wheel_joint
+      - body_joint
+      - body_2_joint
+      - head_joint
+      - left_joint1
+      - left_joint2
+      - left_joint3
+      - left_joint4
+      - left_joint5
+      - left_joint6
+      - right_joint1
+      - right_joint2
+      - right_joint3
+      - right_joint4
+      - right_joint5
+      - right_joint6
+      - left_front_roll_joint
+      - left_front_hip_joint
+      - left_front_knee_joint
+      - left_front_wheel_joint
+      - right_front_roll_joint
+      - right_front_hip_joint
+      - right_front_knee_joint
+      - right_front_wheel_joint
 

src/dual_arm_action_server/include/dual_arm_action_server/dual_arm_action_server.hpp

@@ -5,6 +5,7 @@
 #include "interfaces/msg/arm_error.hpp"
 #include "interfaces/srv/clear_arm_error.hpp"
 #include "interfaces/srv/inverse_kinematics.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 #include <atomic>
@@ -40,6 +41,7 @@ private:
     const std::shared_ptr<interfaces::srv::InverseKinematics::Request> request,
     std::shared_ptr<interfaces::srv::InverseKinematics::Response> response);
   void error_timer_callback();
+  void joint_state_timer_callback();
 
   bool validate_goal(const DualArm::Goal & goal, std::string & error_msg) const;
   std::vector<interfaces::msg::ArmMotionParams> normalize_goal_params(
@@ -54,6 +56,12 @@ private:
   rclcpp::Publisher<interfaces::msg::ArmError>::SharedPtr arm_error_pub_;
   rclcpp::TimerBase::SharedPtr error_timer_;
 
+  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_pub_;
+  rclcpp::TimerBase::SharedPtr joint_state_timer_;
+  sensor_msgs::msg::JointState joint_state_msg_;
+  size_t left_arm_joint_start_index_;
+  size_t right_arm_joint_start_index_;
+
   RealRobotDriver driver_;
   std::atomic<bool> executing_left_;
   std::atomic<bool> executing_right_;

src/dual_arm_action_server/launch/dual_arm_action_server.launch.py

@@ -4,6 +4,20 @@ from ament_index_python.packages import get_package_share_directory
 import os
 
 
+def load_urdf_file(file_path: str) -> str:
+    if not os.path.exists(file_path):
+        raise FileNotFoundError(f"URDF file does not exist: {file_path}")
+    if not os.path.isfile(file_path):
+        raise IsADirectoryError(f"URDF path is not a file: {file_path}")
+    with open(file_path, "r") as f:
+        content = f.read()
+    if not content.strip():
+        raise ValueError(f"URDF file is empty: {file_path}")
+    if "<robot" not in content:
+        raise ValueError(f"URDF file missing <robot> tag: {file_path}")
+    return content
+
+
 def generate_launch_description():
     config_path = os.path.join(
         get_package_share_directory("dual_arm_action_server"),
@@ -11,6 +25,13 @@ def generate_launch_description():
         "dual_arm_action_server.yaml",
     )
 
+    urdf_path = os.path.join(
+        get_package_share_directory("dual_arm_moveit_action_server"),
+        "urdf",
+        "FHrobot.urdf",
+    )
+    urdf_content = load_urdf_file(urdf_path)
+
     return LaunchDescription([
         Node(
             package="dual_arm_action_server",
@@ -18,6 +39,11 @@ def generate_launch_description():
             name="dual_arm_action_server",
             output="screen",
             parameters=[config_path],
-        )
+        ),
+        Node(
+            package="robot_state_publisher",
+            executable="robot_state_publisher",
+            parameters=[{"robot_description": urdf_content}],
+        ),
     ])
 

src/dual_arm_action_server/package.xml

@@ -12,6 +12,7 @@
   <depend>rclcpp_action</depend>
   <depend>interfaces</depend>
   <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
   <depend>tf2</depend>
 
   <test_depend>ament_lint_auto</test_depend>

src/dual_arm_action_server/src/dual_arm_action_server_node.cpp

@@ -9,6 +9,9 @@ namespace dual_arm_action_server
 
 DualArmActionServer::DualArmActionServer(const rclcpp::Node::SharedPtr & node)
 : node_(node),
+  left_arm_joint_start_index_(9),
+  right_arm_joint_start_index_(15),
+  driver_(),
   executing_left_(false),
   executing_right_(false)
 {
@@ -23,6 +26,9 @@ void DualArmActionServer::initialize()
   node_->declare_parameter<std::string>("right_push_ip");
   node_->declare_parameter<int>("push_port");
   node_->declare_parameter<int>("push_cycle");
+  node_->declare_parameter<std::vector<std::string>>("joint_names", std::vector<std::string>());
+  node_->declare_parameter<int>("left_arm_joint_start_index", 9);
+  node_->declare_parameter<int>("right_arm_joint_start_index", 15);
 
   std::string left_arm_ip;
   std::string right_arm_ip;
@@ -97,6 +103,24 @@ void DualArmActionServer::initialize()
     std::chrono::milliseconds(500),
     std::bind(&DualArmActionServer::error_timer_callback, this));
 
+  joint_state_msg_.name = node_->get_parameter("joint_names").as_string_array();
+  if (joint_state_msg_.name.empty()) {
+    RCLCPP_ERROR(node_->get_logger(), "joint_names is empty, joint_states will not be published");
+  } else {
+    left_arm_joint_start_index_ = static_cast<size_t>(
+      node_->get_parameter("left_arm_joint_start_index").as_int());
+    right_arm_joint_start_index_ = static_cast<size_t>(
+      node_->get_parameter("right_arm_joint_start_index").as_int());
+
+    joint_state_pub_ = node_->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
+    joint_state_msg_.position.resize(joint_state_msg_.name.size(), 0.0);
+    joint_state_msg_.header.frame_id = "base_link";
+
+    joint_state_timer_ = node_->create_wall_timer(
+      std::chrono::milliseconds(50),
+      std::bind(&DualArmActionServer::joint_state_timer_callback, this));
+  }
+
   RCLCPP_INFO(node_->get_logger(), "DualArm action server is ready");
 }
 
@@ -472,6 +496,35 @@ void DualArmActionServer::error_timer_callback()
   publish_error(RIGHT_ARM);
 }
 
+void DualArmActionServer::joint_state_timer_callback()
+{
+  if (!joint_state_pub_ || !driver_.is_connected()) {
+    return;
+  }
+
+  constexpr double kDegToRad = 3.14159265358979323846 / 180.0;
+
+  std::vector<double> joints_left;
+  std::vector<double> joints_right;
+  if (!driver_.get_current_joints(joints_left, joints_right)) {
+    return;
+  }
+
+  if (joint_state_msg_.position.size() >= right_arm_joint_start_index_ + USED_ARM_DOF) {
+    for (size_t i = 0; i < USED_ARM_DOF && i < joints_left.size(); ++i) {
+      joint_state_msg_.position[left_arm_joint_start_index_ + i] = joints_left[i] * kDegToRad;
+    }
+    for (size_t i = 0; i < USED_ARM_DOF && i < joints_right.size(); ++i) {
+      joint_state_msg_.position[right_arm_joint_start_index_ + i] = joints_right[i] * kDegToRad;
+    }
+  }
+
+  joint_state_msg_.header.stamp = node_->get_clock()->now();
+  joint_state_msg_.velocity.clear();
+  joint_state_msg_.effort.clear();
+  joint_state_pub_->publish(joint_state_msg_);
+}
+
 }  // namespace dual_arm_action_server
 
 int main(int argc, char ** argv)