unit4_pp/scripts/unit4_exercise_server.py
def make_plan(req):
  ''' 
  Callback function used by the service server to process requests.
  It returns a PathPlanningPluginResponse
  '''
  # Convert costmap from a 1-D tuple flat map representation
  map = list(req.costmap_ros)
  # Change values on the map from unknown to free space
  map[map==255] = 1
  width = req.width
  height = req.height
  start_index = req.start
  goal_index = req.goal
  # side of each grid map square in meters
  map_resolution = 0.1
  # origin of grid map (bottom left pixel) w.r.t. world coordinates (Rviz's origin)
  map_origin = [-20.0, -20.0]

  initial_position = indexToGridCell(start_index, width)
  target_position = indexToGridCell(goal_index, width)
  viz = TreeViz(map_resolution, map_origin, id=1, frame="map")

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

  # Calculate a path using RRT
  path = rrt(initial_position, target_position, width, height, map, map_resolution, map_origin, viz)

  if not path:
    rospy.logwarn("No path returned by RRT")
    return_path = []
  else:
    # print time statistics
    execution_time = rospy.Time.now() - start_time
    print("\n")
    rospy.loginfo('+++++++++++ RRT execution metrics ++++++++++')
    rospy.loginfo('Total execution time: %s seconds', str(execution_time.to_sec()))
    rospy.loginfo('++++++++++++++++++++++++++++++++++++++++++++')
    print("\n")
    rospy.loginfo('RRT: Path sent to navigation stack')

    return_path = []
    for cell in path:
        return_path.append(cell[0]+width*cell[1])

  resp = PathPlanningPluginResponse()
  resp.plan = return_path
  
  return resp
 
  Callback function used by the service server to process requests.
  It returns a PathPlanningPluginResponse
  
Callback function used by the service server to process requests.
It returns a PathPlanningPluginResponse
Callback function used by the service server to process requests . It returns a PathPlanningPluginResponse
def make_plan(req):
  map = list(req.costmap_ros)
  map[map==255] = 1
  width = req.width
  height = req.height
  start_index = req.start
  goal_index = req.goal
  map_resolution = 0.1
  map_origin = [-20.0, -20.0]
  initial_position = indexToGridCell(start_index, width)
  target_position = indexToGridCell(goal_index, width)
  viz = TreeViz(map_resolution, map_origin, id=1, frame="map")
  start_time = rospy.Time.now()
  path = rrt(initial_position, target_position, width, height, map, map_resolution, map_origin, viz)
  if not path:
    rospy.logwarn("No path returned by RRT")
    return_path = []
  else:
    execution_time = rospy.Time.now() - start_time
    print("\n")
    rospy.loginfo('+++++++++++ RRT execution metrics ++++++++++')
    rospy.loginfo('Total execution time: %s seconds', str(execution_time.to_sec()))
    rospy.loginfo('++++++++++++++++++++++++++++++++++++++++++++')
    print("\n")
    rospy.loginfo('RRT: Path sent to navigation stack')
    return_path = []
    for cell in path:
        return_path.append(cell[0]+width*cell[1])
  resp = PathPlanningPluginResponse()
  resp.plan = return_path
  return resp
def make_plan ( req ) : map = list ( req . costmap_ros ) map [ map == 255 ] = 1 width = req . width height = req . height start_index = req . start goal_index = req . goal map_resolution = 0.1 map_origin = [ - 20.0 , - 20.0 ] initial_position = indexToGridCell ( start_index , width ) target_position = indexToGridCell ( goal_index , width ) viz = TreeViz ( map_resolution , map_origin , id = 1 , frame = "map" ) start_time = rospy . Time . now ( ) path = rrt ( initial_position , target_position , width , height , map , map_resolution , map_origin , viz ) if not path : rospy . logwarn ( "No path returned by RRT" ) return_path = [ ] else : execution_time = rospy . Time . now ( ) - start_time print ( "\n" ) rospy . loginfo ( '+++++++++++ RRT execution metrics ++++++++++' ) rospy . loginfo ( 'Total execution time: %s seconds' , str ( execution_time . to_sec ( ) ) ) rospy . loginfo ( '++++++++++++++++++++++++++++++++++++++++++++' ) print ( "\n" ) rospy . loginfo ( 'RRT: Path sent to navigation stack' ) return_path = [ ] for cell in path : return_path . append ( cell [ 0 ] + width * cell [ 1 ] ) resp = PathPlanningPluginResponse ( ) resp . plan = return_path return resp
Callback function used by the service server to process requests.
Callback function used by the service server to process requests .
''' 
  Callback function used by the service server to process requests.
  It returns a PathPlanningPluginResponse
  ''' # Convert costmap from a 1-D tuple flat map representation # Change values on the map from unknown to free space # side of each grid map square in meters # origin of grid map (bottom left pixel) w.r.t. world coordinates (Rviz's origin) # time statistics # Calculate a path using RRT # print time statistics
req