ROS Mini Challenge #4 – Controlling a drone with ROS Actions

15/02/2020

What we are going to learn

Learn how to create a ROS Action message and compile it for C++, to control a drone.

List of resources used in this post

The ROSject of the project with the simulation and the ROS Packages: www.rosject.io/l/e80ec70/
Robot Ignite Academy courses, if you want to learn more about ROS Actions: www.robotigniteacademy.com/en/
Post in ROS Discourse where the challenge was posted: discourse.ros.org/t/rosject-of-the-week-challenge-4/11706

Opening the ROSject

Once you clicked in the link to get a copy of the ROSject,

When you click in the ROSject Link (www.rosject.io/l/e80ec70/), you will get a copy of it. You can then download it if you want to use it on our local computer, or you can just click Open to have it opened in ROSDS.
Once it’s open, you can find all the code associated with this challenge in the ~/catkin_ws folder. To see the full code, open the IDE by going to the top menu and select Tools->IDE.

Code Editor (IDE) – ROSDS

Launching the simulation

Go to the top menu and select Simulations. On the menu that appears, click on the Choose launch file… button.

Choose lunch file to open simulation in ROSDS

Now, from the launch files list, select the launch file named rotw4.launch from the drone_construct package.

ROS Mini Challenge #4 – Drone – ROSDS

Finally, click on the Launch button to start the simulation. In the end, you should get something like this:

Drone simulation in ROSDS

The problem to solve

As you can see, this ROSject contains 1 package inside its catkin_ws workspace: rotw4_code. The purpose of this packages is to allow you to control the Drone (Take Off or Land) using an Action Server.

So, the main purpose of this ROSject is to be able to send a Goal to this Action Server, specifying a keyword in the goal (either “UP” or “DOWN”), and the Drone receives and executes this goal. The steps in order to achieve this are the following:

First, you will need to compile the Action Server and the Action message used. For this, execute the following command:

cd ~/catkin_ws/
catkin_make
source ~/catkin_ws/devel/setup.bash

Once, the compilation has finished successfully, you will start the Action Server by executing the following command:

rosrun rotw4_code rotw4_action_node

Once the Action Server is up and running, you can send goals to the Action with the following commands.

In order to make the Drone TAKE OFF:

rostopic pub /rotw4_action/goal rotw4_code/ActionMsgActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  goal: 'UP'"

You should see the Drone taking off, like in the following image:

Drone taking off (moving up) in ROSDS

And in order to make the Drone LAND, you could use the following command:

rostopic pub /rotw4_action/goal rotw4_code/ActionMsgActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  goal: 'DOWN'"

You should see the Drone landing:

Drone landing (moving down) in ROSDS

Also, you will see in the Action Server a message indicating that the action was successfully executed.

Action Server succeeded in ROSDS

Ok, so… where’s the problem? If you have tried to reproduce the steps described above you have already seen that it DOES NOT WORK. When you follow the pipeline you can see that even the 1st step, which is to compile the package, it does not work (though it is not the only error here). But Why? Let’s find it in the section below.

Solving the ROS Mini Challenge

According to the instructions, we need to have a look at the rotw4_code package and try to find there the errors.

If you looked carefully, after running the first commands, you had an error:

cd ~/catkin_ws/
catkin_make

The error was:

Building CXX object rotw4_code/CMakeFiles/rotw4_action_node.dir/src/rotw4_action.cpp.o
/home/user/catkin_ws/src/rotw4_code/src/rotw4_action.cpp:3:34: fatal error: rotw4_code/ActionMsg.h: No such file or directory
compilation terminated.
rotw4_code/CMakeFiles/rotw4_action_node.dir/build.make:62: recipe for target 'rotw4_code/CMakeFiles/rotw4_action_node.dir/src/rotw4_action.cpp.o' failed
make[2]: *** [rotw4_code/CMakeFiles/rotw4_action_node.dir/src/rotw4_action.cpp.o] Error 1
CMakeFiles/Makefile2:1909: recipe for target 'rotw4_code/CMakeFiles/rotw4_action_node.dir/all' failed
make[1]: *** [rotw4_code/CMakeFiles/rotw4_action_node.dir/all] Error 2
Makefile:138: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j2 -l2" failed

The problem is that it can’t find the ActionMsg.h, so, which tell us that our ~/catkin_ws/src/rotw4_code/action/ActionMsg.action is not being compiled. Let’s then open the CMakeLists.txt file located in the rotw4_code package, to see where is the error.

If we look at the line 153 of the CMakeLists.txt file, we find the following:

add_executable(rotw4_action_node src/rotw4_action.cpp)
target_link_libraries(rotw4_action_node
${catkin_LIBRARIES}
)

