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 @@ + + 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) @@ -36,6 +40,8 @@ urdf_path: $(arg urdf_path) servo_config_path: $(arg servo_config_path) + publish_imu: $(arg publish_imu) + imu_frame_id: $(arg imu_frame_id) 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 @@ -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