From 593603af3592dc3a1573f6bab72106b832e1e9e2 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Wed, 10 Jan 2024 21:20:30 +0900
Subject: [PATCH] Enable publishing imu (#12)

---
 .../launch/kxr_controller.launch              |  6 +++++
 .../node_scripts/rcb4_ros_bridge.py           | 23 +++++++++++++++++++
 ros/kxr_controller/test/kxr_controller.test   |  4 ++++
 3 files changed, 33 insertions(+)

diff --git a/ros/kxr_controller/launch/kxr_controller.launch b/ros/kxr_controller/launch/kxr_controller.launch
index 1a8f6a2d..83101f66 100644
--- a/ros/kxr_controller/launch/kxr_controller.launch
+++ b/ros/kxr_controller/launch/kxr_controller.launch
@@ -3,6 +3,8 @@
   <arg name="urdf_path" default="$(find kxr_models)/urdf/kxrl2l2a6h2m.urdf" />
   <arg name="servo_config_path" default="$(find kxr_models)/config/kxrl2l2a6h2m_servo_config.yaml" />
   <arg name="namespace" default="" />
+  <arg name="publish_imu" default="true" />
+  <arg name="imu_frame_id" default="/bodyset94472077639384" />
 
   <group if="$(eval len(arg('namespace')) > 0)" ns="$(arg namespace)" >
     <node name="rcb4_ros_bridge"
@@ -13,6 +15,8 @@
       <rosparam subst_value="true" >
         urdf_path: $(arg urdf_path)
         servo_config_path: $(arg servo_config_path)
+        publish_imu: $(arg publish_imu)
+        imu_frame_id: $(arg namespace)/$(arg imu_frame_id)
       </rosparam>
     </node>
 
@@ -36,6 +40,8 @@
       <rosparam subst_value="true" >
         urdf_path: $(arg urdf_path)
         servo_config_path: $(arg servo_config_path)
+        publish_imu: $(arg publish_imu)
+        imu_frame_id: $(arg imu_frame_id)
       </rosparam>
     </node>
 
diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index c4a41903..f0b199e0 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -12,6 +12,7 @@
 from kxr_controller.msg import ServoOnOffResult
 import numpy as np
 import rospy
+import sensor_msgs.msg
 from sensor_msgs.msg import JointState
 from skrobot.model import RobotModel
 from skrobot.utils.urdf import no_mesh_load_mode
@@ -194,6 +195,15 @@ def __init__(self):
         self.proc_robot_state_publisher = run_robot_state_publisher(
             clean_namespace)
 
+        self.publish_imu = rospy.get_param('~publish_imu', True)
+        if self.publish_imu:
+            self.imu_frame_id = rospy.get_param(
+                '~imu_frame_id', clean_namespace + '/' + r.root_link.name)
+            self.imu_publisher = rospy.Publisher(
+                clean_namespace + '/imu',
+                sensor_msgs.msg.Imu,
+                queue_size=1)
+
     def velocity_command_joint_state_callback(self, msg):
         servo_ids = []
         av_length = len(self.arm.servo_sorted_ids)
@@ -315,6 +325,15 @@ def servo_on_off_callback(self, goal):
             self.arm.servo_angle_vector(servo_ids, servo_vector, velocity=10)
         return self.servo_on_off_server.set_succeeded(ServoOnOffResult())
 
+    def create_imu_message(self):
+        msg = sensor_msgs.msg.Imu()
+        msg.header.frame_id = self.imu_frame_id
+        msg.header.stamp = rospy.Time.now()
+        q_wxyz = self.arm.read_quaternion()
+        (msg.orientation.w, msg.orientation.x,
+         msg.orientation.y, msg.orientation.z) = q_wxyz
+        return msg
+
     def run(self):
         rate = rospy.Rate(100)
 
@@ -335,6 +354,10 @@ def run(self):
                                 av[self.id_to_index[servo_id]]))
                         msg.name.append(name)
             self.current_joint_states_pub.publish(msg)
+
+            if self.publish_imu:
+                imu_msg = self.create_imu_message()
+                self.imu_publisher.publish(imu_msg)
             rate.sleep()
 
 
diff --git a/ros/kxr_controller/test/kxr_controller.test b/ros/kxr_controller/test/kxr_controller.test
index d0db389e..29209365 100644
--- a/ros/kxr_controller/test/kxr_controller.test
+++ b/ros/kxr_controller/test/kxr_controller.test
@@ -24,6 +24,8 @@
         timeout_3: 30
         topic_4: kxr_fullbody_controller/servo_on_off/status
         timeout_4: 30
+        topic_5: imu
+        timeout_5: 30
       </rosparam>
     </test>
   </group>
@@ -44,6 +46,8 @@
         timeout_3: 30
         topic_4: kxr_fullbody_controller/servo_on_off/status
         timeout_4: 30
+        topic_5: imu
+        timeout_5: 30
       </rosparam>
     </test>
   </group>