unit5_pp/scripts/unit5_pp_server.py
def make_plan(req):
  ''' 
  Callback function used by the service server to process
  requests from clients. It returns a msg of type PathPlanningPluginResponse
  '''
  global got_potential_field_update
  # costmap as 1-D array representation
  costmap = req.costmap_ros
  # number of columns in the occupancy grid
  width = req.width
  # number of rows in the occupancy grid
  height = req.height
  start_index = req.start
  goal_index = req.goal

  # side of each grid map square in meters
  resolution = 0.2
  # origin of grid map
  origin = [-7.4, -7.4, 0]

  # wait for total potential field update
  got_potential_field_update = False
  while not got_potential_field_update:
    pass

  viz = DescentViz(start_index, width, resolution, origin, id=1, frame="map")

  # time statistics
  start_time = rospy.Time.now()

  # calculate path using gradient descent
  path = gradient_descent(start_index, goal_index, width, height, total_potential_field, viz)

  if not path:
    rospy.logwarn("Path server: No path returned by gradient descent")
    path = []
    # cancel move_base's navigation goal
    cancel_goal_pub.publish(GoalID())
  else:
    execution_time = rospy.Time.now() - start_time
    print("\n")
    rospy.loginfo('+++++++++++ Gradient Descent execution metrics ++++++++++')
    rospy.loginfo('Total execution time: %s seconds', str(execution_time.to_sec()))
    rospy.loginfo('++++++++++++++++++++++++++++++++++++++++++++')
    print("\n")
    rospy.loginfo('Path server: Path sent to navigation stack')

  resp = PathPlanningPluginResponse()
  resp.plan = path
  return resp
 
  Callback function used by the service server to process
  requests from clients. It returns a msg of type PathPlanningPluginResponse
  
Callback function used by the service server to process
requests from clients. It returns a msg of type PathPlanningPluginResponse
Callback function used by the service server to process requests from clients . It returns a msg of type PathPlanningPluginResponse
def make_plan(req):
  global got_potential_field_update
  costmap = req.costmap_ros
  width = req.width
  height = req.height
  start_index = req.start
  goal_index = req.goal
  resolution = 0.2
  origin = [-7.4, -7.4, 0]
  got_potential_field_update = False
  while not got_potential_field_update:
    pass
  viz = DescentViz(start_index, width, resolution, origin, id=1, frame="map")
  start_time = rospy.Time.now()
  path = gradient_descent(start_index, goal_index, width, height, total_potential_field, viz)
  if not path:
    rospy.logwarn("Path server: No path returned by gradient descent")
    path = []
    cancel_goal_pub.publish(GoalID())
  else:
    execution_time = rospy.Time.now() - start_time
    print("\n")
    rospy.loginfo('+++++++++++ Gradient Descent execution metrics ++++++++++')
    rospy.loginfo('Total execution time: %s seconds', str(execution_time.to_sec()))
    rospy.loginfo('++++++++++++++++++++++++++++++++++++++++++++')
    print("\n")
    rospy.loginfo('Path server: Path sent to navigation stack')
  resp = PathPlanningPluginResponse()
  resp.plan = path
  return resp
def make_plan ( req ) : global got_potential_field_update costmap = req . costmap_ros width = req . width height = req . height start_index = req . start goal_index = req . goal resolution = 0.2 origin = [ - 7.4 , - 7.4 , 0 ] got_potential_field_update = False while not got_potential_field_update : pass viz = DescentViz ( start_index , width , resolution , origin , id = 1 , frame = "map" ) start_time = rospy . Time . now ( ) path = gradient_descent ( start_index , goal_index , width , height , total_potential_field , viz ) if not path : rospy . logwarn ( "Path server: No path returned by gradient descent" ) path = [ ] cancel_goal_pub . publish ( GoalID ( ) ) else : execution_time = rospy . Time . now ( ) - start_time print ( "\n" ) rospy . loginfo ( '+++++++++++ Gradient Descent execution metrics ++++++++++' ) rospy . loginfo ( 'Total execution time: %s seconds' , str ( execution_time . to_sec ( ) ) ) rospy . loginfo ( '++++++++++++++++++++++++++++++++++++++++++++' ) print ( "\n" ) rospy . loginfo ( 'Path server: Path sent to navigation stack' ) resp = PathPlanningPluginResponse ( ) resp . plan = path return resp
Callback function used by the service server to process
requests from clients.
Callback function used by the service server to process requests from clients .
''' 
  Callback function used by the service server to process
  requests from clients. It returns a msg of type PathPlanningPluginResponse
  ''' # costmap as 1-D array representation # number of columns in the occupancy grid # number of rows in the occupancy grid # side of each grid map square in meters # origin of grid map # wait for total potential field update # time statistics # calculate path using gradient descent # cancel move_base's navigation goal
req