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 <actionlib/client/simple_action_client.h>
00031 #include <gtest/gtest.h>
00032 #include <move_base_msgs/MoveBaseAction.h>
00033 #include <planner_cspace_msgs/PlannerStatus.h>
00034 #include <ros/ros.h>
00035 #include <memory>
00036
00037 class AbortTest : public ::testing::Test
00038 {
00039 public:
00040 AbortTest()
00041 : node_()
00042 {
00043 move_base_ = std::make_shared<ActionClient>("/move_base");
00044 status_sub_ = node_.subscribe("/planner_3d/status",
00045 10,
00046 &AbortTest::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 ~AbortTest()
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 CreateGoalInRock()
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.19;
00072 goal.target_pose.pose.position.y = 1.90;
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 move_base_msgs::MoveBaseGoal CreateGoalInFree()
00081 {
00082 move_base_msgs::MoveBaseGoal goal;
00083 goal.target_pose.header.stamp = ros::Time::now();
00084 goal.target_pose.header.frame_id = "map";
00085 goal.target_pose.pose.position.x = 1.24;
00086 goal.target_pose.pose.position.y = 0.65;
00087 goal.target_pose.pose.position.z = 0.0;
00088 goal.target_pose.pose.orientation.x = 0.0;
00089 goal.target_pose.pose.orientation.y = 0.0;
00090 goal.target_pose.pose.orientation.z = 0.0;
00091 goal.target_pose.pose.orientation.w = 1.0;
00092 return goal;
00093 }
00094
00095 ros::NodeHandle node_;
00096 ros::Subscriber status_sub_;
00097 ActionClientPtr move_base_;
00098 planner_cspace_msgs::PlannerStatus status_;
00099 };
00100
00101 TEST_F(AbortTest, AbortByGoalInRock)
00102 {
00103
00104 move_base_->sendGoal(CreateGoalInRock());
00105 while (move_base_->getState().state_ !=
00106 actionlib::SimpleClientGoalState::ACTIVE)
00107 {
00108 ros::Duration(1.0).sleep();
00109 }
00110
00111
00112 while (move_base_->getState().state_ ==
00113 actionlib::SimpleClientGoalState::ACTIVE)
00114 {
00115 ros::Duration(1.0).sleep();
00116 }
00117 ros::Duration(1.0).sleep();
00118
00119
00120 ASSERT_EQ(actionlib::SimpleClientGoalState::ABORTED,
00121 move_base_->getState().state_);
00122 ASSERT_EQ(planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND,
00123 status_.error);
00124
00125
00126 move_base_->sendGoal(CreateGoalInFree());
00127 while (move_base_->getState().state_ !=
00128 actionlib::SimpleClientGoalState::ACTIVE)
00129 {
00130 ros::Duration(1.0).sleep();
00131 }
00132 while (move_base_->getState().state_ ==
00133 actionlib::SimpleClientGoalState::ACTIVE)
00134 {
00135 ros::Duration(1.0).sleep();
00136 }
00137 ros::Duration(1.0).sleep();
00138
00139
00140 ASSERT_EQ(actionlib::SimpleClientGoalState::SUCCEEDED,
00141 move_base_->getState().state_);
00142 ASSERT_EQ(planner_cspace_msgs::PlannerStatus::GOING_WELL,
00143 status_.error);
00144 }
00145
00146 int main(int argc, char** argv)
00147 {
00148 testing::InitGoogleTest(&argc, argv);
00149 ros::init(argc, argv, "test_abort");
00150
00151 ros::AsyncSpinner spinner(1);
00152 spinner.start();
00153 int ret = RUN_ALL_TESTS();
00154 spinner.stop();
00155 ros::shutdown();
00156 return ret;
00157 }