Skip to content

Contents

Introduction

In the previous tutorial we set up a controller that pubs & subs. Now we want to modify the simulator to do the same. Then the simulator will be able to publish the state as well as subscribe to the control input.

Just like for the controller we will make the simulator into a class with the appropriate Callback methods. However, unlike the controller we will not be able to do everything within the Callback. The simulator should act as our ground-truth, and should be persistent. A Callback function is only called when a new message is published to the corresponding topic, and it is therefore not appropriate to advance the state from within the Callback.

Instead we will add the control input as a class member variable. This variable will then be overwritten with the latest input from the controller whenever the Callback method is executed i.e whenever a new control input is published by the controller. All the while the simulator will continue to advance the state, using the applyModel method of our CRS-based dynamics model. This is exactly the same structure used in CRS' ros4crs/ros_simulator/, which we will refer to to guide our architectural choices.

We will in fact use several different files and directories as a reference for improving our simulator, ensuring that we have good compatibility with the rest of CRS. You can use any of these as a template to create the simulator or just write one from scratch! The only important thing is the structure.

  • src/ros4crs/ros_backtracker/ as a potential template
  • src/ros4crs/ros_simulator/ could also be used as template, requires carefully removing unnecessary files and linking correct ones.
  • Our previous simulator package, i.e. src/ros4crs/parafoil_simulator/

Step-by-Step

Setup Folder Structure

Just as we did when we created the barebones ROS simulator, we can duplicate & rename ros_backtracker as a starting point as it already containts the entire architecture of a ROS package.

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

If you decide to duplicate the ros_backtracker package as a foundation for the simulator 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.

Header Files

We want to end up with the following methods and member variables as part of our simulator class

  • advanceState method to propagate state forward
  • inputCallback to update a last_input member variable

To update include/ros_backtracker/linear_backtracker.h or build the file ourselves we can follow these steps

  1. Name file & folder appropriately e.g. include/parafoil_simulator/parafoil_3dof_simulator.h
  2. Update ifndef & define
  3. Update includes
  4. Update namespace
  5. Remove any unnecessary bits if working from a template e.g. from ros_backtracker:
    • Remove errorstate
    • Remove collision_sub
    • Remove errorCallback
    • Remove error parameters
  6. Rename class e.g. Parafoil3dofSimulator
  7. Add model member variable model
  8. Remove nh_private
  9. Add member variables for current_state_ and last_input_
  10. Setup sub/pub architecture
    • state_pub
    • input_sub
  11. Setup inputCallback (can replace stateCallback of template) including @brief, @param, arguments
  12. Update constructor including @brief, @param, arguments
  13. Add public method advanceState(double timestep)
  14. Add public state publish method publishStates()
#ifndef PARAFOIL_SIMULATOR_PARAFOIL_3DOF_SIMULATOR_H
#define PARAFOIL_SIMULATOR_PARAFOIL_3DOF_SIMULATOR_H

#include <crs_msgs/parafoil_3dof_input.h>
#include <crs_msgs/parafoil_3dof_state.h>
#include <numeric>
#include <ros/ros.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>

namespace parafoil_simulator
{

class Parafoil3dofSimulator
{
private:
    // Model
    std::unique_ptr<crs_models::parafoil_3dof_model::DiscreteParafoil3dofModel> model_;

    // Node handles
    ros::NodeHandle nh_;

    // Publisher
    ros::Publisher state_pub_;
    // Subscriber
    ros::Subscriber input_sub_;

    // Model state and input
    crs_models::parafoil_3dof_model::parafoil_3dof_state current_state_ = {0.0, 0.0, -100.0, 0.785398, 0.0};
    crs_models::parafoil_3dof_model::parafoil_3dof_input last_input_ = {0.0};

    /**
    * @brief Input callback.
    *
    * @param control_input
    */
    void inputCallback(crs_msgs::parafoil_3dof_input::ConstPtr control_input);


public:
    /**
    * @brief Construct a new 3dof parafoil simulator.
    *
    * @param nh
    */
    Parafoil3dofSimulator(ros::NodeHandle nh);

    void advanceState(double timestep);
    void publishStates();
};
}  // namespace parafoil_simulator

#endif  // PARAFOIL_SIMULATOR_PARAFOIL_3DOF_SIMULATOR_H

Source Files

