def __on_packet(self, buf):
global last_joint_states, last_joint_states_lock
now = rospy.get_rostime()
stateRT = RobotStateRT.unpack(buf)
self.last_stateRT = stateRT
msg = JointState()
msg.header.stamp = now
msg.header.frame_id = "From real-time state data"
msg.name = joint_names
msg.position = [0.0] * 6
for i, q in enumerate(stateRT.q_actual):
msg.position[i] = q + joint_offsets.get(joint_names[i], 0.0)
msg.velocity = stateRT.qd_actual
msg.effort = [0] * 6
pub_joint_states.publish(msg)
with last_joint_states_lock:
last_joint_states = msg
wrench_msg = WrenchStamped()
wrench_msg.header.stamp = now
wrench_msg.wrench.force.x = stateRT.tcp_force[0]
wrench_msg.wrench.force.y = stateRT.tcp_force[1]
wrench_msg.wrench.force.z = stateRT.tcp_force[2]
wrench_msg.wrench.torque.x = stateRT.tcp_force[3]
wrench_msg.wrench.torque.y = stateRT.tcp_force[4]
wrench_msg.wrench.torque.z = stateRT.tcp_force[5]
pub_wrench.publish(wrench_msg)
def __on_packet(buf):
stateRT = RobotStateRT.unpack(buf)
msg = RobotStateRTMsg()
msg.time = stateRT.time
msg.q_target = stateRT.q_target
msg.qd_target = stateRT.qd_target
msg.qdd_target = stateRT.qdd_target
msg.i_target = stateRT.i_target
msg.m_target = stateRT.m_target
msg.q_actual = stateRT.q_actual
msg.qd_actual = stateRT.qd_actual
msg.i_actual = stateRT.i_actual
msg.tool_acc_values = stateRT.tool_acc_values
msg.tcp_force = stateRT.tcp_force
msg.tool_vector = stateRT.tool_vector
msg.tcp_speed = stateRT.tcp_speed
msg.digital_input_bits = stateRT.digital_input_bits
msg.motor_temperatures = stateRT.motor_temperatures
msg.controller_timer = stateRT.controller_timer
msg.test_value = stateRT.test_value
msg.robot_mode = stateRT.robot_mode
msg.joint_modes = stateRT.joint_modes
pub_robot_stateRT.publish(msg)
msg = JointState()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = "From real-time state data"
msg.name = joint_names
msg.position = [0.0] * 6
for i, q in enumerate(stateRT.q_actual):
msg.position[i] = q + joint_offsets.get(joint_names[i], 0.0)
msg.velocity = stateRT.qd_actual
msg.effort = [0] * 6
pub_joint_statesRT.publish(msg)
def __on_packet(self, buf):
global last_joint_states, last_joint_states_lock
now = rospy.get_rostime()
stateRT = RobotStateRT.unpack(buf)
self.last_stateRT = stateRT
msg = JointState()
msg.header.stamp = now
msg.header.frame_id = "From real-time state data"
msg.name = joint_names
msg.position = [0.0] * 6
for i, q in enumerate(stateRT.q_actual):
msg.position[i] = q + joint_offsets.get(joint_names[i], 0.0)
msg.velocity = stateRT.qd_actual
msg.effort = [0]*6
pub_joint_states.publish(msg)
with last_joint_states_lock:
last_joint_states = msg
wrench_msg = WrenchStamped()
wrench_msg.header.stamp = now
wrench_msg.wrench.force.x = stateRT.tcp_force[0]
wrench_msg.wrench.force.y = stateRT.tcp_force[1]
wrench_msg.wrench.force.z = stateRT.tcp_force[2]
wrench_msg.wrench.torque.x = stateRT.tcp_force[3]
wrench_msg.wrench.torque.y = stateRT.tcp_force[4]
wrench_msg.wrench.torque.z = stateRT.tcp_force[5]
pub_wrench.publish(wrench_msg)
def __on_packet(buf):
stateRT = RobotStateRT.unpack(buf)
msg = RobotStateRTMsg()
msg.time = stateRT.time
msg.q_target = stateRT.q_target
msg.qd_target = stateRT.qd_target
msg.qdd_target = stateRT.qdd_target
msg.i_target = stateRT.i_target
msg.m_target = stateRT.m_target
msg.q_actual = stateRT.q_actual
msg.qd_actual = stateRT.qd_actual
msg.i_actual = stateRT.i_actual
msg.tool_acc_values = stateRT.tool_acc_values
msg.tcp_force = stateRT.tcp_force
msg.tool_vector = stateRT.tool_vector
msg.tcp_speed = stateRT.tcp_speed
msg.digital_input_bits = stateRT.digital_input_bits
msg.motor_temperatures = stateRT.motor_temperatures
msg.controller_timer = stateRT.controller_timer
msg.test_value = stateRT.test_value
msg.robot_mode = stateRT.robot_mode
msg.joint_modes = stateRT.joint_modes
pub_robot_stateRT.publish(msg)
msg = JointState()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = "From real-time state data"
msg.name = joint_names
msg.position = [0.0] * 6
for i, q in enumerate(stateRT.q_actual):
msg.position[i] = q + joint_offsets.get(joint_names[i], 0.0)
msg.velocity = stateRT.qd_actual
msg.effort = [0]*6
pub_joint_statesRT.publish(msg)
