/smb_mission_planner

Primary LanguagePython

General Info

This repo adds mission planning functionality to the stack. Because it should be easy to get into the code and modify it, everything is kept compact and at a minimum level of abstraction. The mission planner is based on the popular state machine library smach, but no prior knowledge is required.

Some terminology to start with:

  • In the context of this package, a waypoint denotes a named pose in the global frame.
  • A mission is a collection of tasks that are executed sequentially (e.g. a path through several waypoints).
  • A mission plan defines the logic of how and when different missions are executed. The mission plan is implemented as a smach state machine.
  • Each mission has its mission data, containing relevant information for the mission (e.g. its waypoints).

Installation

  • Clone this repo into your workspace. If you followed the instructions on how to install the SMB software, the source code of this package is already located in your workspace.
  • Install smach_ros from here (a state machine library written in Python with ROS support)
  • Install yaml with pip install pyyaml
  • (Buid the package with catkin build smb_mission_planner)

Executing your mission

Basic features

The mission_planner.py executes the previously defined mission plan.

Start the simulation and the path planner. As soon as the path planner is ready to receive goals, start the mission_planner with

roslaunch smb_mission_planner mission_planner.launch

The robot will now try to reach the specified waypoints of each mission one by one, as defined in the yaml file. In the command line window of the mission_planner you can find information on where and in which mission you currently are.

Examples

In the folder src/smb_mission_planner/missions you can find two mission examples:

  • twist_mission: this mission executes a set of twist commands defined in the mission's config file.
  • waypoint_mission: this mission publishes waypoints defined in the mission's config file to the path planner

To add or remove this mission, please modify the file src/smb_mission_planner/mission_planner.py. This can be done by adding or removing the mission's class to the state machine in the function createStateMachine(self).

# Add waypoint mission
smach.StateMachine.add('Waypoint Mission', FARWaypointMission(self.missions_data['waypoint_mission'], self.reference_frame), transitions={'Completed': 'Twist Mission', 'Aborted': 'Failure', 'Next Waypoint': 'Waypoint Mission'})

# Add waypoint mission (using legacy navigation)
smach.StateMachine.add('Waypoint Mission', WaypointMission(self.missions_data['waypoint_mission'], self.reference_frame), transitions={'Completed': 'Twist Mission', 'Aborted': 'Failure', 'Next Waypoint': 'Waypoint Mission'})

# Add twist mission
smach.StateMachine.add('Twist Mission', TwistMission(self.missions_data['twist_mission'], self.reference_frame), transitions={'Completed': 'Success', 'Aborted': 'Failure', 'Next Twist': 'Twist Mission'})

The function takes the arguments:

  • mission name
  • mission class: here we pass the mission's class and give as an argument the waypoints defined in the mission.yaml file
  • transitions: this defines what the state machine has to do in the next step. E.g. if the outcome is Next Waypoint, the next step is to execute the Waypoint Mission

Advanced features

Specify mission data file

You can use a roslaunch argument to specify a file path for the input file, e.g.

roslaunch smb_mission_planner mission_planner.launch mission_file_name:=mission_name.yaml

Constructing a State Machine

A mission plan (state machine) defines the connections (transitions) between different states. The mission plan is implemented as a smach state machine.

The proposed workflow for planning a navigation mission could be as follows:

  1. Record mission data by using the mission_recorder
  2. Create a class with your plan in the folder src/smb_mission_planner/missions
  3. Execute your missions by using the mission_planner

Each of these steps is explained in detail in the upcoming sections.

Record missions

Basic Features

As it is fairly tedious to input poses manually for the mission waypoints, the mission_recorder helps you out. It generates a yaml file with all the waypoint poses you recorded, grouped by mission.

You can launch it with

roslaunch smb_mission_planner mission_recorder.launch

which starts the node. You can then give recording instructions with ros services. To record a mission, call

rosservice call /record_mission {"mission_name","waypoint_1_name, waypoint_2_name, ..."}

where you can use your own mission_name and waypoint_names. The number of waypoints can be selected arbitrarily, just add more to the list. After you sent the /record_mission service, instructions will appear in the command window where you launched the node. You can now input the waypoint poses of the current mission one by one. This can be done

  • in rviz by clicking 2D Nav Goal and visually placing the pose on your map.
  • by sending the desired pose in the topic /move_base_simple/goal.
  • by calling the service rosservice call /record_base_pose, which will record the current base pose as a waypoint. Make sure that the odometry topic for the base pose is set correctly (see Advanced Features on how to do that).

After having recorded all your missions, stop the node with Ctrl-C. All your recorded missions will be dumped to a yaml file (mission.yaml per default). Of course, you can also manually edit the generated yaml file to combine different recording sessions and to add or edit waypoints manually, etc.

Advanced Features

Remove Missions

Remove missions while the node is running with:

rosservice call /remove_mission "mission_name"

Remove Waypoint

Remove waypoints in missions while the node is running with:

rosservice call /remove_waypoint {"mission_name","waypoint_name"}

Specify a file for file dump

You can use a roslaunch argument to specify a file path for the output file, e.g.

roslaunch smb_mission_planner mission_recorder.launch mission_file_name:=mission_name.yaml

Prevent file dump

Prevent file dump with

rosservice call /toggle_file_dump "False"

or re-enable it with "True".

Choose your input topic for recording

You can use a roslaunch argument to specify the waypoints' pose topic for the recording:

roslaunch smb_mission_planner mission_recorder.launch waypoint_topic_name:=/move_base_simple/goal

Choose your odometry topic for recording

You can use a roslaunch argument to specify the base's frame and the reference frame:

roslaunch smb_mission_planner mission_recorder.launch base_frame:=base_link reference_frame:=map

Mission planning

Basic Features

You can combine your recorded missions to create a mission plan by connecting them in the mission_planner.py. The mission_planner.py should be modified by you to add more missions and connect them accordingly. Here, a smach state machine is built up. To learn more about it, visit the tutorials. Make sure to assign to each mission its respective mission data, i.e. its recorded information of the yaml file.

Currently, we provide you with a TwistMission and WaypointMission (see the files in src/smb_mission_planner/mission), which implement the following:

  • Waypoints are set one by one, in the order they were defined in each mission in the yaml config file.
  • If the robot is unable to reach a waypoint (not reaching waypoint in max_time_out), it will abort the mission.
  • A waypoint is reached if the xy-position is within a certain tolerance.
  • If it cannot find the start of a mission, it will abort it.

To add a new mission type with your custom behavior, see the next subsection below.

Advanced features

Add your mission types (e.g. to trigger a measurement instead of just reaching a waypoint):

  • Create a new mission class (similar to the FARWaypointMission in a new file).
  • Don't forget to inherit from smach.State and to implement the __init__ and execute methods.
  • Add your new mission type to the mission_planner.py to use it.

Where to go from here

  • Try to record and execute a mission plan
  • Modify the mission plan in the mission_planner.py file, by adding e.g. more mission states of the FARWaypointMission to the state machine.
  • Add your mission types, e.g. to trigger a measurement instead of just reaching a waypoint.

Common pitfalls

  • It is easy to forget to change the mission names in the mission_planner.py when recording new missions.
  • The rosservice argument must be of the form {"mission_name","waypoint_1_name, waypoint_2_name, ..."} without a space after the separating comma between the strings.
  • Make sure that your waypoints' pose topic, your base pose frame, and reference frame are set correctly.