Now we just need to fill in the source file to match the header file either by writing it ourselves or updating our template e.g. src/ros_backtracker.cpp

  1. Ensure filename matches header e.g. parafoil_3dof_simulator.cpp
  2. Update includes
  3. Update namespace
  4. Remove unnecessary bits from template
    • Remove errorCallback
    • Remove errorstate if/else
  5. Update classname to match header e.g. Parafoil3dofSimulator
  6. Remove nh_private
  7. Update pub/sub architecture (don't forget msg types)
  8. Update Constructor
    • Add model_ & params instantiation
    • Add state_pub_ and input_sub_ instantiation
  9. Add advanceState method which calls applyModel method to propagate state
  10. Add inputCallback to update last_input_ with latest control_input message
  11. Add publishStates method
#include "parafoil_simulator/parafoil_3dof_simulator.h"

namespace parafoil_simulator
{

Parafoil3dofSimulator::Parafoil3dofSimulator(ros::NodeHandle nh)
{
    crs_models::parafoil_3dof_model::parafoil_3dof_params params = {9.89, 4.72, 0.341, 0.43};
    model_ = std::make_unique<crs_models::parafoil_3dof_model::DiscreteParafoil3dofModel>(params);

    state_pub_ = nh.advertise<crs_msgs::parafoil_3dof_state>("state", 1);
    input_sub_ = nh.subscribe("control_input", 1, &Parafoil3dofSimulator::inputCallback, this);
}

void Parafoil3dofSimulator::advanceState(double timestep)
{
    current_state_ = model_->applyModel(current_state_, last_input_, timestep);
}

void Parafoil3dofSimulator::inputCallback(crs_msgs::parafoil_3dof_input::ConstPtr control_input)
{
    last_input_.delta_a = control_input->delta_a;
}

void Parafoil3dofSimulator::publishStates()
{
    crs_msgs::parafoil_3dof_state msg;
    msg.x = current_state_.pos_x;
    msg.y = current_state_.pos_y;
    msg.z = current_state_.pos_z;
    msg.psi = current_state_.psi;
    msg.psi_rate = current_state_.psi_rate;
    state_pub_.publish(msg);
}
}  // namespace parafoil_simulator

Application Files

Update app/simulation_node.cpp from previous parafoil_simulator

  1. Create simulator pointer to our new simulator class e.g. parafoil_simulator::Parafoil3dofSimulator
  2. Call class methods defined above to advance the state and then publish the current state. Note that we do not need to call the inputCallback as this will be executed automatically whenever a control input is published by the controller
#include <ros/ros.h>
#include <ros_crs_utils/parameter_io.h>

#include "parafoil_simulator/parafoil_3dof_simulator.h"


int main(int argc, char** argv)
{
    ros::init(argc, argv, "simulation_node");
    ros::NodeHandle nh = ros::NodeHandle();
    ros::Rate loop_rate(10);

    auto* simulator = new parafoil_simulator::Parafoil3dofSimulator(nh);

    while (ros::ok())
    {
        simulator->advanceState(0.1);
        simulator->publishStates();

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

NOTE in future should combine into more elegant advanceSimulator shown in ros_simulator

Launch Files

Finally make sure to update the simulator/ launch file if you haven't already such that it spawns both a simulator and 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>

Admin Files

Update CmakeLists.txt

  1. Update project name
  2. If used ros_backtracker as template make sure to add find_package(casadi)
  3. Change library to add correct file from src
  4. Add simulation_node as executable
  5. Link simulation_node with casadi
cmake_minimum_required(VERSION 2.8.3)
project(parafoil_simulator)

set(CMAKE_CXX_STANDARD 17)
find_package(catkin_simple REQUIRED)
find_package(casadi)
catkin_simple()
catkin_package()

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

#############
# Libraries #
#############
cs_add_library(${PROJECT_NAME}
        src/parafoil_3dof_simulator.cpp
        )

###############
# Executables #
###############
cs_add_executable(simulation_node
app/simulation_node.cpp)
target_link_libraries(simulation_node ${PROJECT_NAME} casadi)

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

Update package.xml

  1. Update package name
  2. Update maintainer name & email if necessary
<?xml version="1.0"?>
<package format="2">
<name>parafoil_simulator</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>dynamic_models</depend>
    <depend>crs_msgs</depend>
    <depend>ros_crs_utils</depend>
    <depend>commons</depend>

    <!-- Optional Dependencies -->
    <depend>parafoil_3dof_model</depend>

    <export></export>
</package>

Review

Should have following structure:

parafoil_simulator
├── app
│   └── simulation_node.cpp
├── CMakeLists.txt
├── include
│   └── parafoil_simulator
│       └── parafoil_3dof_simulator.h
├── launch
│   └── parafoil_3dof_launch.launch
├── package.xml
└── src
    └── parafoil_3dof_simulator.cpp

Up Next

Now you have a working ROS architecture which you can launch with roslaunch! The built in simulator in CRS is really just a scaled up version of these very same concepts which allows several different models,and several different sensors. Speaking of sensors shouldn't we add those to our architecture? Yep, we should and that's what we'll do in the next tutorial!