Rostest and move_base actionlib

Question

Hi

I have a package which relies on the move_base action server interface. Is it possible to mock the move_base server in rostest/gtest, so i can perform unittests? I have tried creating a simple action server in the test, as

class MoveBaseAction
{
protected:
  ros::NodeHandle nh_;
  typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;
  MoveBaseActionServer * as_; // NodeHandle instance must be created before this line. Otherwise strange error occurs.
  std::string action_name_;
  // create messages that are used to published feedback/result
  move_base_msgs::MoveBaseFeedback feedback_;
  move_base_msgs::MoveBaseResult result_;

public:

  MoveBaseAction(std::string name) :
    action_name_(name)
  {
    ROS_ERROR("Starting server...");
    //as_ = new actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction>(nh_, name, boost::bind(&MoveBaseAction::executeCB, this, _1), false),
    as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBaseAction::executeCB, this, _1), false);
    as_->registerGoalCallback(boost::bind(&MoveBaseAction::goalCB, this));
    as_->start();
  }

  ~MoveBaseAction(void)
  {
  }
  void goalCB(){

  }

  void executeCB(const move_base_msgs::MoveBaseGoalConstPtr &goal)
  {
    // helper variables
    ros::Rate r(1);
    bool success = true;

    // push_back the seeds for the fibonacci sequence
    move_base_msgs::MoveBaseFeedback feedback_;
    double N = 100;
    double dx = goal->target_pose.pose.position.x/N;
    double dy = goal->target_pose.pose.position.y/N;
    fprintf(stderr,"goal is (%f,%f)", goal->target_pose.pose.position.x,goal->target_pose.pose.position.y);
    // start executing the action
    for(int i = 1; i <= N; i++) {
      // check that preempt has not been requested by the client
      if (as_->isPreemptRequested() || !ros::ok()) {
        printf("%s: Preempted", action_name_.c_str());
        // set the action state to preempted
        as_->setPreempted();
        success = false;
        break;
      }

      feedback_.base_position.pose.position.x = dx*i;
      feedback_.base_position.pose.position.y = dy*i;
      fprintf(stderr,"Progress is (%f,%f)", feedback_.base_position.pose.position.x, feedback_.base_position.pose.position.y);

      // publish the feedback
      as_->publishFeedback(feedback_);
      // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
      r.sleep();
    }

    if(success) {
      fprintf(stderr,"%s: Succeeded", action_name_.c_str());
      // set the action state to succeeded
      as_->setSucceeded(result_);
    }
  }
};

And then use the class in the test as

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
MoveBaseAction as("move_base");
MoveBaseClient ac_("move_base", true);

//wait for the action server to come up
while(!ac_.waitForServer(ros::Duration(1.0)) && ros::ok()){
  ros::spinOnce();
  ROS_ERROR("Waiting for the move_base action server");
}

But it hangs in the waitForServer loop. I think it have something to do with the fact i am not spinning ros, but i cannot figure out a proper way to do it. I would like to able to test my own class, which works as an action client to move_base. Regards

Christian

---

Originally posted by FOXTER on ROS Answers with karma: 153 on 2017-04-05

Post score: 0

Answer 1

I think you are abit confused about how you would want to write your test. There are 2 ways to write test in your case:

1. Library level test: since you can create an MoveBaseAction object, you can just you can access all the class attributes and functions (after add the FRIEND_TEST to header file, of course). So you dont have to use waitForServer , sendGoal , etc. Instead, you can just create a MoveBaseGoal object and call the execute function directly. After that, you can just compare the outcome of result_ object.
2. Node level test: you can create a proper node (something similar to https://github.com/ros-planning/navigation/blob/indigo-devel/move_base/src/move_base_node.cpp). Then you can launch this node during your test. In this case, you can use your test node as action client (like http://wiki.ros.org/actionlib_tutorials/Tutorials/SimpleActionClient)

---

Originally posted by DavidN with karma: 544 on 2017-04-05

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by FOXTER on 2017-04-05:
Yes thank you!. I Tried approach number 2, and basically put the class I attached into a node by it self, and launching it through the rostest launch file, and it works perfectly.