import logging; logger = logging.getLogger("morse." + __name__)
import json
from morse.middleware.socket_datastream import SocketPublisher
[docs]def fill_missing_pr2_joints(joints):
pr2_joints = {
'laser_tilt_mount_joint':0.0,
'fl_caster_rotation_joint':0.0,
'fl_caster_l_wheel_joint':0.0,
'fl_caster_r_wheel_joint':0.0,
'fr_caster_rotation_joint':0.0,
'fr_caster_l_wheel_joint':0.0,
'fr_caster_r_wheel_joint':0.0,
'bl_caster_rotation_joint':0.0,
'bl_caster_l_wheel_joint':0.0,
'bl_caster_r_wheel_joint':0.0,
'br_caster_rotation_joint':0.0,
'br_caster_l_wheel_joint':0.0,
'br_caster_r_wheel_joint':0.0,
'r_gripper_motor_slider_joint':0.0,
'r_gripper_motor_screw_joint':0.0,
'r_gripper_l_finger_joint':0.0,
'r_gripper_r_finger_joint':0.0,
'r_gripper_l_finger_tip_joint':0.0,
'r_gripper_r_finger_tip_joint':0.0,
'r_gripper_joint':0.0,
'l_gripper_motor_slider_joint':0.0,
'l_gripper_motor_screw_joint':0.0,
'l_gripper_l_finger_joint':0.0,
'l_gripper_r_finger_joint':0.0,
'l_gripper_l_finger_tip_joint':0.0,
'l_gripper_r_finger_tip_joint':0.0,
'l_gripper_joint':0.0,
'torso_lift_joint':0.0,
'torso_lift_motor_screw_joint':0.0,
'head_pan_joint':0.0,
'head_tilt_joint':0.0,
'l_shoulder_pan_joint':0.0,
'l_shoulder_lift_joint':0.0,
'l_upper_arm_roll_joint':0.0,
'l_elbow_flex_joint':0.0,
'l_forearm_roll_joint':0.0,
'l_wrist_flex_joint':0.0,
'l_wrist_roll_joint':0.0,
'r_shoulder_pan_joint':0.0,
'r_shoulder_lift_joint':0.0,
'r_upper_arm_roll_joint':0.0,
'r_elbow_flex_joint':0.0,
'r_forearm_roll_joint':0.0,
'r_wrist_flex_joint':0.0,
'r_wrist_roll_joint':0.0}
for k, v in joints.items():
if k != 'timestamp':
for name, value in v.items():
pr2_joints[name] = value
return pr2_joints
[docs]class PR2JointStatePublisher(SocketPublisher):
_type_name = "a JSON dict containing the values of each of the Willow Garage's PR2 joints"
[docs] def encode(self):
joints = fill_missing_pr2_joints(self.component_instance.local_data)
return (json.dumps(joints) + '\n').encode()