def publishWrench(data, hand): wr = data.estimated_external_wrench wrench_msg = Wrench() wrench_msg.force = Vector3(wr.x, wr.y, wr.z) wrench_msg.torque = Vector3(wr.c, wr.b, wr.a) wrench_stamped_msg = WrenchStamped() wrench_stamped_msg.wrench = wrench_msg wrench_stamped_msg.header.stamp = rospy.Time(0) wrench_stamped_msg.header.frame_id = hand_to_frame[hand] hand_wrench_pub[hand].publish(wrench_stamped_msg)
def run(self): while not self._isEnd: if self._isRunning: self.pub = rospy.Publisher(self._topic, WrenchStamped, queue_size=10) while self._isRunning: msg = WrenchStamped() msg.header.stamp = rospy.Time.now() self._lock.acquire() msg.wrench = self._wrench msg.header.frame_id = self._frameId self._lock.release() self.pub.publish(msg) self._rate.sleep() rospy.sleep(0.1) print "Thread ended"
def got_joy_msg(joy_msg): # 6 start, 7 stop if joy_msg.buttons[6]: pass elif joy_msg.buttons[7]: pass if joy_msg.buttons[1]: pass elif joy_msg.buttons[3]: pass axes = list(joy_msg.axes) if np.fabs(axes[1]) < 0.1: axes[1] = 0.0 if np.fabs(axes[0]) < 0.1: axes[0] = 0.0 if np.fabs(axes[3]) < 0.1: axes[3] = 0.0 for i in range(len(axes)): axes[i] = np.round(axes[i], 2) linear_x = axes[1] * max_x_force linear_y = axes[0] * max_y_force angular_z = axes[3] * -max_z_torque seq = joy_msg.header.seq wrench = WrenchStamped() wrench.wrench = Wrench() wrench.wrench.force = Vector3() wrench.wrench.torque = Vector3() wrench.wrench.force.x = linear_x wrench.wrench.force.y = linear_y wrench.wrench.force.z = 0 wrench.wrench.torque.x = 0 wrench.wrench.torque.y = 0 wrench.wrench.torque.z = angular_z wrench.header.seq = seq wrench.header.frame_id = '/robot' wrench.header.stamp = joy_msg.header.stamp pub.publish(wrench)
def publish_wrench(fx, fy, tau): wrench = WrenchStamped() wrench.wrench = Wrench() wrench.wrench.force = Vector3() wrench.wrench.torque = Vector3() wrench.wrench.force.x = fx wrench.wrench.force.y = fy wrench.wrench.force.z = 0 wrench.wrench.torque.x = 0 wrench.wrench.torque.y = 0 wrench.wrench.torque.z = tau wrench.header.seq = 0 wrench.header.frame_id = '/base_link' wrench.header.stamp = rospy.Time.now() wrench_pub.publish(wrench)
def connect(self): try: print("Warning: Connecting to UR ip address: " + self.host_ip) self.socket_object.connect((self.host_ip, self.port)) if self.write_object is True: print("Warning: File Write Location: " + self.output_file_location) f = open(self.output_file_location + "DataStream.csv", "w") try: print("Publishing FT300 data, Press ctrl + c to stop") while not rospy.is_shutdown(): now = rospy.get_rostime() data = self.socket_object.recv(1024) msg = WrenchStamped() msg.header.stamp = now msg.wrench = conversions.to_wrench(list(eval(data))) rospy.logdebug(data) self.publisher.publish(msg) self.publisher_rate.sleep() except KeyboardInterrupt: f.close self.socket_object.close return False except Exception as e: print("Error: No Connection!! Please check your ethernet cable :)" + str(e)) return False
def dq_to_geometry_msgs_wrench_stamped(force, torque): ws = WrenchStamped() _add_header(ws) ws.wrench = dq_to_geometry_msgs_wrench(force, torque) return ws
#if (status == 14): # print msg #status = (status + 1) % 15 else: if (key == '\x03'): break if lastMove: wrench = Wrench() wrench.force.x = lastMove[0] * speed wrench.force.y = lastMove[1] * speed wrench.force.z = lastMove[2] * speed wrench.torque.x = lastMove[3] * turn wrench.torque.y = lastMove[4] * turn wrench.torque.z = lastMove[5] * turn ws = WrenchStamped() ws.header.stamp = rospy.Time.now() ws.header.frame_id = frame_id ws.wrench = wrench pub.publish(ws) except Exception as e: print e finally: ws = WrenchStamped() ws.header.stamp = rospy.Time.now() ws.header.frame_id = frame_id pub.publish(ws) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
def array_to_wrench_stamped(header, array): msg = WrenchStamped() msg.header = header msg.wrench = array_to_wrench(array) return msg