Navigation Action Server

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()