Programming New Behaviors

Aerostack uses a behavior-based execution system to guide its drones into the desired missions. For trying out the possibilities brought by Aerostack or creating simple missions, the behaviors shipped with the framework will be enough for most users. Nonetheless, when developing more complex missions with application-specific logic they usually end up falling short. Luckily, Aerostack was designed with scalability in mind and allows its users to solve the problem by programming their own behaviors. This section will explain how to do this.

Before starting, the user should be aware that programming behaviors has a slightly different nature than the rest of the framework: Aerostack usually requires little to no knowledge from the user in order to acomplish their deseried tasks, however, developing behaviors needs programming, and programming is an inherently complex activity, so this section will inevitably demand more from the reader to follow. In concrete, this tutorial assumes the reader holds knowledge about the C++ programming language, the ROS middleware and the workings of behaviors.

The explanation will be done by analysing and developing an Aerostack behavior and explaining every design aspect of it. Specifically, this tutorial shows a simplified version of the BehaviorFollowPathWithDF behavior. This behavior receives a sequence of points through a specifc topic and makes the drone move to each position sequentially.

The first step involved is creating the ROS package where the behavior will reside. This is done as with any other behavior package:

$ roscd¹

$ cd ../src

$ catkin_create_pkg quadrotor_motion_with_df_control roscpp behavior_execution_manager

The consensus when developing Aerostack behaviors is to group several of them inside one ROS package instead of creating one package for each behavior. Said package should be named around a common trait of all the constituent behaviors. The name given here is the same as the package shipped with the framework: quadrotor_motion_with_df_control. The quadrotor_motion_with_df_control package contains behaviors oriented to control the movement quadrotor of the drone, that being its common trait.

The dependencies of a package are usually unkown until the actual programming is started and they are commonly added on demand. That said, there are some dependencies that are inherent to behavior programming, those being roscpp and behavior_execution_manager. These are safe to be included on package creation.

Once the package is created the next step is creating the file which will hold the behavior code and proceed with the actual programming. Metada files are usually changed after the behavior has been entirely programmed. Aerostack follows a custom style guide for project layout. This custom layout is based on the ROS standard one. The convention is to divide the code into two directories inside src: the include directory, which contains all the behavior headers, and the source directory, where the implementation for each behavior resides. The user is encouraged to check the Aerostack implementation of the quadrotor_motion_with_df_control as an example. The way to proceed then is to create the header and the implementation files inside their respective directores:

$ cd src²

$ mkdir include

$ touch include/behavior_follow_path_with_df.h

$ mkdir source

$ touch source/behavior_follow_path_with_df.cpp

The next step will be to edit both files to specify the desired logic. This is the most complex and important task of all the ones involved, as it cannot be automated or done without deep thinking. It is out of the scope of this tutorial explaining how to implement user's algorithms or which libraries to use in order achieve their desired behavior. It is up to the user's programming capabilities.

It is common to start editing the header. Any editor able to edit plain text will be enough for programming. Open the header and type:

// Aerostack uses a specific pattern when declaring header guards:
/*
#ifndef <CappedHeaderName>
#define <CappedHeaderName>

<Content>

#endif
*/
// Using nonstandard extensions like:
/*
#pragma once
*/
// Is uncommon and should be avoided.
#ifndef FOLLOW_PATH_H
#define FOLLOW_PATH_H

// When writting include directives, it is common to write all package-external headers with the
// `<Header>` notation and the local headers as `"Header"`.
// It is also recommended to divide the directives in
// sections depending on the precedence of them. Note the division into the
// ROS section, the Behavior section, the Aerostack section and the YAML section.
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <ros_utils_lib/ros_utils.hpp>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32MultiArray.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>

#include <behavior_execution_manager_msgs/BehaviorActivationFinished.h>
#include <BehaviorExecutionManager.h>

#include <aerostack_msgs/FlightState.h>
#include <aerostack_msgs/FlightActionCommand.h>

#include <yaml-cpp/yaml.h>

