def on_core_robot_state(channel, data): core_robot_state = joint_state_t.decode(data) #m = pose_t.decode(data) o = robot_state_t() o.utime = core_robot_state.utime o.num_joints = core_robot_state.num_joints o.joint_name = core_robot_state.joint_name o.joint_position = core_robot_state.joint_position o.joint_velocity = core_robot_state.joint_velocity o.joint_effort = core_robot_state.joint_effort nrot = quaternion_t() nvec = vector_3d_t() p = position_3d_t() p.rotation = nrot p.translation = nvec o.pose = p t = twist_t() t.linear_velocity = nvec t.angular_velocity = nvec o.twist = t o.pose.translation.x = 0 o.pose.translation.y = 0 o.pose.translation.z = 0 o.pose.rotation.w = 1 o.pose.rotation.x = 0 o.pose.rotation.y = 0 o.pose.rotation.z = 0 ft = force_torque_t() o.force_torque = ft lc.publish("EST_ROBOT_STATE", o.encode())
def on_joint_state(channel, data): global joint_state joint_state = joint_state_t.decode(data)
def on_core_robot_state(channel, data): global core_robot_state core_robot_state = joint_state_t.decode(data)
def on_joint_state(channel, data): global joint_state, received_joint_state joint_state = joint_state_t.decode(data) received_joint_state = True