Skip to content

Contents

Introduction

The barebones ROS simulator we wrote in the previous tutorial doesn't actually leverage any ROS features, we just called our new CRS models within some C++ code which was merely launched by ROS. Now let's actually turn our simulator into a proper ROS node and publish our state using the message files we defined earlier.

The ROS wiki has a very nice page on writing simpile publishers and subscribers.

Step-by-Step

Include Messages

Include the state msg header. Note that while we didn't actually write any .h file for our message (just the .msg) the .h version is automatically generated.

#include <crs_msgs/parafoil_3dof_state.h>

Setup ROS node

Starting from where we left off in the previous tutorial.

  1. Initialize the ROS node with ros::init and a node name e.g. simulator
  2. Create a Nodehandle which will give us access to many ros functions
  3. Create a Publisher using out state msg
  4. Define the loop_rate (frequency)
int main(int argc, char** argv)
{
  // Code from last time
  crs_models::parafoil_3dof_model::parafoil_3dof_params params = {9.89, 4.72, 0.341, 0.43};
  crs_models::parafoil_3dof_model::parafoil_3dof_state state = {0.0, 0.0, -100.0, 0.785398, 0.0};
  crs_models::parafoil_3dof_model::parafoil_3dof_input control_input = {0.0};

  std::cout << state << std::endl;

  // ROS node setup
  ros::init(argc, argv, "simulator");
  ros::NodeHandle nh;
  ros::Publisher state_pub = nh.advertise<crs_msgs::parafoil_3dof_state>("state", 100);
  ros::Rate loop_rate(10);
}

Add a Loop

  1. Add a while (ros::ok()) loop. The exit condition is ros::ok, this will terminate on ctrl-c event or when master server kills the node. Essentially we are letting ROS take care of terminating the node.
  2. Do something with your model. In this case we are going to control one of our system states with a simple P-controller. Then call applyModel to advance the state.
  3. Create a msg object of the state msg type we included,, assign its values and publish the message
  4. Print the state so we can check that everything is working in the terminal
  5. Add a ros::spinOnce() which causes the node to cycle in ROS, as well as a loop_rate.sleep() command to ensure the node loops at the desired loop_rate we defined above.
int main(int argc, char** argv)
{

  // --snip--

  while (ros::ok())
  {
    std::unique_ptr<crs_models::parafoil_3dof_model::DiscreteParafoil3dofModel> model;
    model = std::make_unique<crs_models::parafoil_3dof_model::DiscreteParafoil3dofModel>(params);

    // apply p-controller and propagate model
    control_input.delta_a = -5.0*state.psi;
    state = model->applyModel(state, control_input, 0.1);

    // msg creation & publishing
    crs_msgs::parafoil_3dof_state msg;
    msg.x = state.pos_x;
    msg.y = state.pos_y;
    msg.z = state.pos_z;
    msg.psi = state.psi;
    msg.psi_rate = state.psi_rate;
    state_pub.publish(msg);

    // print msg
    std::cout << msg << std::endl;

    // spin and sleep
    ros::spinOnce();
    loop_rate.sleep();
  }


  return 0;
}

Review

the entire file should then appear as follows

#include <ros/ros.h>
#include <casadi/casadi.hpp>

#include <iostream>

#include <crs_msgs/parafoil_3dof_state.h>

#include <parafoil_3dof_model/parafoil_3dof_model_discrete.h>
#include <parafoil_3dof_model/parafoil_3dof_params.h>
#include <parafoil_3dof_model/parafoil_3dof_state.h>
#include <parafoil_3dof_model/parafoil_3dof_input.h>

int main(int argc, char** argv)
{
  // Code from last time
  crs_models::parafoil_3dof_model::parafoil_3dof_params params = {9.89, 4.72, 0.341, 0.43};
  crs_models::parafoil_3dof_model::parafoil_3dof_state state = {0.0, 0.0, -100.0, 0.785398, 0.0};
  crs_models::parafoil_3dof_model::parafoil_3dof_input control_input = {0.0};

  std::cout << state << std::endl;

  // ROS node setup
  ros::init(argc, argv, "simulator");
  ros::NodeHandle nh;
  ros::Publisher state_pub = nh.advertise<crs_msgs::parafoil_3dof_state>("state", 100);
  ros::Rate loop_rate(10);

  while (ros::ok())
  {
    std::unique_ptr<crs_models::parafoil_3dof_model::DiscreteParafoil3dofModel> model;
    model = std::make_unique<crs_models::parafoil_3dof_model::DiscreteParafoil3dofModel>(params);

    // apply p-controller and propagate model
    control_input.delta_a = -5.0*state.psi;
    state = model->applyModel(state, control_input, 0.1);

    // msg creation & publishing
    crs_msgs::parafoil_3dof_state msg;
    msg.x = state.pos_x;
    msg.y = state.pos_y;
    msg.z = state.pos_z;
    msg.psi = state.psi;
    msg.psi_rate = state.psi_rate;
    state_pub.publish(msg);

    // print msg
    std::cout << msg << std::endl;

    // spin and sleep
    ros::spinOnce();
    loop_rate.sleep();
  }


  return 0;
}

Up Next

Now we have a real ROS node which publishes the state. However, just publishing is not enough for a generic ROS node. Next we will add a controller that publishes and subscribes