def publishState(self, state, ros_pub_dec, numDoF):
# Publish our robot state to ROS topics /robotname/state/* periodically
self.publish_time += 1
if bROS and self.publish_time > ros_pub_dec:
publish_time = 0
# Construct /robotname/state/imu ROS message
msg = Imu()
msg.linear_acceleration.x = state['imu/linear_acceleration'][0]
msg.linear_acceleration.y = state['imu/linear_acceleration'][1]
msg.linear_acceleration.z = state['imu/linear_acceleration'][2]
msg.angular_velocity.x = state['imu/angular_velocity'][0]
msg.angular_velocity.y = state['imu/angular_velocity'][1]
msg.angular_velocity.z = state['imu/angular_velocity'][2]
msg.orientation_covariance = state['imu/orientation_covariance']
msg.linear_acceleration_covariance = np.empty(9)
msg.linear_acceleration_covariance.fill(
state['imu/linear_acceleration_covariance'])
msg.angular_velocity_covariance = np.empty(9)
msg.angular_velocity_covariance.fill(
state['imu/angular_velocity_covariance'])
roll, pitch, yaw = state[
'imu/euler'] # Convert from euler to quaternion using ROS tf
quaternion = tf.transformations.quaternion_from_euler(
roll, pitch, yaw)
msg.orientation.x = quaternion[0]
msg.orientation.y = quaternion[1]
msg.orientation.z = quaternion[2]
msg.orientation.w = quaternion[3]
self.pubs[0].publish(msg)
# Construct /robotname/state/pose
msg = Pose()
msg.orientation.x = quaternion[0]
msg.orientation.y = quaternion[1]
msg.orientation.z = quaternion[2]
msg.orientation.w = quaternion[3]
# TODO: Get height from robot state, have robot calculate it
msg.position.z = 0.0
self.pubs[7].publish(msg)
# Construct /robotname/state/batteryState
msg = BatteryState()
msg.current = state['battery/current']
msg.voltage = state['battery/voltage']
#num_cells = 8
num_cells = 4
if 'battery/cell_voltage' in state:
if state['battery/cell_voltage'] > 0:
num_cells = len(state['battery/cell_voltage'])
# FIXME: do i really need this?
def percentage(total_voltage, num_cells):
# Linearly interpolate charge from voltage
# https://gitlab.com/ghostrobotics/SDK/uploads/6878144fa0e408c91e481c2278215579/image.png
charges = [0.0, 0.2, 0.4, 0.6, 0.8, 1.0]
voltages = [3.2, 3.5, 3.6, 3.65, 3.8, 4.2]
return np.interp(total_voltage / num_cells, voltages, charges)
if msg.percentage <= 0:
msg.percentage = percentage(msg.voltage, num_cells)
self.pubs[1].publish(msg)
# Construct /robotname/state/behaviorId
msg = UInt32()
msg.data = state['behaviorId']
self.pubs[2].publish(msg)
# Construct /robotname/state/behaviorMode
msg = UInt32()
msg.data = state['behaviorMode']
self.pubs[3].publish(msg)
# Construct /robotname/state/joint
msg = JointState()
msg.name = []
msg.position = []
msg.velocity = []
msg.effort = []
for j in range(len(state['joint/position'])):
msg.name.append(str(j))
msg.position.append(state['joint/position'][j])
msg.velocity.append(state['joint/velocity'][j])
msg.effort.append(state['joint/effort'][j])
self.pubs[4].publish(msg)
msg.position = convert_to_leg_model(msg.position)
# Translate for URDF in NGR
# vision60 = False
# if(vision60):
# for i in range(8, 2):
# msg.position[i] += msg.position[i-1];
# msg.velocity[i] += msg.velocity[i-1];
# else:
# # other URDF
# # for URDF of Minitaur FIXME use the class I put in ethernet.py for RobotType
# msg.position.extend([0, 0, 0, 0, 0, 0, 0, 0])
# msg.position[11], msg.position[10], r = minitaurFKForURDF(msg.position[0], msg.position[1])
# msg.position[14], msg.position[15], r = minitaurFKForURDF(msg.position[2], msg.position[3])
# msg.position[9], msg.position[8], r = minitaurFKForURDF(msg.position[4], msg.position[5])
# msg.position[13], msg.position[12], r = minitaurFKForURDF(msg.position[6], msg.position[7])
# # other URDF problems (order important)
# for j in range(4):
# msg.position[j] = -msg.position[j]
self.pubs[5].publish(msg)
# Construct /robotname/state/joystick
msg = Joy()
msg.axes = state['joy/axes']
msg.buttons = state['joy/buttons']
self.pubs[6].publish(msg)
