Navigation Action Server - RoBorregos/robocup-home GitHub Wiki

Navigation Action Server

Implementation Design

Overview

The Navigation Action Server provides actions to achieve specific tasks for the competition; in this design, the Navigation Module is activated by the Main Engine, which sends a request for a specific action. Then, the action server executes the needed actions with the help of the move_base action server.

The services provided by the navigation action servers include three different actions: go_to, search_room, and approach_goal.

Action Servers

Action providers have these common features:

Valid states Valid Headers Valid feedback
PENDING
ACTIVE
SUCCEEDED
ABORTED		 gt-place
so-place
ao-pose
 from main_engine:
TODO

[WIP] go_to

Objective: Move the robot to a certain pre-mapped point in the house.

Functionality:

  1. Obtains a goal (in form of a string) from the main_engine and maps it to a pre-mapped dictionary of goals.
  2. If valid, it uses the MoveBaseAction to send this goal to the move_base module, create a path, and follow it.
  3. Waits for a predefined time (120 seconds) for the robot to reach or fail to reach the goal; changes state accordingly.
Feedback Valid goals Involved servers
  • string path_status: Gives an idea of how far the robot is positioned from the goal
  • double distance_from_goal
 kitchen
restroom
living_room
dinning_room
entrance bedroom
  • Move_base: States if the robot has reached a certain goal in the house.

State cases:

Case description State
Validating goal string given PENDING
Robot going to the goal ACTIVE
Goal reached SUCCEEDED
Goal not reached within specific time frame (120 seconds) ABORTED
Robot stuck: cancel action + call for recovery system ABORTED
Action canceled ABORTED

[WIP] search_room

Objective: go around the room for the robot to be able to identify an object of interest.

Functionality:

  1. Get instructions to search an area/room as a goal.
  2. We send the goal to the get_room_path module to generate an array of goal points.
  3. The path is validated: 1. All goals have to be inside the given room. 2. All goals have to be reachable.
  4. The array of goal points is sent to create a path and follow it using move_base node.
  5. The robot starts moving through the goal path.
  6. Note: The robot will rotate its neck to search for the specific object using the VisionActionServer.
  7. Depending on the feedback status of this server the robot will continue rotating or stopping if an object was found
  8. Once the room search is completed, the robot will send a successful status to the Main Engine and stop in its place.
  9. If not found, the robot will stop at the last goal.
Feedback Information needed Involved servers
  • string searching_status: states whether the object has been found, if the object is still being searched or if the robot is stuck/unable to move.
  • double percentage_room_traveled: states the percentage of the room traveled
  • Room/goal
  • An array of PoseStamp coordinates (from get_room_path module) representing a valid map path around objects of interest (tables, fridge, counter, cupboards) or around the room.
  • An interruption or message which states that the object has been found.
  • Robot position
    • Nav stack: move_base
      • States if the robot has reached a certain goal in the room.
    • Nav module: get_room_path
      • Gives the room path (list of goals) for the robot to follow and search
    • Nav module: recovery_system
      • When prompted it stops the robot's movement and makes it ask for help or direct it to a valid position.

    State cases:

    Case description State
    Validating path given PENDING
    Robot looking for object ACTIVE
    Object found SUCCEEDED
    Object not found ABORTED
    Robot stuck: cancel action + call for recovery system ABORTED
    Action canceled ABORTED

    approach_goal

    Objective: approach a visualized object to the robot's reach.

    Functionality:

    1. The service is activated.
    2. The object transform is obtained.
    3. A goal is calculated and validated to approach the robot to the object at its arm's reach.
    4. The goal is reached.
    Feedback Info needed Involved servers
    • string approach_status: Tells if robot is approaching object, robot cannot approach object, or if robot could not complete the task.
    • The position in space or reference frame of the object, with its corresponding transform.
    • Object detection: object_position
      • Gives the position of the object, relative to the robot's base frame
    • Nav stack: move_base
      • States if the robot has reached a certain goal in the room.

    State cases:

    Case description State
    Robot is calculating and validating posible closest goal to the object. PENDING
    Robot is approaching the object ACTIVE
    Robot approached object successfully SUCCEEDED
    Time limit or approach exceeded ABORTED
    Invalid calculated goal / goal cannot be calculated = object cannot be reached ABORTED
    Robot stuck: cancel action + call for recovery system ABORTED
    Canceled action ABORTED

    [WIP] Action Server Algorithm

    This is the actual action server algorithm that is being used, it receives a string and looks for its PoseStamped value in a JSON. For more details of the code visit the navigation action code.

    Navigation Action Server

    This action sever recives a string from the main engine with the format of flagOfAction-Attribute, which can be of the following types:

    Go To Search Object Approach Object Cancel Goal
    gt-location so-roomToSearch ao-roomToSearch cl-null

    The respones are handled by the action server and then it calls an instance of the action:

    if action == "gt":
  self.executeGoToAction(goal_given)

elif action == "so":
  self.executeSearchRoomAction(room_to_search)

elif action == "ao":
  self.executeApproachObjectAction(room_to_search)
    To see the specified class of the three actions click here

    After the corresponding calls have been made, the server sets the target area to approach calling an instance of the navigation stack movement node MoveBase

    def send_goal(self, goal_pose_given):
  self._goal = MoveBase()
    ⚠️ **GitHub.com Fallback** ⚠️