test/planner_action.cpp

Go to the documentation of this file.

#include <gtest/gtest.h>
#include <gmock/gmock.h>

#include <actionlib/client/simple_action_client.h>
#include <actionlib/server/simple_action_server.h>
#include <mbf_abstract_core/abstract_planner.h>
#include <mbf_msgs/GetPathAction.h>

#include <mbf_abstract_nav/planner_action.h>
#include <mbf_abstract_nav/abstract_planner_execution.h>

#include <boost/chrono.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/condition.hpp>

#include <string>

using namespace mbf_abstract_nav;

// in this test we will use an action server and an action client, since
// we cannot mock the goal-handle.

using geometry_msgs::PoseStamped;
using mbf_abstract_core::AbstractPlanner;

// a mocked planner, allowing to control the outcome
struct MockPlanner : public AbstractPlanner
{
  MOCK_METHOD6(makePlan, uint32_t(const PoseStamped&, const PoseStamped&, double, std::vector<PoseStamped>&, double&,
                                  std::string&));

  // the cancel call is for now uninteresting. if this changes, we can also mock it.
  bool cancel()
  {
    return true;
  }
};

using mbf_abstract_nav::AbstractPlannerExecution;
using mbf_msgs::GetPathAction;

using testing::_;
using testing::DoAll;
using testing::Eq;
using testing::Field;
using testing::Return;
using testing::SetArgReferee;
using testing::Test;

// a mocked action server
struct MockedActionServer : public actionlib::ActionServerBase<GetPathAction>
{
  // define the types we are using
  typedef actionlib::ServerGoalHandle<GetPathAction> GoalHandle;
  typedef actionlib::ActionServerBase<GetPathAction> ActionServerBase;

  MockedActionServer(boost::function<void(GoalHandle)> goal_cb, boost::function<void(GoalHandle)> cancel_cb)
    : ActionServerBase(goal_cb, cancel_cb, true)
  {
  }

  // the mocked method
  MOCK_METHOD2(publishResult, void(const actionlib_msgs::GoalStatus&, const Result&));

  // methods below are not required for now but might be mocked in the future
  virtual void initialize()
  {
  }

  void publishFeedback(const actionlib_msgs::GoalStatus&, const Feedback&)
  {
  }

  void publishStatus()
  {
  }
};

// action which will trigger our condition variable, so we can wait until the runImpl thread reaches makePlan
ACTION_P(Notify, cv)
{
  cv->notify_all();
}

// test-fixture
struct PlannerActionFixture : public Test
{
  boost::shared_ptr<MockPlanner> planner;     
  boost::condition_variable done_condition_;  
  MoveBaseFlexConfig config;                  

  std::string action_name;
  TFPtr tf;

  // todo change this from const ref
  std::string global_frame;
  std::string local_frame;
  ros::Duration dur;
  mbf_utility::RobotInformation robot_info;

  PlannerAction planner_action;
  ros::NodeHandle nh;

  MockedActionServer action_server;

  mbf_msgs::GetPathResult expected_result;
  mbf_msgs::GetPathActionGoalPtr goal;

  PlannerActionFixture()
    : planner(new MockPlanner())
    , action_name("action_name")
    , tf(new TF())
    , global_frame("global_frame")
    , local_frame("local_frame")
    , dur(0)
    , goal(new mbf_msgs::GetPathActionGoal())
    , robot_info(*tf, global_frame, local_frame, dur)
    , planner_action(action_name, robot_info)
    , action_server(boost::bind(&PlannerActionFixture::callAction, this, _1),
                    boost::bind(&PlannerActionFixture::cancelAction, this, _1))
  {
    tf->setUsingDedicatedThread(true);
    config.planner_patience = 0;
  }

  void TearDown()
  {
    // call the server - this is where the actual test call happens
    action_server.goalCallback(goal);

    // wait until the cv gets triggered
    boost::mutex m;
    boost::unique_lock<boost::mutex> lock(m);
    done_condition_.wait_for(lock, boost::chrono::seconds(1));
  }

  void callAction(MockedActionServer::GoalHandle goal_handle)
  {
    planner_action.start(goal_handle, boost::make_shared<AbstractPlannerExecution>("plugin", planner, tf, config));
  }

  void cancelAction(MockedActionServer::GoalHandle goal_handle)
  {
    planner_action.cancel(goal_handle);
  }
};

TEST_F(PlannerActionFixture, emptyPath)
{
  // goal with frames, so we can pass tf-lookup
  goal->goal.use_start_pose = true;
  goal->goal.start_pose.header.frame_id = goal->goal.target_pose.header.frame_id = global_frame;

  // setup the expectation
  // we dont return anything here, so the outcome should be empty path
  EXPECT_CALL(*planner, makePlan(_, _, _, _, _, _)).WillOnce(Return(0));
  EXPECT_CALL(action_server,
              publishResult(_, Field(&mbf_msgs::GetPathResult::outcome, Eq(mbf_msgs::GetPathResult::EMPTY_PATH))))
      .Times(1)
      .WillOnce(Notify(&done_condition_));
}

