def init_viz(self): self.as_poly = PolygonMarker('autosequence', (), color=Colors.BLUE+Alpha(0.5), width=0.02, closed=False) self.as_hover_point_sphere = SphereMarker('hover_point', (0,0,0), radius=0.02, color=Colors.BLUE+Alpha(0.5)) self.mg = MarkerGroup(VizModuleBase.mapub) self.mg.add(self.as_poly, self.as_hover_point_sphere) self.init_int_marker()
class AutosequencesVizModule(VizModuleBase): """ Visualization module for control_mode_autosequence. Provides: - polygon representing autosequence path - markers for autosequence points - labels - heading indicators at each point """ got_autosequence = False current_autosequence = None latest_autoseq_info = None latest_hover_info = None loaded_as_name = "" def __init__(self): super(AutosequencesVizModule, self).__init__(id="autoseq") def init_viz(self): self.as_poly = PolygonMarker('autosequence', (), color=Colors.BLUE+Alpha(0.5), width=0.02, closed=False) self.as_hover_point_sphere = SphereMarker('hover_point', (0,0,0), radius=0.02, color=Colors.BLUE+Alpha(0.5)) self.mg = MarkerGroup(VizModuleBase.mapub) self.mg.add(self.as_poly, self.as_hover_point_sphere) def init_vars(self): pass def init_subscribers(self): prefix = self.subscriber_topic_prefix #self.llstatus_sub = rospy.Subscriber(prefix+'autopilot/LL_STATUS', LLStatus, self.llstatus_callback) # AscTec specific.. how to make more generic? self.autoseq_info_sub = rospy.Subscriber(prefix+'control_mode_autosequence/autosequence_info', control_mode_autosequence_info, self.autoseq_info_callback) self.hover_info_sub = rospy.Subscriber(prefix+'control_mode_autosequence/info', control_mode_hover_info, self.hover_info_callback) self.controller_status_sub = rospy.Subscriber(prefix+'controller/status', controller_status, self.controller_status_callback) def publish_timer_callback(self, event): if self.got_autosequence: as_points = [(p.hover_point.x, p.hover_point.y, 0) for p in self.current_autosequence.points] self.as_poly.set(points=as_points) if self.latest_hover_info is not None: slhi = self.latest_hover_info self.as_hover_point_sphere.set(origin=(slhi.north_cmd, slhi.east_cmd, 0)) now = rospy.Time.now() VizModuleBase.publish(self,now) def autoseq_info_callback(self, msg): #rospy.loginfo('got autoseq info: %s' % str(msg)) self.latest_autoseq_info = msg def hover_info_callback(self, msg): self.latest_hover_info = msg def _new_autosequence(self): return (self.latest_autoseq_info is not None and len(self.latest_autoseq_info.defined_autosequences) > 0 and (self.latest_autoseq_info.current_autosequence != self.loaded_as_name)) def controller_status_callback(self, msg): self.latest_controller_status = msg #rospy.loginfo('got controller status: %s' % str(msg)) if msg.active_mode == 'autosequence': if self._new_autosequence(): self.got_autosequence = False if (not self.got_autosequence and self.latest_autoseq_info is not None and len(self.latest_autoseq_info.defined_autosequences) > 0 and len(self.latest_autoseq_info.current_autosequence) > 0): as_name = self.latest_autoseq_info.current_autosequence self.get_autosequence(as_name) def get_autosequence(self, as_name): get_auto_proxy = rospy.ServiceProxy('control_mode_autosequence/get_autosequence', GetAutosequence) rospy.loginfo("Asking for autosequence '%s'..." % as_name) resp = get_auto_proxy(autosequence_name=as_name) if resp.found: self.current_autosequence = resp.autosequence rospy.loginfo('Got autosequence: %s' % str(self.current_autosequence)) self.got_autosequence = True self.loaded_as_name = as_name else: rospy.logerr("Service call failed: autosequence '%s' not found" % as_name)
class AutosequencesVizModule(VizModuleBase): """ Visualization module for control_mode_autosequence. Provides: - polygon representing autosequence path - markers for autosequence points - labels - heading indicators at each point """ got_autosequence = False current_autosequence = None latest_autoseq_info = None latest_hover_info = None loaded_as_name = "" def __init__(self): super(AutosequencesVizModule, self).__init__(id="autoseq") def init_viz(self): self.as_poly = PolygonMarker('autosequence', (), color=Colors.BLUE+Alpha(0.5), width=0.02, closed=False) self.as_hover_point_sphere = SphereMarker('hover_point', (0,0,0), radius=0.02, color=Colors.BLUE+Alpha(0.5)) self.mg = MarkerGroup(VizModuleBase.mapub) self.mg.add(self.as_poly, self.as_hover_point_sphere) self.init_int_marker() def init_vars(self): pass def init_subscribers(self): prefix = self.subscriber_topic_prefix #self.llstatus_sub = rospy.Subscriber(prefix+'autopilot/LL_STATUS', LLStatus, self.llstatus_callback) # AscTec specific.. how to make more generic? self.autoseq_info_sub = rospy.Subscriber(prefix+'control_mode_autosequence/autosequence_info', control_mode_autosequence_info, self.autoseq_info_callback) self.hover_info_sub = rospy.Subscriber(prefix+'control_mode_autosequence/info', control_mode_hover_info, self.hover_info_callback) self.controller_status_sub = rospy.Subscriber(prefix+'controller/status', controller_status, self.controller_status_callback) def init_int_marker(self): self.ims = InteractiveMarkerServer("simple_marker") self.im = InteractiveMarker() self.im.header.frame_id = "/ned" self.im.name = "my_marker" self.im.description = "Simple 1-DOF control" bm = Marker() bm.type = Marker.CUBE bm.scale.x = bm.scale.y = bm.scale.z = 0.45 bm.color.r = 0.0 bm.color.g = 0.5 bm.color.b = 0.5 bm.color.a = 1.0 bc = InteractiveMarkerControl() bc.always_visible = True bc.markers.append(bm) self.im.controls.append(bc) rc = InteractiveMarkerControl() rc.name = "move_x" rc.interaction_mode = InteractiveMarkerControl.MOVE_AXIS self.im.controls.append(rc) self.ims.insert(self.im, self.process_marker_feedback) self.ims.applyChanges() def process_marker_feedback(self, feedback): p = feedback.pose.position print feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y) + ", " + str(p.z) def publish_timer_callback(self, event): if self.got_autosequence: as_points = [(p.hover_point.x, p.hover_point.y, 0) for p in self.current_autosequence.points] self.as_poly.set(points=as_points) if self.latest_hover_info is not None: slhi = self.latest_hover_info self.as_hover_point_sphere.set(origin=(slhi.north_cmd, slhi.east_cmd, 0)) now = rospy.Time.now() VizModuleBase.publish(self,now) def autoseq_info_callback(self, msg): #rospy.loginfo('got autoseq info: %s' % str(msg)) self.latest_autoseq_info = msg def hover_info_callback(self, msg): self.latest_hover_info = msg def _new_autosequence(self): return (self.latest_autoseq_info is not None and len(self.latest_autoseq_info.defined_autosequences) > 0 and (self.latest_autoseq_info.current_autosequence != self.loaded_as_name)) def controller_status_callback(self, msg): self.latest_controller_status = msg #rospy.loginfo('got controller status: %s' % str(msg)) if msg.active_mode == 'autosequence': if self._new_autosequence(): self.got_autosequence = False if (not self.got_autosequence and self.latest_autoseq_info is not None and len(self.latest_autoseq_info.defined_autosequences) > 0 and len(self.latest_autoseq_info.current_autosequence) > 0): as_name = self.latest_autoseq_info.current_autosequence self.get_autosequence(as_name) def get_autosequence(self, as_name): get_auto_proxy = rospy.ServiceProxy('control_mode_autosequence/get_autosequence', GetAutosequence) rospy.loginfo("Asking for autosequence '%s'..." % as_name) resp = get_auto_proxy(autosequence_name=as_name) if resp.found: self.current_autosequence = resp.autosequence rospy.loginfo('Got autosequence: %s' % str(self.current_autosequence)) self.got_autosequence = True self.loaded_as_name = as_name else: rospy.logerr("Service call failed: autosequence '%s' not found" % as_name)