// Each header usually contains one class declaration, said class being the
// behavior. The pattern inside the codebase is to divide the declaration in
// four groups separated by visibility declarations: The constructors group,
// the members group, the behavior group and finally the callbacks group.
// The user is greatly encouraged to follow this pattern to ensure uniformity
// with the Aerostack codebase.
class BehaviorFollowPathWithDF : public BehaviorExecutionManager {
// The constructos group holds the class constructors and behavior-independent
// public methods declarations.
// In this case only the constructor is needed, but if any other public
// method was required it would be present here.
public:
  BehaviorFollowPathWithDF();

// Private variables and methods are found here in the members group.
// At the same time the variables present are usually further divided into
// smaller subgroups. These usually go by: names of all the used topics,
// subscribers, publishers and miscellaneous.
private:
  // The topic names subsection contains `std::string` instances for
  // each of the topics the behavior will make use of (either by subscribing
  // or publishing).
  // Note the naming pattern: <Topic>_str.
  std::string flight_action_str;
  std::string flight_state_str;
  std::string path_blocked_by_obstacle_str;
  std::string trajectory_str;

  // The topic names subsection holds all needed subscribers.
  // Note the naming pattern: <Topic>_sub.
  ros::Subscriber flight_state_sub;
  ros::Subscriber path_blocked_by_obstacle_sub;
  ros::Subscriber trajectory_sub;

  // The topic names subsection contains all needed publishers.
  // Note the naming pattern: <Topic>_pub.
  ros::Publisher flight_action_pub;

  // The miscellaneous group. The state of the behavior during its
  // execution can be found here.
  bool path_blocked;
  bool should_continue;
  aerostack_msgs::FlightState status_msg;

// The behavior group usually contains the declaration of all the virtual
// methods of the `BehaviorExecutionManager` parent class, that is, this
// group is always constituted by the same members. These will be all the
// methods together with callbacks that most behaviors will have.
private:
  void onConfigure();
  void onActivate();
  void onExecute();
  bool checkSituation();
  void checkGoal();
  void checkProgress();
  void checkProcesses();
  void onDeactivate();

// Finally, the callbacks group contains (not accidentally) all the callbacks
// that will be registered for each subscriber. The recommended naming is
// <Topic>Callback.
public:
  void flightStateCallback(const aerostack_msgs::FlightState &msg);
  void pathBlockedByObstacleCallback(const std_msgs::Bool &msg);
  void trajectoryCallback(const trajectory_msgs::JointTrajectoryPoint& traj);
};

#endif

After editing the header, it is typical to proceed with the behavior implementation:

// A behavior implentation is structured in the follwing way:
// First, an entry point is defined (note that even though all behaviors reside
// in the same directory of a project each of them is a different program, so
// each of them needs an entry point).
// After the entry point all members functions are implemented in the same
// order as they were defined inside al the header (public methods, behavior
// methods and callbacks in that order).

#include "../include/behavior_follow_path_with_df.h"

// The entry point of an Aerostack behavior is structured the same way as for
// any other behavior.
int main(int argc, char** argv){
  ros::init(argc, argv, ros::this_node::getName());

  BehaviorFollowPathWithDF().start();

  return 0;
}

// Aerostack behavior construction is also equivalent to that usual to
// generic behaviors.
BehaviorFollowPathWithDF::BehaviorFollowPathWithDF() {
  setName("follow_path_with_df");

  setExecutionGoal(ExecutionGoals::ACHIEVE_GOAL);
}

// The configure method of an Aerostack behavior its usually limited to reading
// the graph names chosen by the user for each of its used topics.
// The choosen names are read from the parameter server, expecting all names to
// be strings. Behaviors should get names from the parameters obtained by
// contatenating the [standard Aerostack topic](https://github.com/aerostack/install/wiki/Common-ROS-Topics)
// with '_topic', leaving as the default value the complete name.
// For instance, for the `sensor_measurement/camera` from the
// [Sensor Measurements](https://github.com/aerostack/install/wiki/Common-ROS-Topics#sensor-measurements)
// section, the behavior would read from the parameter server the parameter
// `~camera_topic`, defaulting to `sensor_measurement/camera` if the parameter
// could not be found.
// The reader should note that no actual subscription or advertisement of any
// topic is happening in the function, trhy will happen later on. All names
// inside the topic names subgroup should be set after configuration.
void BehaviorFollowPathWithDF::onConfigure(){
  ros_utils_lib::getPrivateParam<std::string>("~flight_action_topic", flight_action_str, "actuator_command/flight_action");
  ros_utils_lib::getPrivateParam<std::string>("~flight_state_topic", flight_state_str, "self_localization/flight_state");
  ros_utils_lib::getPrivateParam<std::string>("~path_blocked_by_obstacle_topic", path_blocked_by_obstacle_str, "environnment/path_blocked_by_obstacle");
  ros_utils_lib::getPrivateParam<std::string>("~trajectory_topic", trajectory_str, "motion_reference/trajectory");
}

