Running tasks parallel to an action

Unit 8, Exercise 8.6: I am trying to make the drone takeoff, move, and land at a set interval while the action is happening. I am not quite understanding the behavior of the drone in response to the following code that I wrote. First, if I put the nested For loops as shown, the drone will repeatedly takeoff, move around, and land, but at the end although the print statement executes and says landed, the drone remains moving around no matter what goal time sent. What is happening here?

Second, if I comment out the nest For loops and leave the inside statements as they are, the drone will not do anything the first iteration of the while loop, but will execute the remaining iterations except for the last one where again, it does not land although the output says it landed. Why doesn’t it do anything the first iteration?

Lastly, I can make the drone takeoff, move, and land from the command line by publishing once each to the respective topics. The only restriction seems to be that the drone has to have taken off before I can move or land it which makes sense. Since it takes just one command from the command line to, say, takeoff, why does it take a loop in the cpp and constantly publishing a few times to do the same thing? I am not talking about only getstatus() loop which I understand is needed to do things in parallel to an action. It seems even if I make the drone takeoff before the action goes active, I need to put it in a loop. Why so?
image

Hey Janak,

A lot of information here, let’s take it one by one. Please check the following:

  1. What kind of message is in cmdVel? It should be of type “Empty” for takeoff and land. I asked as I didn’t see that part of your code.
  2. Publishing to a publisher does not work if it’s not yet “ready”. This usually happens if you try to publish immediately after creating the publisher.

Have a look at that and then let’s know what’s left.

@bayodesegun Thank you very much for your response. To answer your first comment/question, I did have an “Empty” type message for takeoff and land. Your second comment made a lot of sense, and I implemented your solution to ensure there is at least one subscriber available before publishing. It solved half of my problem: the issue of the drone not taking off in the first iteration. However, for a goal of 10 seconds, it still says the drone landed, although it has not. I understand as soon as the action is complete, the while loop is exited, but how come the publishing does not happen although the print statement after it is executed? I have pasted the updated code below for your perusal.

#include <actionlib/client/simple_action_client.h>
#include <ardrone_as/ArdroneAction.h>
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <std_msgs/Empty.h>

int nImage = 0;

// action done
void doneCb(const actionlib::SimpleClientGoalState &state,
            const ardrone_as::ArdroneResultConstPtr &result) {
  ROS_INFO("The action has been completed");
  ros::shutdown();
}

// action active
void activeCb() { ROS_INFO("Goal just went active"); }

