Skip to content

Contents

Introduction

In the previous tutorial we added a ROS simulator node which published the system state at each timestep. Of course a generic node will need to receive some data (subscribe to it), process it somehow, and then send that data on to the next node (publish it). This tutorial describes creating a controller node which subscribes to the system state, generates the control signal, and then publishes this information. While this is written to cover that specific example, it really serves as the tutorial for the entire pub'n'sub concept.

Again, we will reference the very nice page on writing simpile publishers and subscribers

Callback Troubles

Unlike publishers, which are relatively straightforward to implement, subscribers have an additional complications: they function via a callback function. This function is called (pretty much) every time a message is published to the topic they are subscribed to.

The following code from the ros wiki entry demonstrates the structure:

#include "ros/ros.h"
#include "std_msgs/String.h"

void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
  ros::spin();
  return 0;
}

It is important to realize that main is only called once on node startup, and it is the Callback which is executed for each published message. If we want to publish and subscribe this presents several issues:

  • We cannot directly publish from the callback, as it does not have a NodeHandle
  • It would not be practical to initialize a ros node with ros::init(argc, argv, "name") within the callback
  • We also cannot write the messages from the subscription to a global variable to then publish inside main since this is not executed on every iteration

The most obvious and robust solution is to make the node use a class with the nh and publishers/subscribers as member variables, and the Callback as a class method. This is also what has been done in the other packages within ros4crs/ and is what we will use as a reference.

Step-by-Step

Setup Folder Structure

Just like we did when writing the barebones ROS simulator, let's take a look at an existing folder structure. We can then either use this as a reference, or duplicate the entire folder and remove/rename the files to get the structure we need. In this case will look at src/ros4crs/ros_backtracker/

ros_backtracker
├── app
│   ├── crash_detector_node.cpp
│   ├── input_filter_node.cpp
│   └── linear_backtracker_node.cpp
├── CMakeLists.txt
├── config
│   └── linear_backtrack_config.yaml
├── include
│   └── ros_backtracker
│       ├── crash_detector.h
│       ├── input_filter.h
│       └── linear_backtracker.h
├── launch
│   └── default.launch
├── package.xml
└── src
    ├── crash_detector.cpp
    ├── input_filter.cpp
    └── linear_backtracker.cpp

By now this should be a pretty familiar structure for a ROS package.

If you decide to duplicate this folder as a foundation for your new controller you can go ahead and delete the crash_detector and input_filter source, header, and application files. In the rest of this tutorial we will assume you have copied the folder architecture and will refer to "renaming" files and "removing" unnecessary functions.

In the end we want to end up with a structure equivalent to the following:

parafoil_controllers
├── app
│   └── parafoil_3dof_controller_node.cpp
├── CMakeLists.txt
├── include
│   └── parafoil_controllers
│       └── parafoil_3dof_controller.h
├── launch
├── package.xml
└── src
    └── parafoil_3dof_controller.cpp

Header Files

Setup include/ros_backtracker/linear_backtracker.h

  1. Rename file & folder e.g. to include/parafoil_controllers/parafoil_3dof_controller.h
  2. Update ifdef e.g. to PARAFOIL_CONTROLLER_PARAFOIL_3DOF_CONTROLLER_H
  3. Update includes
  4. Rename class e.g. to Parafoil3dofController
  5. Update namespace
  6. Remove unnecessary pieces
    • remove nh_private (to simplify)
    • remove Errorstate
    • remove collision_sub
    • remove backtracker parameters
    • remove errorCallback
  7. Update method briefs
  8. Update msg types

We are then just left with a constructor and the stateCallback function which will also handle publishing.

#ifndef PARAFOIL_CONTROLLERS_PARAFOIL_3DOF_CONTROLLER_H
#define PARAFOIL_CONTROLLERS_PARAFOIL_3DOF_CONTROLLER_H

#include <crs_msgs/parafoil_3dof_input.h>
#include <crs_msgs/parafoil_3dof_state.h>
#include <numeric>
#include <ros/ros.h>

namespace parafoil_controllers
{

class Parafoil3dofController
{
private:
  ros::NodeHandle nh_;
  // ros::NodeHandle nh_private_;

  ros::Publisher input_pub_;
  ros::Subscriber state_sub_;

  /**
  * @brief State callback.
  *
  * @param measured_state
  */
  void stateCallback(crs_msgs::parafoil_3dof_state::ConstPtr measured_state);

public:
  /**
  * @brief Construct a new Parafoil 3dof Controller object.
  *
  * @param nh
  */
  Parafoil3dofController(ros::NodeHandle nh);
};
}  // namespace parafoil_controllers

#endif  // PARAFOIL_CONTROLLERS_PARAFOIL_3DOF_CONTROLLER_H

Source Files

