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)