37 #include <global_planner_tests/easy_costmap.h> 38 #include <global_planner_tests/global_planner_tests.h> 41 #include <gtest/gtest.h> 46 const char map_path[] =
"package://dlux_plugins/test/robert_frost.png";
60 bool pathsEqual(
const nav_2d_msgs::Path2D& path_a,
const nav_2d_msgs::Path2D& path_b)
62 if (path_a.header.frame_id != path_b.header.frame_id || path_a.poses.size() != path_b.poses.size())
66 for (
unsigned int i=0; i < path_a.poses.size(); i++)
68 if (path_a.poses[i].x != path_b.poses[i].x || path_a.poses[i].y != path_b.poses[i].y
69 || path_a.poses[i].theta != path_b.poses[i].theta)
86 bool path_caching =
false,
double improvement_threshold = -1.0)
89 std::shared_ptr<global_planner_tests::EasyCostmap> easy_costmap =
90 std::make_shared<global_planner_tests::EasyCostmap>(std::string(
map_path), 1.0);
94 private_nh.
setParam(
"path_caching", path_caching);
95 private_nh.
setParam(
"improvement_threshold", improvement_threshold);
96 private_nh.
setParam(
"potential_calculator",
"dlux_plugins::Dijkstra");
97 private_nh.
setParam(
"traceback",
"dlux_plugins::GridPath");
98 private_nh.
setParam(
"print_statistics",
true);
99 global_planner.
initialize(nh, ns, tf, easy_costmap);
104 nav_2d_msgs::Pose2DStamped start;
105 nav_2d_msgs::Pose2DStamped goal;
107 start.header.frame_id = info.
frame_id;
108 gridToWorld(info, 2, 5, start.pose.x, start.pose.y);
109 goal.header.frame_id = info.
frame_id;
110 gridToWorld(info, 17, 5, goal.pose.x, goal.pose.y);
116 nav_2d_msgs::Path2D first_path = global_planner.
makePlan(start, goal);
122 nav_2d_msgs::Path2D second_path = global_planner.
makePlan(start, goal);
124 if (expect_different)
126 EXPECT_FALSE(
pathsEqual(first_path, second_path));
130 EXPECT_TRUE(
pathsEqual(first_path, second_path));
134 TEST(GlobalPlannerReplanning, no_cache)
140 TEST(GlobalPlannerReplanning, any_cache)
146 TEST(GlobalPlannerReplanning, improve_cache)
152 TEST(GlobalPlannerReplanning, big_improve)
158 int main(
int argc,
char **argv)
160 ros::init(argc, argv,
"global_oscillation_test");
161 testing::InitGoogleTest(&argc, argv);
162 return RUN_ALL_TESTS();
void setBarrier(nav_core2::Costmap &costmap, const unsigned char value)
void replanning_test(const std::string &ns, bool expect_different, bool path_caching=false, double improvement_threshold=-1.0)
Check to see whether path caching is working as expected.
void setCost(const unsigned int x, const unsigned int y, const unsigned char cost)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void gridToWorld(const NavGridInfo &info, int mx, int my, double &wx, double &wy)
TFSIMD_FORCE_INLINE const tfScalar & y() const
bool pathsEqual(const nav_2d_msgs::Path2D &path_a, const nav_2d_msgs::Path2D &path_b)
TEST(GlobalPlannerReplanning, no_cache)
nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped &start, const nav_2d_msgs::Pose2DStamped &goal) override
NavGridInfo getInfo() const
void initialize(const ros::NodeHandle &parent, const std::string &name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override
int main(int argc, char **argv)
std::shared_ptr< tf::TransformListener > TFListenerPtr
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const