// When activating, an Aerostack behavior usually subscribes and advertises all
// needed topics, reads configuration and initializes internal state.
// Both the subscribers and the publishers will use the names read during
// configuration. Special care should be taken not to forget prefixing the
// namespace to the each of the names, as Aerostack behaviors are expected
// to read given topics from inside their behavior namespace.
void BehaviorFollowPathWithDF::onActivate(){
  std::string namespace_ = "/" + getNamespace() + "/";
  ros::NodeHandle node_handle = getNodeHandle();

  // Each subscription should use the correspoding callback as declared in the
  // header. All the subscriptions should be online at this point, with the
  // expectation of ones dependent on some internal state, which should be
  // registered once such state is reached.
  path_blocked_by_obstacle_sub = node_handle.subscribe(namespace_ + path_blocked_by_obstacle_str, 1, &BehaviorFollowPathWithDF::pathBlockedByObstacleCallback, this);

  trajectory_sub = node_handle.subscribe(namespace_+ trajectory_str, 1, &BehaviorFollowPathWithDF::trajectoryCallback,this);

  // All publishers should be up after activation, with the exception of those
  // requiring a specific state or those who are used once. The first ones should
  // be left uninitalized and the second ones should not be members of the class
  // and be created as local variables when needed.
  flight_action_pub = node_handle.advertise<aerostack_msgs::FlightActionCommand>(namespace_ + flight_action_str, 1, true);

  // Aerostack nodes receive configuration from the behavior activation parameters.
  // These are interpreted as YAML nodes. The usual procedure is to transform
  // the text to a YAML node (from <yaml-cpp/yaml.h>) and read its entries,
  // setting missing values to a default.
  should_continue = YAML::Load(getParameters())["should_continue"].IsDefined()

  path_blocked = false;

  aerostack_msgs::FlightActionCommand msg;

  msg.header.frame_id = "behavior_follow_path";
  msg.action = aerostack_msgs::FlightActionCommand::MOVE;

  flight_action_pub.publish(msg);
}

// During execution behaviors usually publish to topics or update internal
// state. Execution is used when a topic needs receive new messages frequently
// or when a costly computation is needed.
void BehaviorFollowPathWithDF::onExecute() { }

// Aerostack behavior checking is done the same way as for
// generics behaviors.
bool BehaviorFollowPathWithDF::checkSituation(){
  if (status_msg.state == aerostack_msgs::FlightState::LANDED){
    setErrorMessage("Error: Drone is landed.");

    return false;
  }

  return true;
}

void BehaviorFollowPathWithDF::checkGoal() {
  if(!should_continue) {
    BehaviorExecutionManager::setTerminationCause(behavior_execution_manager_msgs::BehaviorActivationFinished::GOAL_ACHIEVED);
  }

}

void BehaviorFollowPathWithDF::checkProgress() {
  if(path_blocked){
    BehaviorExecutionManager::setTerminationCause(behavior_execution_manager_msgs::BehaviorActivationFinished::WRONG_PROGRESS);

    setErrorMessage("Error:The path is blocked.");
  }
}

void BehaviorFollowPathWithDF::checkProcesses() { }

// Deactivation ensures the framework state is stable and shutdowns all
// subscribers and publishers acquired during activation.
void BehaviorFollowPathWithDF::onDeactivate(){
  // Ensuring the framework integrity usually involves stoping any running
  // tasks that may be half-way executed when the behavior is deactivated.
  // In this example the movement of the dron is set to hover before
  // deactivating due to the possibility of it being still moving.
  // Behaviors should ensure integrity only for the state they managed
  // during their execution, so they don't affect any other running behavior.
  aerostack_msgs::FlightActionCommand msg;

  msg.header.frame_id = "behavior_follow_path";
  msg.action = aerostack_msgs::FlightActionCommand::HOVER;

  flight_action_pub.publish(msg);

  path_blocked_by_obstacle_sub.shutdown();

  trajectory_sub.shutdown();

  flight_action_pub.shutdown();
}

// Callbacks usually interpret the data received from their topic and
// update internal state acordingly or publish to advertised topics.
void BehaviorFollowPathWithDF::pathBlockedByObstacleCallback(const std_msgs::Bool &msg){
  path_blocked = msg.data;
}

void BehaviorFollowPathWithDF::flightStateCallback(const aerostack_msgs::FlightState &msg){
  status_msg = msg;
}

