Commit 138aded3 authored by Rodrigo Ventura's avatar Rodrigo Ventura

/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
parent 970f1c1a
...@@ -103,7 +103,8 @@ class Replanner: ...@@ -103,7 +103,8 @@ class Replanner:
req.costmap.info.origin.orientation.z = 0 req.costmap.info.origin.orientation.z = 0
req.costmap.info.origin.orientation.w = 1 req.costmap.info.origin.orientation.w = 1
req.costmap.data = (COST_MULTIPLIER*igrid).flatten() req.costmap.data = (COST_MULTIPLIER*igrid).flatten()
self.replan_srv(req) ret = self.replan_srv(req)
return ret.success
def replan_handler(self, req): def replan_handler(self, req):
rospy.loginfo("Received replanning request") rospy.loginfo("Received replanning request")
...@@ -114,10 +115,10 @@ class Replanner: ...@@ -114,10 +115,10 @@ class Replanner:
rospy.loginfo("Received %d laser scans in %f secs"%(self.scan_count,SAMPLING_TIME)) rospy.loginfo("Received %d laser scans in %f secs"%(self.scan_count,SAMPLING_TIME))
#self.pub_costmap() #self.pub_costmap()
#rospy.loginfo("Published costmap") #rospy.loginfo("Published costmap")
self.call_replan() ret = self.call_replan()
rospy.loginfo("Called replanning") rospy.loginfo("Called replanning; Success=%s"%(ret))
self.mapping = False self.mapping = False
return EmptyResponse() return NavReplanResponse(ret)
def main(self): def main(self):
rospy.init_node('navigation_replanner', argv=sys.argv) rospy.init_node('navigation_replanner', argv=sys.argv)
...@@ -132,7 +133,7 @@ class Replanner: ...@@ -132,7 +133,7 @@ class Replanner:
self.lidar_sub = rospy.Subscriber('scan', LaserScan, self.laser_handler) self.lidar_sub = rospy.Subscriber('scan', LaserScan, self.laser_handler)
self.cmap_pub = rospy.Publisher('navigator/costmap', OccupancyGrid) self.cmap_pub = rospy.Publisher('navigator/costmap', OccupancyGrid)
self.replan_srv = rospy.ServiceProxy('navigator/costmap', NavReplan) 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 # Ready to serve
rospy.loginfo("Replanning node ready") rospy.loginfo("Replanning node ready")
rospy.spin() rospy.spin()
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment