work in progress -- costmap

rosbuild/scout_navigation/navgoal.py

@@ -13,7 +13,7 @@ from geometry_msgs.msg import *
 
 
 TOPIC_VREF = "navigator/param/vref"
-
+TOPIC_CMAP = "navigator/costmap"
 
 class NavGoalBase:
 
@@ -66,8 +66,8 @@ class NavGoalBase:
 class NavGoalFixed(NavGoalBase):
     progress = None
     monitor = None
-    vref = None
-    sub_vref = None
+    vref = cmap = None
+    sub_vref = sub_cmap = None
 
     ## interface methods
 
@@ -112,6 +112,8 @@ class NavGoalFixed(NavGoalBase):
     def disable(self):
         if self.sub_vref is not None:
             self.sub_vref.unregister()
+        if self.sub_cmap is not None:
+            self.sub_cmap.unregister()
         self.g.unregister_monitor('navgoal')
 
     ## execution monitoring
@@ -160,6 +162,9 @@ class NavGoalFixed(NavGoalBase):
     def vref_handler(self, msg):
         self.vref = msg.data
 
+    def cmap_handler(self, msg):
+        self.cmap = (msg.info, msg.data)
+
     ## setup methods
 
     def setup_topics(self):
@@ -167,10 +172,22 @@ class NavGoalFixed(NavGoalBase):
         if self.sub_vref is not None:
             self.sub_vref.unregister()
         self.sub_vref = rospy.Subscriber(TOPIC_VREF, Float64, self.vref_handler)
+        if self.sub_cmap is not None:
+            self.sub_cmap.unregister()
+        self.sub_cmap = rospy.Subscriber(TOPIC_CMAP, Float64, self.cmap_handler)
+
+    def set_costmap(self, meta, data):
+        """meta is MapMetaData; data is int8[] where 100 is no cost"""
+        self.cmap = (meta, data)
+        # ESTOU AQUI
 
     def set_fixed_goal_pose(self, x, y, t):
         t1 = time.time()
-        if self.g.planner.solve((x, y)):
+        # convert costmap into speedmap factor
+        # ESTOU AQUI
+        smapf = None
+
+        if self.g.planner.solve((x,y), smapf):
             print "Solver took %f seconds"%(time.time()-t1)
             self.goal_pose = (x, y, t)
             self.goal_heading = t

rosbuild/scout_navigation/planner.py

@@ -117,8 +117,8 @@ class FastMarching(Map):
         near = self.speedmap<maxd
         self.speedmap[near] = -self.distmap[near]**2/maxd + 2*self.distmap[near]
         
-    def solve(self, goal):
-        """goal is (x,y) world coordinate"""
+    def solve(self, goal, speedmap_factor=None):
+        """goal is (x,y) world coordinate; speedmap_factor is array to be multiplied by speedmap"""
         (i, j) = self.p2i(goal)
         if not self.free[i, j]:
             return False
@@ -128,6 +128,8 @@ class FastMarching(Map):
         speed = np.zeros(self.occgrid.shape)
         t1 = time.time()
         speed[self.free] = self.speedmap[self.free]
+        if speedmap_factor is not None:
+            speed *= speedmap_factor
         t2 = time.time()
         t1 = t2-t1
         self.field = skfmm.travel_time(phi, speed)
@@ -145,13 +147,15 @@ class FastMarching(Map):
         print "[speedmap: %fms, FMM: %fms, interp: %fms, distmap: %fms]"%tuple([1000*t for t in [t1,t2,t3,t5]])
         return True
 
-    def solve_zone(self, zone):
+    def solve_zone(self, zone, speedmap_factor=None):
         """goal is a boolean array"""
         phi = np.ones(self.occgrid.shape)
         phi[zone] = -1
         phi = np.ma.masked_array(phi, np.logical_not(self.free))
         speed = np.zeros(self.occgrid.shape)
         speed[self.free] = self.speedmap[self.free]
+        if speedmap_factor is not None:
+            speed *= speedmap_factor
         self.field = skfmm.travel_time(phi, speed)
         # DISABLED: self.make_interpolator()
         self.field_isospeed = skfmm.distance(phi)