void BehaviorFollowPathWithDF::trajectoryCallback(const trajectory_msgs::JointTrajectoryPoint& traj){
  // Note that arbitrary computation can be performed inside callbacks, even
  // though the developer should be careful not to block the behavior execution
  // for too long, as incoming messages from other topics may be lost if not
  // handled fast enough or the receiver of published topics may show
  // glittering due to not receaving new orders fast enough.
  // Costly computations should be done incrementally during execution,
  // performing little steps in each iteration.
  double value = 0.0f;

  for (int i = 0; i < 3; ++i){
    value += fabs(traj.velocities[i]);
    value += fabs(traj.accelerations[i]);
  }

  should_continue = value != 0.0f;
}

Once the behavior is fully implemented compilation is required. Metafiles must be changed for a correct execution of Catkin. First, the package manifest must be updated to contain all the package information and its dependencies:

<?xml version="1.0"?>
<!-- Package manifests are usually written in version 1 due to historical -->
<!-- reasons and new behaviors should stick to it. -->
<package>
  <name>quadrotor_motion_with_df_control</name>

  <version>0.1.0</version>

  <description>Quadrotor differential flatness control system of behaviors.</description>

  <author>Your Name</author>

  <maintainer email="your.name@your.domain">Your Name</maintainer>

  <license>BSD-3</license>

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>behavior_execution_manager</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>aerostack_msgs</build_depend>
  <build_depend>behavior_execution_manager_msgs</build_depend>
  <build_depend>ros_utils_lib</build_depend>
  <build_depend>tf</build_depend>

  <run_depend>roscpp</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>behavior_execution_manager</run_depend>
  <run_depend>geometry_msgs</run_depend>
  <run_depend>aerostack_msgs</run_depend>
  <run_depend>behavior_execution_manager_msgs</run_depend>
  <run_depend>ros_utils_lib</run_depend>
  <run_depend>tf</run_depend>
</package>

The CMakeLists.txt files must also contain all needed declarations:

cmake_minimum_required(VERSION 2.8.3)

project(quadrotor_motion_with_df_control)

# If the behavior requires a newer version other than the ROS default, it
# should be stated after the `project` commnand. The Aerostack codebase uses
# C++11 and C++17, being those the recommended options for new packages.
add_definitions(-std=c++11)

# The structure followed by Aerostack for writting CMake files consists
# of first declaring variables with the location of include and source
# directories, followed by the declaration of another variable containing
# all include and source files. After that catkin packages are searched
# and the Catkin package declaration is found. Finally, a target is created
# for each behavior inside the package.

set(QUADROTOR_MOTION_WITH_DF_CONTROL_INCLUDE_DIR
  src/include
)

set(QUADROTOR_MOTION_WITH_DF_CONTROL_SOURCE_DIR
  src/source
)

# The reader should remember behavior packages can hold the definition of
# several behaviors. The example only includes one header and one
# implementation, but there should be a pair for each desired behavior.
set(QUADROTOR_MOTION_WITH_DF_CONTROL_HEADER_FILES
        ${QUADROTOR_MOTION_WITH_DF_CONTROL_INCLUDE_DIR}/behavior_follow_path_with_df.h
)

set(QUADROTOR_MOTION_WITH_DF_CONTROL_SOURCE_FILES
        ${QUADROTOR_MOTION_WITH_DF_CONTROL_SOURCE_DIR}/behavior_follow_path_with_df.cpp
)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
  behavior_execution_manager
  geometry_msgs
  aerostack_msgs
  behavior_execution_manager_msgs
  ros_utils_lib
  tf
)