we didn’t add the dependencies. To solve this compilation part, let’s add the line below right after line 153:

add_dependencies(rotw4_action_node ${rotw4_action_node_EXPORTED_TARGETS}

so, in the end, it will look like:

add_executable(rotw4_action_node src/rotw4_action.cpp)
add_dependencies(rotw4_action_node ${rotw4_action_node_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(rotw4_action_node
${catkin_LIBRARIES}
)

Let’s now try to compile our package again:

cd ~/catkin_ws/
catkin_make

Now we have another error. Although it looks similar, at least now the messages were compiled:

[ 97%] Building CXX object rotw4_code/CMakeFiles/rotw4_action_node.dir/src/rotw4_action.cpp.o
/home/user/catkin_ws/src/rotw4_code/src/rotw4_action.cpp:3:34: fatal error: rotw4_code/ActionMsg.h: No such file or directory
compilation terminated.

We can confirm the message was generated with find ~/catkin_ws/devel/ -name ActionMsg*.h:

$ find ~/catkin_ws/devel/ -name ActionMsg*.h
/home/user/catkin_ws/devel/include/rotw4_code/ActionMsgActionResult.h
/home/user/catkin_ws/devel/include/rotw4_code/ActionMsgActionFeedback.h
/home/user/catkin_ws/devel/include/rotw4_code/ActionMsgAction.h
/home/user/catkin_ws/devel/include/rotw4_code/ActionMsgFeedback.h
/home/user/catkin_ws/devel/include/rotw4_code/ActionMsgGoal.h
/home/user/catkin_ws/devel/include/rotw4_code/ActionMsgActionGoal.h
/home/user/catkin_ws/devel/include/rotw4_code/ActionMsgResult.h

As we can see, we have no file named ActionMsg.h, which is imported by rotw4_code/src/rotw4_action.cpp. Let’s then fix rotw4_action.cpp on line 3 and import ActionMsgAction.h. Now let’s try to compile again:

cd ~/catkin_ws/
catkin_make

Now we have different errors:

Scanning dependencies of target rotw4_action_node
[ 97%] Building CXX object rotw4_code/CMakeFiles/rotw4_action_node.dir/src/rotw4_action.cpp.o
/home/user/catkin_ws/src/rotw4_code/src/rotw4_action.cpp:11:33: error: 'ActionMsg' is not a member of 'rotw4_code'
   actionlib::SimpleActionServer<rotw4_code::ActionMsg> as_;
                                 ^
/home/user/catkin_ws/src/rotw4_code/src/rotw4_action.cpp:11:33: error: 'ActionMsg' is not a member of 'rotw4_code'
/home/user/catkin_ws/src/rotw4_code/src/rotw4_action.cpp:11:54: error: template argument 1 is invalid
   actionlib::SimpleActionServer<rotw4_code::ActionMsg> as_;
                                                      ^
/home/user/catkin_ws/src/rotw4_code/src/rotw4_action.cpp: In constructor 'ActionClass::ActionClass(std::__cxx11::string)':
/home/user/catkin_ws/src/rotw4_code/src/rotw4_action.cpp:29:26: error: expression list treated as compound expression in mem-initializer [-fpermissive]
         action_name_(name) {
                          ^
/home/user/catkin_ws/src/rotw4_code/src/rotw4_action.cpp:30:9: error: request for member 'start' in '((ActionClass*)this)->ActionClass::as_', which is of non-class type 'int'
     as_.start();
         ^
/home/user/catkin_ws/src/rotw4_code/src/rotw4_action.cpp: In member function 'void ActionClass::executeCB(const ActionMsgGoalConstPtr&)':
/home/user/catkin_ws/src/rotw4_code/src/rotw4_action.cpp:43:13: error: request for member 'isPreemptRequested' in '((ActionClass*)this)->ActionClass::as_', which is of non-class type 'int'
     if (as_.isPreemptRequested() || !ros::ok()) {
             ^
/home/user/catkin_ws/src/rotw4_code/src/rotw4_action.cpp:46:11: error: request for member 'setPreempted' in '((ActionClass*)this)->ActionClass::as_', which is of non-class type 'int'
       as_.setPreempted();
           ^
/home/user/catkin_ws/src/rotw4_code/src/rotw4_action.cpp:62:9: error: request for member 'publishFeedback' in '((ActionClass*)this)->ActionClass::as_', which is of non-class type 'int'
     as_.publishFeedback(feedback_);
         ^
/home/user/catkin_ws/src/rotw4_code/src/rotw4_action.cpp:68:11: error: request for member 'setSucceeded' in '((ActionClass*)this)->ActionClass::as_', which is of non-class type 'int'
       as_.setSucceeded(result_);

We have a bunch of errors, so, let’s look at the first one first, which says:

rotw4_action.cpp:11:33: error: 'ActionMsg' is not a member of 'rotw4_code' actionlib::SimpleActionServer<rotw4_code::ActionMsg> as_;

The problem is almost the same as before. Instead of using ActionMsgAction, we used ActionMsg. Let’s fix that on line 11, and try to compile again:

cd ~/catkin_ws/
catkin_make

Congratulations, now everything compiled as expected:

Scanning dependencies of target rotw4_action_node
[ 97%] Building CXX object rotw4_code/CMakeFiles/rotw4_action_node.dir/src/rotw4_action.cpp.o
[100%] Linking CXX executable /home/user/catkin_ws/devel/lib/rotw4_code/rotw4_action_node
[100%] Built target rotw4_action_node
Scanning dependencies of target rotw4_code_generate_messages
[100%] Built target rotw4_code_generate_messages

Now, let’s run our action server:

rosrun rotw4_code rotw4_action_node

Now we could try to make the robot take off by running the following command in a different web shell:

rostopic pub /rotw4_action/goal rotw4_code/ActionMsgActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  goal: 'UP'"

The only problem is that the robot doesn’t move as expected yet, although the action server says succeeded.

rotw4_action: Succeeded

If we look the file rotw4_action.cpp, in the executeCB method, everything seems to be correct.

void executeCB(const rotw4_code::ActionMsgGoalConstPtr &goal) {

    std::string upDown = goal->goal;
    takeoff_pub_ = nh_.advertise<std_msgs::Empty>("/drone/takeoff", 1000);
    land_pub_ = nh_.advertise<std_msgs::Empty>("/drone/land", 1000);

    // check that preempt has not been requested by the client
    if (as_.isPreemptRequested() || !ros::ok()) {
      ROS_INFO("%s: Preempted", action_name_.c_str());
      // set the action state to preempted
      as_.setPreempted();
      success_ = false;
    }

    if (upDown == "UP") {
      takeoff_pub_.publish(takeoff_msg_);
      feedback_.feedback = "Taking Off Drone...";
    }

    if (upDown == "DOWN") {
      land_pub_.publish(land_msg_);
      feedback_.feedback = "Landing Drone...";
    }

    // feedback_.feedback = i;
    // publish the feedback
    as_.publishFeedback(feedback_);

    if (success_) {

      ROS_INFO("%s: Succeeded", action_name_.c_str());
      // set the action state to succeeded
      as_.setSucceeded(result_);
    }
  }

The problem is that we are instantiating the topic publishers and publishing on them straight away, and the topics need some time after being defined to start being used. To solve this, let’s add a while loop to publish at least 4 times, once a second:

void executeCB(const rotw4_code::ActionMsgGoalConstPtr &goal) {

    std::string upDown = goal->goal;
    takeoff_pub_ = nh_.advertise<std_msgs::Empty>("/drone/takeoff", 1000);
    land_pub_ = nh_.advertise<std_msgs::Empty>("/drone/land", 1000);
    ros::Rate rate(1);

    // check that preempt has not been requested by the client
    if (as_.isPreemptRequested() || !ros::ok()) {
      ROS_INFO("%s: Preempted", action_name_.c_str());
      // set the action state to preempted
      as_.setPreempted();
      success_ = false;
    }

    if (upDown == "UP") {
      int i = 0;
      while (i < 4) {
        takeoff_pub_.publish(takeoff_msg_);
        feedback_.feedback = "Taking Off Drone...";
        i++;
        rate.sleep();
      }
    }

    if (upDown == "DOWN") {
      land_pub_.publish(land_msg_);
      feedback_.feedback = "Landing Drone...";
    }

    // feedback_.feedback = i;
    // publish the feedback
    as_.publishFeedback(feedback_);

    if (success_) {

      ROS_INFO("%s: Succeeded", action_name_.c_str());
      // set the action state to succeeded
      as_.setSucceeded(result_);
    }
  }

If we now compile the code again

cd ~/catkin_ws/
catkin_make

run the node:

rosrun rotw4_code rotw4_action_node

and publish the message to take off again:

rostopic pub /rotw4_action/goal rotw4_code/ActionMsgActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  goal: 'UP'"

now the robot should work as expected.

Youtube video

So this is the post for today. Remember that we have the live version of this post on YouTube. If you liked the content, please consider subscribing to our youtube channel. We are publishing new content ~every day.

Keep pushing your ROS Learning.

Topics: