def update(self): '''update the marker to reflect the current joint angle''' p1 = self.pose_base.p p2 = self.pose_base * (self.axis*self.chi) marker.align(self.marker, p1, p2, config.axisWidth) self.marker.color = color_code(abs(self.chi - self.chi_desired) / config.error_length)
def axis_marker(tw, id = 0, ns = 'twist'): """ make a marker message showing the instantaneous rotation axis of a twist message""" t = kdl.Twist(kdl.Vector(tw.linear.x, tw.linear.y, tw.linear.z), kdl.Vector(tw.angular.x, tw.angular.y, tw.angular.z)) try: (x, rot) = listener.lookupTransform(target_frame, ref_frame, rospy.Time(0)) (trans, rotp) = listener.lookupTransform(target_frame, ref_point, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException): print 'tf exception!' return marker.create(id=id, ns=ns, action=Marker.DELETE) # put the twist in the right frame f = posemath.fromTf( (trans, rot) ) t = f*t direction = t.rot location = t.rot * t.vel / kdl.dot(t.rot, t.rot) lr = t.rot.Norm() lt = t.vel.Norm() m = marker.create(id=id, ns=ns, type=Marker.CYLINDER) if lr < 0.0001 and lt < 0.0001: return marker.create(id=id, ns=ns, action=Marker.DELETE) if lr < 0.001: # very short axis, assume linear movement location = kdl.Vector(0,0,0) direction = t.vel if lt < min_length: direction *= min_length / lt m.type = Marker.CUBE m = marker.align(m, location, location + direction, 0.02) elif lr < min_length: direction = direction / lr * min_length m = marker.align(m, location - direction, location + direction, 0.02) elif lr > max_length: direction = direction / lr * max_length m = marker.align(m, location - direction, location + direction, 0.02) else: #BAH! How do I make this better? m = marker.align(m, location - direction, location + direction, 0.02) m.header.frame_id = target_frame m.frame_locked = True if(use_colors): m.color = colors[id % len(colors)] else: m.color = ColorRGBA(0.3, 0.3, 0.3, 1) return m
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 update(self): '''update the marker to reflect the current joint angle''' p1 = self.pose_base * (-self.axis * config.axisLength) p2 = self.pose_base * ( self.axis * config.axisLength) marker.align(self.marker, p1, p2, config.axisWidth) self.marker.color = color_code(abs(self.chi - self.chi_desired) / config.error_angle) if self.weight == 0.0: self.marker.color = ColorRGBA(0.0, 0.0, 1.0, 1.0)
def cylinders(frm, to, diameter, ns=''): m1 = marker.create(id=21, ns=ns, type=Marker.CYLINDER) m1.color = ColorRGBA(1, 0, 0, 0.7) m1 = marker.align(m1, frm, kdl.Vector(to[0], frm[1], frm[2]), diameter) m2 = marker.create(id=22, ns=ns, type=Marker.CYLINDER) m2.color = ColorRGBA(0, 1, 0, 0.7) m2 = marker.align(m2, frm, kdl.Vector(frm[0], to[1], frm[2]), diameter) m3 = marker.create(id=23, ns=ns, type=Marker.CYLINDER) m3.color = ColorRGBA(0, 0, 1, 0.7) m3 = marker.align(m3, frm, kdl.Vector(frm[0], frm[1], to[2]), diameter) return [m1, m2, m3]
def markers(self): ''' get the markers for visualization ''' self._compute_lengths() frame = self._transform() mrks = [] pos = kdl.Vector(0,0,0) l_index = 0 for i,t in enumerate(self.jac): twist = frame*t lr = twist.rot.Norm() lv = twist.vel.Norm() if lr < self.eps and lv < self.eps: # null twist, make a delete marker mrks.append(marker.create(id=i, ns=self.name+str(i), action=Marker.DELETE)) continue if lr < self.eps: # pure translational twist location = pos direction = twist.vel / lv * self.lengths[l_index] m = marker.create(id=i, ns=self.name+str(i), type=Marker.CUBE) m = marker.align(m, location, location + direction, self.width) l_index += 1 frame.p = frame.p - direction # move target point to end of this 'axis' pos += direction # remember current end point else: # rotational twist location = twist.rot * twist.vel / kdl.dot(twist.rot, twist.rot) + pos if self.independent_directions: direction = twist.rot * self.rot_length / lr else: # take axis from interaction matrix and transform to location dir_H = self.H[i].rot - location*self.H[i].vel direction = dir_H * self.rot_length / dir_H.Norm() m = marker.create(id=i, ns=self.name+str(i), type=Marker.CYLINDER) m = marker.align(m, location - direction, location + direction, self.width) m.color = self._color(i) m.header.frame_id = self.target_frame mrks.append(m) return mrks
def marker_line(start, end, **args): global _config_ m = marker_base(type=marker.Marker.CYLINDER, **args) marker.align(m, start, end, m.scale.z) return m