#if !ROS_VERSION_MINIMUM(1, 14, 0)
// disable the test on kinetic and lunar, until I figure out how to transform
// from global_frame into global_frame in the tf framework
TEST_F(PlannerActionFixture, DISABLED_success)
#else
TEST_F(PlannerActionFixture, success)
#endif
{
  // create a dummy path
  std::vector<geometry_msgs::PoseStamped> path(10);
  // set the frame such that we can skip tf
  for (size_t ii = 0; ii != path.size(); ++ii)
  {
    path[ii].header.frame_id = global_frame;
    path[ii].pose.orientation.w = 1;
  }

  // goal with frames, so we can pass tf-lookup
  goal->goal.use_start_pose = true;
  goal->goal.start_pose.header.frame_id = goal->goal.target_pose.header.frame_id = global_frame;

  // setup the expectation
  EXPECT_CALL(*planner, makePlan(_, _, _, _, _, _)).WillOnce(DoAll(SetArgReferee<3>(path), Return(0)));
  EXPECT_CALL(action_server,
              publishResult(_, Field(&mbf_msgs::GetPathResult::outcome, Eq(mbf_msgs::GetPathResult::SUCCESS))))
      .Times(1)
      .WillOnce(Notify(&done_condition_));
}

TEST_F(PlannerActionFixture, tfError)
{
  // create a dummy path
  std::vector<geometry_msgs::PoseStamped> path(10);
  // set the frame such that we fail at the tf
  for (size_t ii = 0; ii != path.size(); ++ii)
    path[ii].header.frame_id = "unknown";

  // setup the expectation - we succeed here
  EXPECT_CALL(*planner, makePlan(_, _, _, _, _, _)).WillOnce(DoAll(SetArgReferee<3>(path), Return(0)));
  EXPECT_CALL(action_server,
              publishResult(_, Field(&mbf_msgs::GetPathResult::outcome, Eq(mbf_msgs::GetPathResult::TF_ERROR))))
      .Times(1)
      .WillOnce(Notify(&done_condition_));

  // goal with frames, so we can pass tf-lookup
  goal->goal.use_start_pose = true;
  goal->goal.start_pose.header.frame_id = goal->goal.target_pose.header.frame_id = global_frame;
}

TEST_F(PlannerActionFixture, noRobotPose)
{
  // test case where we fail to get a valid robot pose.
  // in this case we will receive a TF_ERROR from the server.

  // make us use the robot pose
  goal->goal.use_start_pose = false;
  goal->goal.start_pose.header.frame_id = goal->goal.target_pose.header.frame_id = global_frame;

  // set the expectation
  EXPECT_CALL(action_server,
              publishResult(_, Field(&mbf_msgs::GetPathResult::outcome, Eq(mbf_msgs::GetPathResult::TF_ERROR))))
      .Times(1)
      .WillOnce(Notify(&done_condition_));
}

TEST_F(PlannerActionFixture, noPathFound)
{
  // test case where the planner fails.
  // in this case we will receive NO_PLAN_FOUND from the server.
  config.planner_max_retries = 0;

  // valid goal
  goal->goal.use_start_pose = true;
  goal->goal.start_pose.header.frame_id = goal->goal.target_pose.header.frame_id = global_frame;

  // set the expectation: the planner retuns a failure
  EXPECT_CALL(*planner, makePlan(_, _, _, _, _, _)).WillOnce(Return(mbf_msgs::GetPathResult::NO_PATH_FOUND));
  EXPECT_CALL(action_server,
              publishResult(_, Field(&mbf_msgs::GetPathResult::outcome, Eq(mbf_msgs::GetPathResult::NO_PATH_FOUND))))
      .Times(1)
      .WillOnce(Notify(&done_condition_));
}

ACTION(SleepAndFail)
{
  boost::this_thread::sleep_for(boost::chrono::milliseconds(10));
  return 11;
}

TEST_F(PlannerActionFixture, patExceeded)
{
  // test case where the planner fails multple times and we are out of patience

  // setup the config; this will be passed to the execution
  config.planner_max_retries = 5;
  config.planner_patience = 0.001;

  // valid goal
  goal->goal.use_start_pose = true;
  goal->goal.start_pose.header.frame_id = goal->goal.target_pose.header.frame_id = global_frame;

  // set the expectation: the planner retuns a failure
  EXPECT_CALL(*planner, makePlan(_, _, _, _, _, _)).WillRepeatedly(SleepAndFail());
  EXPECT_CALL(action_server,
              publishResult(_, Field(&mbf_msgs::GetPathResult::outcome, Eq(mbf_msgs::GetPathResult::PAT_EXCEEDED))))
      .Times(1)
      .WillOnce(Notify(&done_condition_));
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "read_types");
  ros::NodeHandle nh;
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}