Rostest and move_base actionlib

edit

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