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