// action feedback
void feedbackCb(const ardrone_as::ArdroneFeedbackConstPtr &feedback) {
  ROS_INFO("[Feedback] image n.%d received", nImage);
  ++nImage;
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "drone_action_client");
  ros::NodeHandle nh;

  // declare and initialize action client and wait for server
  actionlib::SimpleActionClient<ardrone_as::ArdroneAction> client(
      "ardrone_action_server", true);
  client.waitForServer();

  // declare and initialize goal and send it to server
  ardrone_as::ArdroneGoal goal;
  goal.nseconds = 10;
  client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);

  // Portion to make the drone takeoff, move around, and land while action is
  // happening
  // declaring and initalizing publishers
  ros::Publisher takeoffPub =
      nh.advertise<std_msgs::Empty>("/drone/takeoff", 1000);
  ros::Publisher landPub = nh.advertise<std_msgs::Empty>("/drone/land", 1000);
  ros::Publisher movePub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1000);
  ros::Rate loop_rate(2);

  std_msgs::Empty cmdVel;          // empty message for takeoff and land topics
  geometry_msgs::Twist cmdVelMove; // message to move drone
  cmdVelMove.linear.x = 0.5;
  cmdVelMove.angular.z = 0.5;

  // While action is active or pending, make the drone move
  while (client.getState() == actionlib::SimpleClientGoalState::ACTIVE ||
         client.getState() == actionlib::SimpleClientGoalState::PENDING) {

    // takeoff portion
    while (takeoffPub.getNumSubscribers() <
           1) { // wait until at least one subscriber is available
      std::cout << "Waiting to takeoff" << std::endl;
      loop_rate.sleep();
      if (client.getState() != actionlib::SimpleClientGoalState::ACTIVE ||
          client.getState() != actionlib::SimpleClientGoalState::PENDING) {
        break;
      }
    }
    takeoffPub.publish(cmdVel);
    std::cout << "Took off" << std::endl;
    loop_rate.sleep();

    // move portion
    while (movePub.getNumSubscribers() < 1) {
      std::cout << "Waiting to move" << std::endl;
      loop_rate.sleep();
      if (client.getState() != actionlib::SimpleClientGoalState::ACTIVE ||
          client.getState() != actionlib::SimpleClientGoalState::PENDING) {
        break;
      }
    }
    movePub.publish(cmdVelMove);
    std::cout << "Moving" << std::endl;
    loop_rate.sleep();

    // hover portion
    cmdVelMove.linear.x = 0.0;
    cmdVelMove.angular.x = 0.0;
    while (movePub.getNumSubscribers() < 1) {
      std::cout << "Waiting to hover" << std::endl;
      loop_rate.sleep();
      if (client.getState() != actionlib::SimpleClientGoalState::ACTIVE ||
          client.getState() != actionlib::SimpleClientGoalState::PENDING) {
        break;
      }
    }
    movePub.publish(cmdVelMove);
    std::cout << "Hovering" << std::endl;
    loop_rate.sleep();

    // land portion
    while (landPub.getNumSubscribers() < 1) {
      std::cout << "Waiting to land" << std::endl;
      loop_rate.sleep();
      if (client.getState() != actionlib::SimpleClientGoalState::ACTIVE ||
          client.getState() != actionlib::SimpleClientGoalState::PENDING) {
        break;
      }
    }
    landPub.publish(cmdVel);
    std::cout << "Landed" << std::endl;
    loop_rate.sleep();
  }

  return 0;
}

Please remove this condition from the loops waiting for the publishers to be ready. It can cause unexpected behaviors like this. The only condition on which the loop should break is that the publisher is ready.

Also, for better code comprehension, you might want to rename your empty message variable to something like emptyMsg or so, so that it won’t be confused with cmdVelMove (I was confused at first until my colleague @roalgoal pointed it out).

@bayodesegun and @roalgoal , thank you for the feedback. However, without those if and break statements, it gets stuck in the loop when the action finishes. How can I make it exit the loop without it? Also, my previous question, for a goal of 10 seconds, why doesn’t it land although the print statement after the landPub.publish statement executes? Everything else is making sense to me except for that.

Which loop gets stuck? Find out why it’s getting stuck and fix it in a sustainable way.

You cannot fix a problem by creating another problem. Don’t any other condition to the loop waiting for the publisher, as I have said.

Have you considered the fact that the publisher might not be ready because that loop just before the land statement exited before the publisher was ready (due to the other condition you added), so that what you published just went into oblivion?

@bayodesegun , let me clarify I am talking about the case where I have removed the conditions inside the subscriber-checking loops. In the end, it always gets stuck in one of those loops, for instance, for a goal of 12 seconds, it gets stuck “waiting to hover”. What do you suggest is an example of a sustainable way to exit it when the action finishes? Also, could you please explain why having conditions and the break statement inside those loops is a problem? You said it can cause unexpected behavior, but I don’t understand how. The loop would be broken only if it is waiting for a subscriber, but action has finished, right? I’m new and just trying to understand. So, please bear with me.

If any of those loops get stuck, it simply means the publisher was not ready or has become “unready”! This is the problem you need to solve. And that explains why the drone would not land, because the publisher was not ready.

Even if the publisher is not ready, the loop would be broken if those extra conditions evaluate to true. Again I say, you cannot put any extra conditions in those loops. If they are getting stuck, find out why.