def display_feature(feature, frame, frame_id, id=1): m = marker.create(type=marker.Marker.CYLINDER, id=id) p = frame * feature.position d = (frame * (feature.position + feature.direction)) - p s = feature.size marker.align(m, p-d*s, p+d*s, 0.01) m.header.frame_id = frame_id m.color = marker.ColorRGBA(0.0, 1.0, 0.0, 1.0) marker.publish(m)
def debug_kinematics(self):
frame = kdl.Frame()
for i in range(10):
if i == 9:
i = -1
self.kin_fwd.JntToCart(self.joints, frame, i)
mrk = marker.create(type=marker.Marker.CUBE,
scale=Vector3(0.1, 0.1, 0.1),
color=ColorRGBA(1, i*0.12, 0, 1),
ns='jnts', id=i)
mrk.header.frame_id = self.base_frame_id
mrk.pose = pm.toMsg(frame)
marker.publish(mrk)
def show(self):
jac = self.robot.jac()
J = numpy.matrix([[jac[i,j] for j in range(jac.columns())]
for i in range(jac.rows())])
F = self.robot.pose()
(p,q,s,k) = get_ellipsoid(F,J)
c = marker.color_code(k)
m = marker.create(type=marker.Marker.SPHERE, ns='ellipsoid', id=42)
m.header.frame_id = self.base_frame_id
m.header.stamp = rospy.Time.now()
m.pose.position = Point(x=p[0], y=p[1], z=p[2])
m.pose.orientation = Quaternion(x=q[1], y=q[2], z=q[3], w=q[0])
m.color = ColorRGBA(r=c[0], g=c[1], b=c[2], a=k)
m.scale = Vector3(x=s[0]*self.scale, y=s[1]*self.scale, z=s[2]*self.scale)
marker.publish(m)
def callback(msg):
constraint_display.set_constraints(msg.constraints)
for m in constraint_display.show():
marker.publish(m)
def callback(msg):
constraint_display.set_constraints(msg.constraints)
for m in constraint_display.show():
marker.publish(m)
if __name__ == "__main__":
rospy.init_node('feature_vis')
# base_frame is an arbitrary frame in which the markers are displayed
# the marker locations are defined by the feature frame_id's!
base_frame_id = rospy.get_param('~base_frame', 'odom')
rospy.loginfo('base frame: ' + base_frame_id)
_config_['frame_id'] = base_frame_id
constraint_display = ConstraintDisplay(base_frame_id)
sub = rospy.Subscriber('/constraint_config', ConstraintConfig, callback)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
constraint_display.transform()
_config_['marker_id'] = 0
for m in constraint_display.show():
marker.publish(m)
rate.sleep()
file_name = sys.argv[1] f = open(file_name) s = 0.015 radius = 0.13 alpha = 0.02 mrk = marker.create(type=marker.Marker.POINTS) mrk.header.frame_id = '/cylinder_pose' mrk.ns = 'discontinuities' mrk.color = ColorRGBA(0,0,0,alpha) mrk.scale = Vector3(s, s, s) mrk.frame_locked = True for line in f: data = map(float, line.split()) ori = kdl.Rotation.Quaternion(*data[0:4]) amplitude = data[4] mrk.points.append(Point(*(ori.UnitX()*radius))) mrk.points.append(Point(*(ori.UnitY()*radius))) mrk.points.append(Point(*(ori.UnitZ()*radius))) mrk.colors.append(ColorRGBA(1,0,0,alpha)) mrk.colors.append(ColorRGBA(0,1,0,alpha)) mrk.colors.append(ColorRGBA(0,0,1,alpha)) f.close() marker.publish(mrk)
