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)