Go to the documentation of this file.00001
00002
00003 import argparse
00004
00005 import roslib; roslib.load_manifest('hrl_ft')
00006 import rospy
00007
00008 from geometry_msgs.msg import WrenchStamped, PoseStamped, Point, PointStamped
00009 from visualization_msgs.msg import Marker
00010
00011 class WrenchToMarker(object):
00012 def __init__(self, scale, frame):
00013 self.scale = scale
00014 self.frame = frame
00015 self.ws_sub = rospy.Subscriber('wrench_in', WrenchStamped,
00016 self.wrench_cb)
00017 self.marker_pub = rospy.Publisher('wrench_marker', Marker)
00018
00019 def wrench_cb(self, ws):
00020 marker = self.form_marker(ws)
00021 self.marker_pub.publish(marker)
00022
00023 def form_marker(self, ws):
00024 force_vec = Marker()
00025 force_vec.header.stamp = rospy.Time.now()
00026 force_vec.header.frame_id = self.frame
00027 force_vec.ns = "ft_sensor"
00028 force_vec.action = 0
00029 force_vec.type = 0
00030 force_vec.scale.x = 0.1
00031 force_vec.scale.y = 0.2
00032 force_vec.scale.z = 0.1
00033 force_vec.color.a = 0.75
00034 force_vec.color.r = 0.0
00035 force_vec.color.g = 1.0
00036 force_vec.color.b = 0.1
00037 force_vec.lifetime = rospy.Duration(1)
00038
00039 origin = Point()
00040 force_point = Point()
00041 force_point.x = self.scale*ws.wrench.force.x
00042 force_point.y = self.scale*ws.wrench.force.y
00043 force_point.z = self.scale*ws.wrench.force.z
00044 force_vec.points.append(origin)
00045 force_vec.points.append(force_point)
00046 return force_vec
00047
00048 if __name__=='__main__':
00049 parser = argparse.ArgumentParser(description="Apply a transform")
00050 parser.add_argument('-s','--scale', default=0.1, type=float,
00051 help='The scaling applied to the vector magnitude.'+
00052 'Length in meters per Newton of force. (m/N)')
00053 parser.add_argument('-f','--frame', default='/l_force_torque_link',
00054 help='The TF frame of the incoming wrench.')
00055 args = parser.parse_known_args()
00056
00057 rospy.init_node('wrench_viz_marker')
00058 wrench_to_marker = WrenchToMarker(scale=args[0].scale, frame=args[0].frame)
00059 rospy.spin()