Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <ros/ros.h>
00031 #include <actionlib/client/simple_action_client.h>
00032 #include <move_base_msgs/MoveBaseAction.h>
00033 #include <planner_cspace_msgs/PlannerStatus.h>
00034
00035 #include <gtest/gtest.h>
00036
00037 class PreemptTest : public ::testing::Test
00038 {
00039 public:
00040 PreemptTest()
00041 : node_()
00042 {
00043 move_base_ = std::make_shared<ActionClient>("/move_base");
00044 status_sub_ = node_.subscribe("/planner_3d/status",
00045 10,
00046 &PreemptTest::StatusCallback,
00047 this);
00048 if (!move_base_->waitForServer(ros::Duration(30.0)))
00049 {
00050 ROS_ERROR("Failed to connect move_base action");
00051 exit(EXIT_FAILURE);
00052 }
00053 }
00054 ~PreemptTest()
00055 {
00056 }
00057
00058 protected:
00059 using ActionClient = actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>;
00060 using ActionClientPtr = std::shared_ptr<ActionClient>;
00061
00062 void StatusCallback(const planner_cspace_msgs::PlannerStatus& msg)
00063 {
00064 status_ = msg;
00065 }
00066 move_base_msgs::MoveBaseGoal CreateGoalInFree()
00067 {
00068 move_base_msgs::MoveBaseGoal goal;
00069 goal.target_pose.header.stamp = ros::Time::now();
00070 goal.target_pose.header.frame_id = "map";
00071 goal.target_pose.pose.position.x = 1.24;
00072 goal.target_pose.pose.position.y = 0.65;
00073 goal.target_pose.pose.position.z = 0.0;
00074 goal.target_pose.pose.orientation.x = 0.0;
00075 goal.target_pose.pose.orientation.y = 0.0;
00076 goal.target_pose.pose.orientation.z = 0.0;
00077 goal.target_pose.pose.orientation.w = 1.0;
00078 return goal;
00079 }
00080
00081 ros::NodeHandle node_;
00082 ros::Subscriber status_sub_;
00083 ActionClientPtr move_base_;
00084 planner_cspace_msgs::PlannerStatus status_;
00085 };
00086
00087 TEST_F(PreemptTest, Preempt)
00088 {
00089 move_base_->sendGoal(CreateGoalInFree());
00090 while (move_base_->getState().state_ !=
00091 actionlib::SimpleClientGoalState::ACTIVE)
00092 {
00093 ros::Duration(1.0).sleep();
00094 }
00095 while (move_base_->getState().state_ ==
00096 actionlib::SimpleClientGoalState::ACTIVE)
00097 {
00098 move_base_->cancelAllGoals();
00099 ros::Duration(1.0).sleep();
00100 }
00101
00102 ASSERT_EQ(actionlib::SimpleClientGoalState::PREEMPTED,
00103 move_base_->getState().state_);
00104 ASSERT_EQ(planner_cspace_msgs::PlannerStatus::GOING_WELL,
00105 status_.error);
00106 ASSERT_EQ(planner_cspace_msgs::PlannerStatus::DONE,
00107 status_.status);
00108 }
00109
00110 int main(int argc, char** argv)
00111 {
00112 testing::InitGoogleTest(&argc, argv);
00113 ros::init(argc, argv, "test_preempt");
00114
00115 ros::AsyncSpinner spinner(1);
00116 spinner.start();
00117 int ret = RUN_ALL_TESTS();
00118 spinner.stop();
00119 ros::shutdown();
00120 return ret;
00121 }