Costmap topic implemented (navigator/costmap), needs further testing

rosbuild/scout_navigation/navgoal.py

@@ -10,7 +10,7 @@ import tf
 
 from std_msgs.msg import *
 from geometry_msgs.msg import *
-
+from nav_msgs.msg import *
 
 TOPIC_VREF = "navigator/param/vref"
 TOPIC_CMAP = "navigator/costmap"
@@ -66,7 +66,7 @@ class NavGoalBase:
 class NavGoalFixed(NavGoalBase):
     progress = None
     monitor = None
-    vref = cmap = None
+    vref = None
     sub_vref = sub_cmap = None
 
     ## interface methods
@@ -110,6 +110,7 @@ class NavGoalFixed(NavGoalBase):
         return self.vref
 
     def disable(self):
+        #print "disable() WAS CALLED"
         if self.sub_vref is not None:
             self.sub_vref.unregister()
         if self.sub_cmap is not None:
@@ -163,7 +164,44 @@ class NavGoalFixed(NavGoalBase):
         self.vref = msg.data
 
     def cmap_handler(self, msg):
-        self.cmap = (msg.info, msg.data)
+        smapf = self.cmap2smapf(msg.info, msg.data)
+        np.savetxt("smapf", smapf)
+        if smapf is not None:
+            # replan with new speed map factor
+            (x,y) = self.goal_pose[0:2]
+            t1 = time.time()
+            if not self.g.planner.solve((x,y), smapf): 
+                rospy.logerr("cmap_handler(): re-planning failed!")
+            print "REPLANNING took %f seconds"%(time.time()-t1)
+        else:
+            rospy.logwarn("IGNORING costmap")
+
+    def cmap2smapf(self, meta, data):
+        """meta is MapMetaData and data is int8[] -- continuous map: [0,100] linearly to [free,occupied], otherwise constant"""
+        def compute_overlap(offset, la, lb):
+            """offset is of B origin w.r.t. A origin; la and lb are lengths of each segment; returns (a1,a2,b1,b2) such that A[a1:a2]=B[b1:b2] works"""
+            a1 = max(0,  offset)
+            b1 = max(0, -offset)
+            a2 = min(la, offset+lb)
+            b2 = min(lb, la-offset)
+            return (a1, a2, b1, b2)
+        assert abs(meta.resolution-self.g.planner.scale)<1e-3, "Costmap scale (%f) MUST match navmap one (%f)"%(meta.resolution,self.g.planner.scale)
+        # compute cropping window
+        joff = int(round( (meta.origin.position.x - self.g.planner.x0) / self.g.planner.scale ))
+        ioff = int(round( self.g.planner.H - meta.height - (meta.origin.position.y - self.g.planner.y0) / self.g.planner.scale ))
+        (ja1,ja2,jb1,jb2) = compute_overlap(joff, self.g.planner.W, meta.width)
+        (ia1,ia2,ib1,ib2) = compute_overlap(ioff, self.g.planner.H, meta.height)
+        print "ioff=%d joff=%d (ia1,ia2,ib1,ib2)=%s (ja1,ja2,jb1,jb2)=%s"%(ioff,joff,(ia1,ia2,ib1,ib2),(ja1,ja2,jb1,jb2))
+        if ia2>ia1 and ja2>ja1:
+            # make speed map factor
+            smapf = np.ones_like(self.g.planner.speedmap)
+            # NOTE: OccupancyGrid is flipped upside down
+            cmap  = np.flipud(np.reshape(data, (meta.height, meta.width)))
+            # NOTE: cells are: -1=unknown, 0=free, 100=lethal
+            smapf[ia1:ia2,ja1:ja2] = np.clip(1.0 - cmap[ib1:ib2, jb1:jb2]/100.0, 0, 1)
+            return smapf
+        else:
+            return None
 
     ## setup methods
 
@@ -174,20 +212,11 @@ class NavGoalFixed(NavGoalBase):
         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
+        self.sub_cmap = rospy.Subscriber(TOPIC_CMAP, OccupancyGrid, self.cmap_handler)
 
     def set_fixed_goal_pose(self, x, y, t):
         t1 = time.time()
-        # convert costmap into speedmap factor
-        # ESTOU AQUI
-        smapf = None
-
-        if self.g.planner.solve((x,y), smapf):
+        if self.g.planner.solve((x,y)):
             print "Solver took %f seconds"%(time.time()-t1)
             self.goal_pose = (x, y, t)
             self.goal_heading = t

rosbuild/scout_navigation/planner.py

@@ -148,7 +148,7 @@ class FastMarching(Map):
         return True
 
     def solve_zone(self, zone, speedmap_factor=None):
-        """goal is a boolean array"""
+        """like solve() but 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))