catkin_package(
  INCLUDE_DIRS ${QUADROTOR_MOTION_WITH_DF_CONTROL_INCLUDE_DIR}
  LIBRARIES ${PROJECT_NAME}
  CATKIN_DEPENDS
  roscpp
  std_msgs
  behavior_execution_manager
  geometry_msgs
  aerostack_msgs
  behavior_execution_manager_msgs
  ros_utils_lib
  tf
  DEPENDS yaml-cpp
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

# Once again, there should be a target for each behavior inside the package.
add_executable(BehaviorFollowPathWithDF ${QUADROTOR_MOTION_WITH_DF_CONTROL_SOURCE_DIR}/behavior_follow_path_with_df.cpp)
target_link_libraries(BehaviorFollowPathWithDF ${catkin_LIBRARIES})
target_link_libraries(BehaviorFollowPathWithDF yaml-cpp)

if(catkin_EXPORTED_LIBRARIES)
  add_dependencies(BehaviorFollowPathWithDF ${catkin_EXPORTED_LIBRARIES})
endif()

With all the files filled, the package can be built:

$ roscd¹

$ cd ..

$ catkin_make

Once compilation finishes, Aerostack behaviors can be executed as any other ROS node, but the usual methodology is to create a lauch file which starts all the package behaviors, leaving all of them waiting for an activation call.

$ mkdir launch

$ cd launch

$ touch quadrotor_motion_with_df_control.launch

The name given to the lauch file is the same as the package apending the .launch extension. The file can then be written:

<launch>
  <!-- Aerostack lauch files are usually divided in two sections: the argumennts -->
  <!-- section and the node section. -->

  <!-- The arguments section relates each command line argument to a parameter -->
  <!-- of the ones read by the package behavior on configuration. -->
  <!-- Note that no parameters are set and no nodes are launched here. -->
  <!-- It is also important the fact that parameters read by the BehaviorExecutionManger -->
  <!-- are also set. -->
  <arg name="namespace"                                     default = "drone1" />
  <arg name="uav_mass"                                      default = "0.7"     />
  <arg name="frecuency"                                     default = "100.0" />
  <arg name="activate_behavior_srv"                         default = "activate_behavior" />
  <arg name="deactivate_behavior_srv"                       default = "deactivate_behavior" />
  <arg name="check_activation_conditions_srv"               default = "check_activation_conditions" />
  <arg name="activation_finished_topic"                     default = "behavior_activation_finished" />
  <arg name="behavior_system"                               default = "quadrotor_motion_with_df_control" />
  <arg name="path_blocked_by_obstacle_topic"                default = "environnment/path_blocked_by_obstacle" />
  <arg name="flight_state_topic"                            default = "self_localization/flight_state" />
  <arg name ="trajectory_topic"                             default = "motion_reference/trajectory" />
  <arg name ="flight_action_topic"                          default = "actuator_command/flight_action" />

  <!-- The group section is further divided in two subsections: the parameters -->
  <!-- subsection and nodes subsection. -->
  <!-- All the declarations inside thse subsections are placed inside a ROS -->
  <!-- namespace created by concatenating the behavior namespace and behavior system. -->
  <group ns="$(arg namespace)/$(arg behavior_system)">
    <!-- The parameter subsection sets all parameters used by the behavior in the -->
    <!-- parameter server. Note that the default values used are the ones declared -->
    <!-- in the aarguments sections. This is made so configuration can be changed -->
    <!-- from the command line. -->
    <param name="~namespace"                               value="$(arg namespace)"                             type="str" />
    <param name="~frecuency"                               value="$(arg frecuency)"                             type="double" />
    <param name="~activate_behavior_srv"                   value="$(arg activate_behavior_srv)"                 type="str" />
    <param name="~deactivate_behavior_srv"                 value="$(arg deactivate_behavior_srv)"               type="str" />
    <param name="~check_activation_conditions_srv"         value="$(arg check_activation_conditions_srv)"       type="str" />
    <param name="~behavior_system"                         value="$(arg behavior_system)"                       type="str" />
    <param name="~path_blocked_by_obstacle_topic"          value="$(arg path_blocked_by_obstacle_topic)"        type="str" />
    <param name="~flight_state_topic"                      value="$(arg flight_state_topic)"                    type="str" />
    <param name="~trajectory_topic"                        value="$(arg trajectory_topic)"                      type="str" />
    <param name="~flight_action_topic"                     value="$(arg flight_action_topic)"                   type="str" />

    <!-- The node section instantiates a ROS node for each behavior inside the package. -->
    <node name="behavior_follow_path_with_df" pkg="quadrotor_motion_with_df_control"  type="BehaviorFollowPathWithDF"  output="screen"/>
  </group>
</launch>

After that last step the behaviors can be launched:

$ roslaunch quadrotor_motion_with_df_control behavior_follow_path_with_df.launch

Finally, when the time arrive they can be activated invoking their activation services.

¹ Asumming the desired workspace is sourced. If not, the user should make sure to be inside the top directory of their workspace.

² Given that the src directory was automatically created by the catkin_create_pkg command. At the day of this writting this utility creates said folder as part of the default package structure, nontheless it may be the case that a future version doesn't behave the same way. The tutorial assumes the command created these files: CMakeLists.txt, package.xml and src (being empty). It is the responsability of the reader to ensure said layout before continuing with the tutorial.