Program New Behaviors

Aerostack uses a behavior-based execution system to simplify the configuration of robot mission plans. Each behavior may be implemented as a ROS nodelet as part of a behavior package that groups several behaviors with common uses. Behaviors may also be implemented as regular ROS nodes.

This tutorial explains the sequence of steps to implement the behavior ROTATE_WITH_PID_CONTROL that is part of the behavior package quadrotor_motion_with_pid_control. All files related to this package can be found in $AEROSTACK_STACK/behaviors/behavior_packages/quadrotor_motion_with_pid_control.

Step 1. Create the correct directory structure for the package

The following lines show the correct directory structure for the behavior package and behavior:

quadrotor_motion_with_pid_control/                          -- Name of the package
      package/                                              -- Behavior package implementation
        CMakeLists.txt                                      -- CMakeLists.txt file
        package.xml                                         -- Package manifest
        quadrotor_motion_with_pid_control.xml               -- File to define the nodelets as a plugin
        launch/
          quadrotor_motion_with_pid_control.launch          -- Launcher
        src/
          include/
            behavior_rotate_with_pid_control.h              -- Header file
          source/
            behavior_rotate_with_pid_control.cpp            -- C++ file
      subpackages/                                          -- Subpackages for this system (if necessary)
        quadrotor_pid_controller/
        thrust_controller/
        path_tracker/

Step 2. Edit the files CMakeLists and package.xml

To program a behavior, the following two C++ classes are used: BehaviorExecutionController and Nodelet. The file CMakeLists.txt and package.xml must have the following lines:

File CMakeLists.txt

find_package(catkin REQUIRED
COMPONENTS
roscpp
behavior_execution_controller
nodelet
aerostack_msgs
geometry_msgs
std_msgs
)

catkin_package(
INCLUDE_DIRS ${QUADROTOR_MOTION_PID_CONTROL_INCLUDE_DIR}
CATKIN_DEPENDS
roscpp
behavior_execution_controller
nodelet
aerostack_msgs
geometry_msgs
std_msgs
)

File package.xml

<build_depend>roscpp</build_depend>
<build_depend>behavior_execution_controller</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>aerostack_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>std_msgs</build_depend>

<run_depend>roscpp</run_depend>
<run_depend>behavior_execution_controller</run_depend>
<run_depend>nodelet</run_depend>
<run_depend>aerostack_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>std_msgs</run_depend>

It is also important to include the tag to specify the xml file used to export the nodelets:

<export>
<nodelet  plugin="${prefix}/quadrotor_motion_with_pid_control.xml"  />
</export>

Step 3. Edit the launcher of the package

The behavior package has a folder called launch with a launcher file. In our example, this file is called quadrotor_motion_with_pid_control.launch.

In this you can include ROS topic names and other parameters.

It is also important to include the following line in this launcher to launch its nodelet manager:

<node  pkg="nodelet"  type="nodelet"  args="manager"  name="quadrotor_motion_with_pid_control_manager"  output="screen" />

And also include a line like the following one for each behavior in the behavior system:

<node  pkg="nodelet"  type="nodelet"  args="load quadrotor_motion_with_pid_control/BehaviorTakeOff quadrotor_motion_with_pid_control_manager" name="behavior_rotate_with_pid_control"  output="screen"/>

Step 4. Edit the <package_name>.xml file

The <package_name>.xml file defines the nodelet as a plugin. To do so, in the first line, you should write the library to which the plugin will be exported:

<library  path="lib/libquadrotor_motion_with_pid_control">

Then, you have to include a tag with the following format for each behavior included in the package:

<class  name="quadrotor_motion_with_pid_control/BehaviorRotateWithPIDControl"  type="quadrotor_motion_with_pid_control::BehaviorRotateWithPIDControl"
base_class_type="nodelet::Nodelet">
    <description>
      BehaviorRotateWithPIDControl.
    </description>
</class>

Step 5. Create the header and implementation C++ files

Each behavior is implemented in C++ as a sub-class of the class BehaviorExecutionController. The header file needs to have the following structure, using as namespace the name of the behavior system:

#include <behavior_execution_controller.h>
namespace  quadrotor_motion_with_pid_control
{
class  BehaviorRotateWithPIDControl : public  BehaviorExecutionController
{
...
};
}

The implementation file must include the following line to export the class to the nodelet manager:

PLUGINLIB_EXPORT_CLASS(quadrotor_motion_with_pid_control::BehaviorRotateWithPIDControl, nodelet::Nodelet)

It also has to include the name of the behavior and its category:

BehaviorRotateWithPidControl::BehaviorRotateWithPidControl() : BehaviorExecutionController() { 
  setName("rotate_with_pid_control"); 
  setExecutionGoal(ExecutionGoals::ACHIEVE_GOAL);
}

The following functions should be implemented for each behavior:

void  onConfigure();
void  onActivate();
void  onDeactivate();
void  onExecute();
bool  checkSituation();
void  checkGoal();
void  checkProgress();
void  checkProcesses();

The meaning of these functions are detailed below:

• onConfigure(): Prepares the execution, reads params and/or files.

void BehaviorRotateWithPidControl::onConfigure()
{
  node_handle = getNodeHandle();
  nspace = getNamespace();

  ros::param::get("~estimated_speed_topic", self_localization_speed_str);
  ros::param::get("~estimated_pose_topic", self_localization_pose_str);
  ros::param::get("~controllers_topic", command_high_level_str);
  ros::param::get("~motion_reference_speed_topic", motion_reference_speed_str);
  ros::param::get("~motion_reference_pose_topic", motion_reference_pose_str);
  ros::param::get("~set_control_mode_service_name", set_control_mode_srv);
  ros::param::get("~status_topic", status_str);
}

