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
def on_core_robot_state(channel, data):
global core_robot_state
core_robot_state = joint_state_t.decode(data)
