def publishStateRos(tup, ros_pub_dec): # Publish our robot state to ROS topics /robotname/state/* periodically global publish_time, count publish_time += 1 #print bROS #print publish_time #print ros_pub_dec if bROS: #print("hey") 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']) #print tup #print len(tup) roll = tup[2] pitch = tup[3] yaw = tup[4] positions = tup[5:13] #print positions velocities = tup[13:21] #print velocities torques = tup[21:29] #print torques 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] 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 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']) # 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) # pubs[1].publish(msg) # # Construct /robotname/state/behaviorId # msg = UInt32() # msg.data = state['behaviorId'] # pubs[2].publish(msg) # # # Construct /robotname/state/behaviorMode # msg = UInt32() # msg.data = state['behaviorMode'] # pubs[3].publish(msg) # Construct /robotname/state/joint msg = JointState() msg.name = [] msg.position = [] msg.velocity = [] msg.effort = [] for j in range(8): msg.name.append(str(j)) msg.position.append(positions[j]) msg.velocity.append(velocities[j]) msg.effort.append(torques[j]) pubs[4].publish(msg) # 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.header.seq = count count = count + 1 msg.header.stamp = rospy.Time.now() msg.position.extend([0, 0, 0, 0, 0, 0, 0, 0]) msg.velocity.extend( [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) msg.effort.extend([0, 0, 0, 0, 0, 0, 0, 0]) msg.name.extend(['8', '9', '10', '11', '12', '13', '14', '15']) 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) msg.position[11] = -msg.position[11] msg.position[14] = -msg.position[14] msg.position[9] = -msg.position[9] msg.position[13] = -msg.position[13] pubs[5].publish(msg)
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)
def publishStateRos(tup, ros_pub_dec): # Publish our robot state to ROS topics /robotname/state/* periodically global publish_time, count, impossible_motion publish_time += 1 # print(tup) if bROS: publish_time = 0 # Construct /robotname/state/imu ROS message msg = Imu() roll = tup[2] pitch = tup[3] yaw = tup[4] positions = tup[5:13] #print positions velocities = tup[13:21] #print velocities torques = tup[21:29] # #print torques currents = tup[29:37] # print("Torque : ", max(np.array(torques)), min(np.array(torques))) # print("Currents : ", max(np.array(currents)), min(np.array(currents))) temperatures = tup[37:45] impossible_motion = tup[45] 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] 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 pubs[3].publish(msg) msg = JointState() msg.name = [] msg.position = [] msg.velocity = [] msg.effort = [] for j in range(8): msg.name.append(str(j)) msg.position.append(positions[j]) msg.velocity.append(velocities[j]) msg.effort.append(torques[j]) pubs[1].publish(msg) # 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.header.seq = count count = count +1 msg.header.stamp = rospy.Time.now() msg.position.extend([0, 0, 0, 0, 0, 0, 0, 0]) msg.velocity.extend([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) msg.effort.extend([0, 0, 0, 0, 0, 0, 0, 0]) msg.name.extend(['8', '9', '10', '11', '12', '13', '14', '15']) 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) msg.position[11] = -msg.position[11] msg.position[14] = -msg.position[14] msg.position[9] = -msg.position[9] msg.position[13] = -msg.position[13] pubs[2].publish(msg) msg = Float64MultiArray() msg.data = currents pubs[4].publish(msg) msg = Float64MultiArray() msg.data = temperatures pubs[5].publish(msg) msg = Float64MultiArray() msg.data = [max(abs(np.array(torques))), max(abs(np.array(currents)))] pubs[6].publish(msg) msg = Float64MultiArray() msg.data = [sum(abs(np.array(torques))), sum(abs(np.array(currents)))] pubs[7].publish(msg) msg = Bool() msg.data = impossible_motion pubs[8].publish(msg)