dynamic vref supported by navigation_by_target

rosbuild/scout_navigation/navgoal.py

@@ -247,6 +247,8 @@ class NavGoalDynamic(NavGoalBase):
     mode = None
     cached_goal = None
     last_solve = None
+    vref = None
+    sub_vref = None
 
     # -- WIP --
 
@@ -391,8 +393,24 @@ class NavGoalDynamic(NavGoalBase):
             else:
                 raise Exception, "unknown mode %s"%(self.mode)
 
+    def reference_speed(self):
+        return self.vref
+
+    def disable(self):
+        if self.sub_vref is not None:
+            self.sub_vref.unregister()
+
+    def vref_handler(self, msg):
+        self.vref = msg.data
+
     ## setup methods
 
+    def setup_topics(self):
+        """setup external parameterization toipic handlers"""
+        if self.sub_vref is not None:
+            self.sub_vref.unregister()
+        self.sub_vref = rospy.Subscriber(TOPIC_VREF, Float64, self.vref_handler)
+
     def set_dynamic_goal(self, target_frame, heading_frame=None, proximity=0, follow_target=False):
         """follow frame target_frame with a proximity distance (body) heading to target_frame; success upon reaching if follow_target True"""
         self.target_frame = target_frame
@@ -412,6 +430,7 @@ class NavGoalDynamic(NavGoalBase):
                 rospy.logerr("set_dynamic_goal(): heading transform '%s' timed out"%(target_frame))
                 return False
         self.mode = 'near'
+        self.setup_topics()
         return True