• onActivate(): Activates the behavior, reads the arguments, creates subscribers and publishers.

void BehaviorRotateWithPidControl::onActivate(){
  //Subscribers and publishers:
  self_localization_pose_sub = node_handle.subscribe("/" + nspace + "/"+self_localization_pose_str, 1, &BehaviorRotateWithPidControl::selfLocalizationPoseCallBack, this);
  motion_reference_speed_pub = node_handle.advertise<geometry_msgs::TwistStamped>("/" + nspace + "/"+motion_reference_speed_str,1, true);

  //Arguments:
  std::string arguments=getParameters();
  YAML::Node config_file = YAML::Load(arguments);
  if(config_file["angle"].IsDefined())
  {
    angle=config_file["angle"].as<double>() * M_PI/180;
    //...
  }
  else
  {
    if(config_file["relative_angle"].IsDefined())
    {
      angle=config_file["relative_angle"].as<double>() * M_PI/180;
      //...
    }
  }
}

• onDeactivate(): Stops the execution.

void BehaviorRotateWithPidControl::onDeactivate()
{
  self_localization_pose_sub.shutdown();
  motion_reference_pose_pub.shutdown();
  //...
}

• onExecute(): Develops the execution.

void BehaviorRotateWithPidControl::onExecute()
{
  // This function is executed in a loop. 
  // Sometimes you do not need to code it and you can execute the behavior in onActivate() function 
}

• checkSituation(): Verifies if the behavior can be executed in the current situation.

bool BehaviorRotateWithPidControl::checkSituation()
{
  if (status_msg.state == aerostack_msgs::FlightState::LANDED){
    setErrorMessage("Error: Drone is landed");
    return false;
  }
  return true;
}

• checkGoal(): Verifies if the behavior has achieved its goal.

bool BehaviorRotateWithPidControl::checkGoal()
{
  //...
  //If goal has been achieved:
  BehaviorExecutionController::setTerminationCause(aerostack_msgs::BehaviorActivationFinished::GOAL_ACHIEVED);
}

• checkProgress(): Supervises the progress of the execution.

bool BehaviorRotateWithPidControl::checkProgress()
{
  //...
  //If progress is wrong:
  BehaviorExecutionController::setTerminationCause(aerostack_msgs::BehaviorActivationFinished::WRONG_PROGRESS);
}

• checkProcesses(): Supervises the state of the necessary processes.

bool BehaviorRotateWithPidControl::checkProcesses()
{
  //...
  //If there is a process failure
  BehaviorExecutionController::setTerminationCause(aerostack_msgs::BehaviorActivationFinished::PROCESS_FAILURE);
}

Step 6. Include the behavior in the catalog

The behavior catalog is the file behavior_catalog.yaml. The catalog includes information about each behavior to describe how to use it. This information includes for example the possible parameters of each behavior and incompatibility with other behaviors.

It is important to include this information in the catalog in order to be able to check its correct use and to coordinate its execution with the execution of other behaviors.

There are some important aspects to consider in this file:

• NO tabs should be used. Spaces are used instead.
• The default_values tag indicates values to be used when no explicit specification for the parameter is found.
• There are two categories of tasks: goal_based and recurrent.
• Each behavior performs a task defined in the catalog.
• There may be several behaviors for the same task.
• Exclusivity constraints represent inconsistencies among tasks.
• The requires tag indicates tasks that are required to carry out the activation.
• For each behavior, is necessary to indicate its behavior package.
• It is possible to specify a particular timeout for the execution of the behavior.
• If a task is specified as default, it will continuously be intended to be activated with low priority.

The behavior catalog can be found here: config/mission/behavior_catalog.yaml. It can be modified for a particular application and placed in other folder.

The behavior corresponding to the example presented in this tutorial (ROTATE_WITH_PID_CONTROL) is described in the catalog in the following lines. We express in the catalog that this behavior performs the task ROTATE with two parameters (angle or relative_angle). We also express that the behavior requires the task PID_MOTION_CONTROL.

#------------------------------------------------------
behaviors:

- behavior: ROTATE_WITH_PID_CONTROL
  package: quadrotor_motion_with_pid_control
  task: ROTATE
  requires:
  - task: PID_MOTION_CONTROL

#------------------------------------------------------
tasks:

- task: ROTATE
  timeout: 30
  parameters:
  - parameter: angle
    allowed_values: [-360,360]
  - parameter: relative_angle
    allowed_values: [-360,360]

#------------------------------------------------------
exclusivity_constraints:

- mutually_exclusive:
  - TAKE_OFF
  - LAND
  - HOVER
  - FOLLOW_PATH
  - MOVE_AT_SPEED
  - MOVE_VERTICAL
  - ROTATE

Step 7. Include the behavior in the launcher of an application

In order to use a behavior in a particular application, the launcher of the application must include its behavior package as it illustrate below for the behavior package quadrotor_motion_with_pid_control:

`#---------------------------------------------------------------------------------------------` \
`# Quadrotor motion with pid control                                                           ` \
`#---------------------------------------------------------------------------------------------` \
--tab --title "Quadrotor motion with pid control" --command "bash -c \"
roslaunch quadrotor_motion_with_pid_control quadrotor_motion_with_pid_control.launch --wait \
  namespace:=drone$NUMID_DRONE;
exec bash\""\

Once the components of Aerostack are launched, if you want to activate manually the behavior, just call the ROS service activate_behavior with the name of the behavior (in general, this is not needed because it is normally done by the behavior execution control system of Aerostack). To activate manually a behavior you can write the following:

rosservice call /drone111/quadrotor_motion_with_pid_control/behavior_rotate_with_pid_control/activate_behavior "timeout: 10000"