How to send multiple goals with actionlib and C++

asked 2020-09-10 13:42:21 -0500

Marcus Barnet gravatar image

updated 2020-09-10 13:56:55 -0500

I would like to be able to send multiple goals to my robot. I developed a simple base planner in C++ that reads the odometry for the robot and try to move it towards a goal P(x,y). I use ROS Melodic and Ubuntu 18.04.

For the moment, I'm using two arrays to load the coordinates of each Point and then a for() loop to cycle on all of them in this way:

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <beginner_tutorials/PointAction.h>

int main (int argc, char **argv)
  ros::init(argc, argv, "planner");
    double x [2] = {20, 25}; 
    double y [2] = {30, 35};
    int length = sizeof(x) / sizeof(x[0]);
  // create the action client
  // true causes the client to spin its own thread
  actionlib::SimpleActionClient<beginner_tutorials::PointAction> ac("planner", true);
  ROS_INFO("Waiting for action server to start.");
  // wait for the action server to start
  ac.waitForServer(); //will wait for infinite time

  ROS_INFO("Action server started, sending goal.");
  // send a goal to the action
  beginner_tutorials::PointGoal goal;

  for( int i = 0; i < length; i++ ) {

      goal.x = x[i];
      goal.y = y[i];
      ROS_INFO("Sending goal %d, point: X: %f, Y: %f", i, x[i], y[i]);

      bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));

      if (finished_before_timeout)
      actionlib::SimpleClientGoalState state = ac.getState();
      ROS_INFO("Action finished: %s",state.toString().c_str());
          ROS_INFO("Action did not finish before the time out.");

  //wait for the action to return

  return 0;

It surely works, but is there any other smarter way or solution to do the same? Could I use a 2D array instead to use three arrays?

Thank you!

edit retag flag offensive close merge delete


I'm not quite sure if I completely understand what you want to do, but this seems to be a simple C++ question?

Can't you use a std::vector<beginner_tutorials::PointGoal> goals and loop over that?

mgruhler gravatar image mgruhler  ( 2020-09-11 00:44:11 -0500 )edit