/navigator/replan service now is a NavReplan service that ignores request and...

/navigator/replan service now is a NavReplan service that ignores request and responds with a success bool

rosbuild_ws/scout_navigation/replanner.py

@@ -103,7 +103,8 @@ class Replanner:
         req.costmap.info.origin.orientation.z = 0
         req.costmap.info.origin.orientation.w = 1
         req.costmap.data = (COST_MULTIPLIER*igrid).flatten()
-        self.replan_srv(req)
+        ret = self.replan_srv(req)
+        return ret.success
 
     def replan_handler(self, req):
         rospy.loginfo("Received replanning request")
@@ -114,10 +115,10 @@ class Replanner:
         rospy.loginfo("Received %d laser scans in %f secs"%(self.scan_count,SAMPLING_TIME))
         #self.pub_costmap()
         #rospy.loginfo("Published costmap")
-        self.call_replan()
-        rospy.loginfo("Called replanning")
+        ret = self.call_replan()
+        rospy.loginfo("Called replanning; Success=%s"%(ret))
         self.mapping = False
-        return EmptyResponse()
+        return NavReplanResponse(ret)
 
     def main(self):
         rospy.init_node('navigation_replanner', argv=sys.argv)
@@ -132,7 +133,7 @@ class Replanner:
         self.lidar_sub = rospy.Subscriber('scan', LaserScan, self.laser_handler)
         self.cmap_pub  = rospy.Publisher('navigator/costmap', OccupancyGrid)
         self.replan_srv  = rospy.ServiceProxy('navigator/costmap', NavReplan)
-        rospy.Service("navigator/replan", Empty, self.replan_handler)
+        rospy.Service("navigator/replan", NavReplan, self.replan_handler)
         # Ready to serve
         rospy.loginfo("Replanning node ready")
         rospy.spin()