costmap functional in simulation

rosbuild/scout_navigation/navgoal.py

@@ -12,6 +12,8 @@ from std_msgs.msg import *
 from geometry_msgs.msg import *
 from nav_msgs.msg import *
 
+from common import *
+
 TOPIC_VREF = "navigator/param/vref"
 TOPIC_CMAP = "navigator/costmap"
 
@@ -165,7 +167,6 @@ class NavGoalFixed(NavGoalBase):
 
     def cmap_handler(self, msg):
         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]
@@ -173,6 +174,7 @@ class NavGoalFixed(NavGoalBase):
             if not self.g.planner.solve((x,y), smapf): 
                 rospy.logerr("cmap_handler(): re-planning failed!")
             print "REPLANNING took %f seconds"%(time.time()-t1)
+            #store_data(dict(smapf=smapf, planner=self.g.planner), "cmap_dump.data")
         else:
             rospy.logwarn("IGNORING costmap")