Update src/linear_backtracker.cpp

  1. Rename file e.g. to parafoil_3dof_controller.cpp
  2. Update includes
  3. Update namespace
  4. Update class name e.g. to Parafoil3dofController
  5. Remove unnecessary bits
    • remove nh_private
    • remove collision_sub
    • remove errorCallback
    • update stateCallback
    • remove errorstate if-else
  6. Fix input & state msg types
  7. Add input assignment dependent on pointer e.g. a p-controller

Again, we are left with the constructor as well as the stateCallback, which takes a pointer to the measured_state as its argument, applies a P controller, and then publishes the result.

#include "parafoil_controllers/parafoil_3dof_controller.h"

namespace parafoil_controllers
{

Parafoil3dofController::Parafoil3dofController(ros::NodeHandle nh)
{
  input_pub_ = nh.advertise<crs_msgs::parafoil_3dof_input>("control_input", 1);
  state_sub_ = nh.subscribe("state", 1, &Parafoil3dofController::stateCallback, this);
}

void Parafoil3dofController::stateCallback(crs_msgs::parafoil_3dof_state::ConstPtr measured_state)
{
  crs_msgs::parafoil_3dof_input input;
  input.delta_a = -5.0*measured_state->psi;
  input.header.stamp = ros::Time::now();
  input_pub_.publish(input);

  std::cout << "input: " << input << std::endl;
};
}  // namespace parafoil_controllers

Application Files

Update app/linear_backtracker_node.cpp

  1. Rename file e.g. to parafoil_3dof_controller_node.cpp
  2. Update includes
  3. Remove nh_private
  4. Rename class to match above defined structure, i.e parafoil_controllers::Parafoil3dofController
  5. Setup a spinner.spin() loop. Note that we do not use a loop_rate here like we did for the simulator. This controller is designed not to execute at a given frequency, but rather every time the state is published.
#include "parafoil_controllers/parafoil_3dof_controller.h"
#include <ros/ros.h>
#include <ros_crs_utils/parameter_io.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "ros_parafoil_3dof_controller_node");
  ros::NodeHandle nh = ros::NodeHandle("");           // /<NAMESPACE>/*
  parafoil_controllers::Parafoil3dofController controller(nh);

  ros::MultiThreadedSpinner spinner(1);  // Use 1 threads
  spinner.spin();                        // spin() will not return until the node has been shutdown
  return 0;
}

Admin Files

Update CMakeLists.txt

  1. Update project name
  2. Remove non-linear_backtracker dependencies
  3. Rename linear_backtracker (to match our file names)
  4. Remove targetlinklibraries($Projectname)
  5. Update provided Libraries and Executables to match our new controller files
  6. Remove tests
cmake_minimum_required(VERSION 2.8.3)
project(parafoil_controllers)

set(CMAKE_CXX_STANDARD 17)
find_package(catkin_simple REQUIRED)
catkin_simple(ALL_DEPS_REQUIRED)

catkin_package()

#############
# Libraries #
#############
include_directories(
include
${catkin_INCLUDE_DIRS}
)

#############
# Libraries #
#############

cs_add_library(${PROJECT_NAME}
      src/parafoil_3dof_controller.cpp
      )

###############
# Executables #
###############

cs_add_executable(parafoil_3dof_controller_node
app/parafoil_3dof_controller_node.cpp)
target_link_libraries(parafoil_3dof_controller_node ${PROJECT_NAME})


##########
# Export #
##########

cs_install()
cs_export(INCLUDE_DIRS ${CATKIN_DEVEL_PREFIX}/include)

Update package.xml

  1. Update project name
  2. Update maintainer name & email if necessary
<?xml version="1.0"?>
<package format="2">
<name>parafoil_controllers</name>
<version>0.0.1</version>
<description>TODO </description>

<author email="norrisg@ethz.ch">Griffin Norris</author>
<maintainer email="norrisg@ethz.ch">Griffin Norris</maintainer>

<license>BSD 2-Clause </license>

<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>catkin_simple</buildtool_depend>
<depend>roscpp</depend>
<depend>crs_msgs</depend>
<depend>ros_crs_utils</depend>
<depend>commons</depend>
<export></export>
</package>

Launch Files

Finally make sure to update the simulator/ launch file such that it also spawns a controller node

<launch> 
<group ns = "test_ns">
    <node pkg="parafoil_simulator" name="parafoil_simulator" type="simulation_node" output="screen">
    </node>
    <node pkg="parafoil_controllers" name="parafoil_controller" type="parafoil_3dof_controller_node" output="screen">
    </node>
  </group>
</launch>

Up Next

This controller node will publish the correct control input to a topic, however we have not updated the simulator to subscribe yet. Let's upgrade the simulator to pub'n'sub in the next tutorial so we truly have a working, although basic, control loop.