Merge pull request #43 from iory/proximity

Add publish sensor values
iory · Mar 1, 2024 · 91c3122 · 91c3122
2 parents b9dfd5c + 1d8f3ac
commit 91c3122
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 0 deletions.
diff --git a/ros/kxr_controller/launch/kxr_controller.launch b/ros/kxr_controller/launch/kxr_controller.launch
@@ -4,6 +4,7 @@
   <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="publish_sensor" default="false" />
   <arg name="imu_frame_id" default="/bodyset94472077639384" />
   <arg name="control_loop_rate" default="20" />
   <arg name="use_rcb4" default="false" doc="Flag to use RCB4 mini board"/>
@@ -27,6 +28,7 @@
         urdf_path: $(arg urdf_path)
         servo_config_path: $(arg servo_config_path)
         publish_imu: $(arg publish_imu)
+        publish_sensor: $(arg publish_sensor)
         imu_frame_id: $(arg namespace)/$(arg imu_frame_id)
         use_rcb4: $(arg use_rcb4)
       </rosparam>
@@ -48,6 +50,7 @@
         urdf_path: $(arg urdf_path)
         servo_config_path: $(arg servo_config_path)
         publish_imu: $(arg publish_imu)
+        publish_sensor: $(arg publish_sensor)
         imu_frame_id: $(arg imu_frame_id)
         use_rcb4: $(arg use_rcb4)
       </rosparam>

diff --git a/ros/kxr_controller/launch/kxr_controller_for_wheel.launch b/ros/kxr_controller/launch/kxr_controller_for_wheel.launch
@@ -2,6 +2,8 @@
 
   <arg name="urdf_path" default="$(find kxr_models)/urdf/kxrmw4a6h2m.urdf" />
   <arg name="servo_config_path" default="$(find kxr_models)/config/kxrmw4a6h2m_servo_config.yaml" />
+  <arg name="publish_imu" default="true" />
+  <arg name="publish_sensor" default="false" />
   <arg name="use_rcb4" default="false" doc="Flag to use RCB4 mini board"/>
   <arg name="device" default="" doc="Device path"/>
   <arg name="model_server_port" default="8123" />
@@ -12,6 +14,8 @@
     <arg name="use_rcb4" value="$(arg use_rcb4)" />
     <arg name="device" value="$(arg device)" />
     <arg name="model_server_port" value="$(arg model_server_port)" />
+    <arg name="publish_imu" value="$(arg publish_imu)" />
+    <arg name="publish_sensor" value="$(arg publish_sensor)" />
   </include>
 
   <include file="$(find kxr_controller)/launch/diff_drive_controller.launch" />

diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -7,6 +7,7 @@
 import sys
 
 import actionlib
+import geometry_msgs.msg
 from kxr_controller.msg import ServoOnOffAction
 from kxr_controller.msg import ServoOnOffResult
 import numpy as np
@@ -194,15 +195,19 @@ def __init__(self):
             clean_namespace)
 
         self.publish_imu = rospy.get_param('~publish_imu', True)
+        self.publish_sensor = rospy.get_param('~publish_sensor', False)
         if self.interface.__class__.__name__ == 'RCB4Interface':
             self.publish_imu = False
+            self.publish_sensor = False
         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)
+        if self.publish_sensor:
+            self._sensor_publisher_dict = {}
 
     def __del__(self):
         if self.proc_controller_spawner:
@@ -344,6 +349,26 @@ def create_imu_message(self):
          msg.linear_acceleration.z) = acc
         return msg
 
+    def publish_sensor_values(self):
+        stamp = rospy.Time.now()
+        msg = geometry_msgs.msg.WrenchStamped()
+        msg.header.stamp = stamp
+        for sensor in self.interface.all_jointbase_sensors():
+            for i in range(4):
+                for typ in ['proximity', 'force']:
+                    key = 'kjs_{}_{}_{}'.format(sensor.id, typ, i)
+                    if typ == 'proximity':
+                        msg.wrench.force.x = sensor.ps[i]
+                    elif typ == 'force':
+                        msg.wrench.force.x = sensor.adc[i]
+                    if key not in self._sensor_publisher_dict:
+                        self._sensor_publisher_dict[key] = rospy.Publisher(
+                            self.clean_namespace
+                            + '/kjs/{}/{}/{}'.format(sensor.id, typ, i),
+                            geometry_msgs.msg.WrenchStamped,
+                            queue_size=1)
+                    self._sensor_publisher_dict[key].publish(msg)
+
     def run(self):
         rate = rospy.Rate(rospy.get_param(
             self.clean_namespace + '/control_loop_rate', 20))
@@ -376,6 +401,8 @@ def run(self):
             if self.publish_imu and self.imu_publisher.get_num_connections():
                 imu_msg = self.create_imu_message()
                 self.imu_publisher.publish(imu_msg)
+            if self.publish_sensor:
+                self.publish_sensor_values()
             rate.sleep()