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 <string>
00031
00032 #include <ros/ros.h>
00033
00034 #include <diagnostic_msgs/DiagnosticArray.h>
00035 #include <geometry_msgs/PoseStamped.h>
00036 #include <nav_msgs/Path.h>
00037
00038 #include <costmap_cspace_msgs/CSpace3DUpdate.h>
00039 #include <planner_cspace_msgs/PlannerStatus.h>
00040
00041 #include <gtest/gtest.h>
00042
00043 TEST(Planner3D, CostmapWatchdog)
00044 {
00045 int cnt = 0;
00046 planner_cspace_msgs::PlannerStatus::ConstPtr status;
00047 nav_msgs::Path::ConstPtr path;
00048 diagnostic_msgs::DiagnosticArray::ConstPtr diag;
00049
00050 const boost::function<void(const planner_cspace_msgs::PlannerStatus::ConstPtr&)> cb_status =
00051 [&status, &cnt](const planner_cspace_msgs::PlannerStatus::ConstPtr& msg) -> void
00052 {
00053 status = msg;
00054 cnt++;
00055 };
00056 const boost::function<void(const nav_msgs::Path::ConstPtr&)> cb_path =
00057 [&path](const nav_msgs::Path::ConstPtr& msg) -> void
00058 {
00059 path = msg;
00060 };
00061 const boost::function<void(const diagnostic_msgs::DiagnosticArray::ConstPtr&)> cb_diag =
00062 [&diag](const diagnostic_msgs::DiagnosticArray::ConstPtr& msg) -> void
00063 {
00064 diag = msg;
00065 };
00066
00067 ros::NodeHandle nh("");
00068 ros::Publisher pub_goal = nh.advertise<geometry_msgs::PoseStamped>("goal", 1, true);
00069 ros::Publisher pub_cost_update = nh.advertise<costmap_cspace_msgs::CSpace3DUpdate>("costmap_update", 1);
00070 ros::Subscriber sub_status = nh.subscribe("planner_3d/status", 1, cb_status);
00071 ros::Subscriber sub_path = nh.subscribe("path", 1, cb_path);
00072 ros::Subscriber sub_diag = nh.subscribe("diagnostics", 1, cb_diag);
00073
00074 geometry_msgs::PoseStamped goal;
00075 goal.header.frame_id = "map";
00076 goal.pose.position.x = 1.9;
00077 goal.pose.position.y = 2.8;
00078 goal.pose.orientation.w = 1.0;
00079 pub_goal.publish(goal);
00080
00081 ros::Rate rate(10);
00082 while (ros::ok())
00083 {
00084
00085 if (cnt == 0 || cnt > 8)
00086 {
00087 costmap_cspace_msgs::CSpace3DUpdate update;
00088 update.header.stamp = ros::Time::now();
00089 update.header.frame_id = "map";
00090 update.width = update.height = update.angle = 0;
00091 pub_cost_update.publish(update);
00092 }
00093
00094 ros::spinOnce();
00095 rate.sleep();
00096
00097 if (!status)
00098 continue;
00099
00100 if (5 < cnt && cnt < 8)
00101 {
00102 ASSERT_EQ(status->error, planner_cspace_msgs::PlannerStatus::DATA_MISSING);
00103
00104 ASSERT_TRUE(static_cast<bool>(path));
00105 ASSERT_EQ(path->poses.size(), 0u);
00106
00107 ASSERT_TRUE(static_cast<bool>(diag));
00108 ASSERT_EQ(diag->status.size(), 1u);
00109 ASSERT_EQ(diag->status[0].level, diagnostic_msgs::DiagnosticStatus::ERROR);
00110 ASSERT_NE(diag->status[0].message.find("missing"), std::string::npos);
00111 }
00112 else if (10 < cnt && cnt < 13)
00113 {
00114 ASSERT_EQ(status->status, planner_cspace_msgs::PlannerStatus::DOING);
00115 ASSERT_EQ(status->error, planner_cspace_msgs::PlannerStatus::GOING_WELL);
00116
00117 ASSERT_EQ(diag->status.size(), 1u);
00118 ASSERT_EQ(diag->status[0].level, diagnostic_msgs::DiagnosticStatus::OK);
00119 ASSERT_NE(diag->status[0].message.find("well"), std::string::npos);
00120 }
00121 else if (cnt >= 13)
00122 {
00123 return;
00124 }
00125 }
00126 ASSERT_TRUE(ros::ok());
00127 }
00128
00129 int main(int argc, char** argv)
00130 {
00131 testing::InitGoogleTest(&argc, argv);
00132 ros::init(argc, argv, "test_navigate");
00133
00134 return RUN_ALL_TESTS();
00135 }