From 03f3aef6a975395e0e8a54d871d98e7e689035e2 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 4 Dec 2024 12:23:25 +0900 Subject: [PATCH 1/8] Add test --- planner_cspace/test/CMakeLists.txt | 7 + .../test/data/global_map_remember.png | Bin 0 -> 998 bytes .../test/data/global_map_remember.yaml | 7 + .../test/data/local_map_remember.png | Bin 0 -> 982 bytes .../test/data/local_map_remember.yaml | 7 + .../test/src/test_navigate_remember.cpp | 310 ++++++++++++++++++ .../test/navigation_remember_rostest.test | 87 +++++ 7 files changed, 418 insertions(+) create mode 100755 planner_cspace/test/data/global_map_remember.png create mode 100755 planner_cspace/test/data/global_map_remember.yaml create mode 100755 planner_cspace/test/data/local_map_remember.png create mode 100755 planner_cspace/test/data/local_map_remember.yaml create mode 100644 planner_cspace/test/src/test_navigate_remember.cpp create mode 100644 planner_cspace/test/test/navigation_remember_rostest.test diff --git a/planner_cspace/test/CMakeLists.txt b/planner_cspace/test/CMakeLists.txt index ea959527b..4d96d2259 100644 --- a/planner_cspace/test/CMakeLists.txt +++ b/planner_cspace/test/CMakeLists.txt @@ -97,6 +97,13 @@ add_rostest(test/navigation_rostest.test DEPENDENCIES test_navigate ) +add_rostest_gtest(test_navigate_remember + test/navigation_remember_rostest.test + src/test_navigate_remember.cpp +) +target_link_libraries(test_navigate_remember ${catkin_LIBRARIES}) +add_dependencies(test_navigate_remember planner_3d patrol) + add_rostest(test/navigation_compat_rostest.test DEPENDENCIES test_navigate ) diff --git a/planner_cspace/test/data/global_map_remember.png b/planner_cspace/test/data/global_map_remember.png new file mode 100755 index 0000000000000000000000000000000000000000..b23344158e479fc5a5abf92925fa74df69a5ea10 GIT binary patch literal 998 zcmVKLZ*U+lnSp_Ufq@}0xwybFAi#%#fq@|}KQEO51AM#2z{tSB zz;IdD(Z$J?fi%FHTu@ZPz`$^Tfq}s&CAB!2fq~%*0|P^Pc}YPD0|R3W0|SFdQg%TJ z0|R3L0|SFdc1Vyj0|R3V0|OIJNoqw20|NttbACZ(QD%BZiGrb}rKN&nN`6wRLU3hq zNosDff@fZGeo;YwQDRAI3IhWJ)D8v)1_oZ2{1OHC#LPSeLsL}-Dual~C?)grM$+xhxmf|p7B=;2nnnfbQ63e)F`Yd zd{`u1lvi}CSe!Vg_*RJ&Nny#OQWes=(obaO$cD-Z%AJ+(QSedZRlJ}yML9}EN#(Wb zR<%ZTKMh%px0?I3CTgeZSnCSuzS29QKi{CnFv`f%Skm~n$vxA{#r++CO)=?RdfInDbtjt*-0cR=O|sSme3TYk~JdpT)k*{8ss|57-*GH|SXK z`H)+o&%(Y$FhvSRDMcH{xWz`r<;Axo%ud{#bT;{UDpQ(Vx=lt@W>wa#>^(X6@|g0~ z3w#QTi)I%eE_qufQSMSvSUIoiZ1vw-y}J1NNe#yue>WSnq_@s%yWSz#>D|@deYlsQ z&%VEI!oG?BCp%7QoqA$A?~LG?vt~V-qcyi=-o6D~3&R#IUi@*X!?Fp>AFecB)w=rT zTHSR`>u+u}*wnH4!B(qnQ@4NE>AP#y9*(`~`;H$_KiGNb^%1|Ln~#g1s6F}QwD*}U z=VZ^fU-)z>?((Ut7T1>D5WU%Y>+7BLyEpIqJUH;k^zrJaiqB@g5PaG7n)yxL+n?`C zKYaRB@cG@>yl?M*RI+y?e7jKeZ#YO-C07pqgK~#9!Vm!-$A22b{TmcIM0|SFG z4#~4H1q}bO$}`f*k%LhIBjbqh=BR>E1)~ZEhXNKf{{PW{qHRz!;#R;o2KLZ*U+lnSp_Ufq@}0xwybFAi#%#fq@|}KQEO51AM#2z{tSB zz;IdD(Z$J?fi%FHTu@ZPz`$^Tfq}s&CAB!2fq~%*0|P^Pc}YPD0|R3W0|SFdQg%TJ z0|R3L0|SFdc1Vyj0|R3V0|OIJNoqw20|NttbACZ(QD%BZiGrb}rKN&nN`6wRLU3hq zNosDff@fZGeo;YwQDRAI3IhWJ)D8v)1_oZ2{1OHC#LPSeLsL}-Dual~C?)grM$+xhxmf|p7B=;2nnnfbQ63e)F`Yd zd{`u1lvi}CSe!Vg_*RJ&Nny#OQWes=(obaO$cD-Z%AJ+(QSedZRlJ}yML9}EN#(Wb zR<%ZTKMh%px0?I3CTgeZSnCSuzS29QKi{CnFv`f%Skm~n$vxA{#r++CO)=?RdfInDbtjt*-0cR=O|sSme3TYk~JdpT)k*{8ss|57-*GH|SXK z`H)+o&%(Y$FhvSRDMcH{xWz`r<;Axo%ud{#bT;{UDpQ(Vx=lt@W>wa#>^(X6@|g0~ z3w#QTi)I%eE_qufQSMSvSUIoiZ1vw-y}J1NNe#yue>WSnq_@s%yWSz#>D|@deYlsQ z&%VEI!oG?BCp%7QoqA$A?~LG?vt~V-qcyi=-o6D~3&R#IUi@*X!?Fp>AFecB)w=rT zTHSR`>u+u}*wnH4!B(qnQ@4NE>AP#y9*(`~`;H$_KiGNb^%1|Ln~#g1s6F}QwD*}U z=VZ^fU-)z>?((Ut7T1>D5WU%Y>+7BLyEpIqJUH;k^zrJaiqB@g5PaG7n)yxL+n?`C zKYaRB@cG@>yl?M*RI+y?e7jKeZ#YO-C05?fQK~#9!VmM0xFwjW>0}g;t@E +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace planner_cspace_msgs +{ +std::ostream& operator<<(std::ostream& os, const PlannerStatus::ConstPtr& msg) +{ + if (!msg) + { + os << "nullptr"; + } + else + { + os << std::endl + << " header: " << msg->header.stamp << " " << msg->header.frame_id << std::endl + << " status: " << static_cast(msg->status) << std::endl + << " error: " << static_cast(msg->error); + } + return os; +} +} // namespace planner_cspace_msgs + +class Navigate : public ::testing::Test +{ +protected: + ros::NodeHandle nh_; + tf2_ros::Buffer tfbuf_; + tf2_ros::TransformListener tfl_; + planner_cspace_msgs::PlannerStatus::ConstPtr planner_status_; + costmap_cspace_msgs::CSpace3D::ConstPtr costmap_; + nav_msgs::Path::ConstPtr path_; + ros::Subscriber sub_costmap_; + ros::Subscriber sub_status_; + ros::Subscriber sub_path_; + ros::ServiceClient srv_forget_; + ros::Publisher pub_initial_pose_; + ros::Publisher pub_patrol_nodes_; + std::vector> traj_; + std::string test_scope_; + + Navigate() + : tfl_(tfbuf_) + { + sub_costmap_ = nh_.subscribe("costmap", 1, &Navigate::cbCostmap, this); + sub_status_ = nh_.subscribe( + "/planner_3d/status", 10, &Navigate::cbStatus, this); + sub_path_ = nh_.subscribe("path", 1, &Navigate::cbPath, this); + srv_forget_ = + nh_.serviceClient( + "forget_planning_cost"); + pub_initial_pose_ = + nh_.advertise("initialpose", 1, true); + pub_patrol_nodes_ = nh_.advertise("patrol_nodes", 1, true); + } + + virtual void SetUp() + { + test_scope_ = "[" + std::to_string(getpid()) + "] "; + + srv_forget_.waitForExistence(ros::Duration(10.0)); + ros::Rate rate(10.0); + + geometry_msgs::PoseWithCovarianceStamped pose; + pose.header.frame_id = "map"; + pose.pose.pose.position.x = 2.1; + pose.pose.pose.position.y = 3.0; + pose.pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), 1.57)); + pub_initial_pose_.publish(pose); + + const ros::Time deadline = ros::Time::now() + ros::Duration(15); + + while (ros::ok()) + { + ros::spinOnce(); + rate.sleep(); + const ros::Time now = ros::Time::now(); + if (now > deadline) + { + FAIL() << test_scope_ << now << " SetUp: transform timeout" << std::endl; + } + if (tfbuf_.canTransform("map", "base_link", now, ros::Duration(0.5))) + { + break; + } + } + + while (ros::ok() && !costmap_) + { + ros::spinOnce(); + rate.sleep(); + const ros::Time now = ros::Time::now(); + if (now > deadline) + { + FAIL() << test_scope_ << now << " SetUp: costmap timeout" << std::endl; + } + } + + std_srvs::EmptyRequest req; + std_srvs::EmptyResponse res; + srv_forget_.call(req, res); + + ros::Duration(1.0).sleep(); + } + void cbCostmap(const costmap_cspace_msgs::CSpace3D::ConstPtr& msg) + { + costmap_ = msg; + std::cerr << test_scope_ << msg->header.stamp << " Costmap received." << std::endl; + } + void cbStatus(const planner_cspace_msgs::PlannerStatus::ConstPtr& msg) + { + if (!planner_status_ || planner_status_->status != msg->status || planner_status_->error != msg->error) + { + std::cerr << test_scope_ << msg->header.stamp << " Status updated." << msg << std::endl; + } + planner_status_ = msg; + } + void cbPath(const nav_msgs::Path::ConstPtr& msg) + { + if (!path_ || path_->poses.size() != msg->poses.size()) + { + if (msg->poses.size() == 0) + { + std::cerr << test_scope_ << msg->header.stamp << " Path updated. (empty)" << std::endl; + } + else + { + std::cerr + << test_scope_ << msg->header.stamp << " Path updated." << std::endl + << msg->poses.front().pose.position.x << ", " << msg->poses.front().pose.position.y << std::endl + << msg->poses.back().pose.position.x << ", " << msg->poses.back().pose.position.y << std::endl; + } + path_ = msg; + } + } + tf2::Stamped lookupRobotTrans(const ros::Time& now) + { + geometry_msgs::TransformStamped trans_tmp = + tfbuf_.lookupTransform("map", "base_link", now, ros::Duration(0.5)); + tf2::Stamped trans; + tf2::fromMsg(trans_tmp, trans); + traj_.push_back(trans); + return trans; + } + void dumpRobotTrajectory() + { + double x_prev(0), y_prev(0); + tf2::Quaternion rot_prev(0, 0, 0, 1); + + std::cerr << test_scope_ << traj_.size() << " points recorded" << std::endl; + + for (const auto& t : traj_) + { + const double x = t.getOrigin().getX(); + const double y = t.getOrigin().getY(); + const tf2::Quaternion rot = t.getRotation(); + const double yaw_diff = rot.angleShortestPath(rot_prev); + if (std::abs(x - x_prev) > 0.1 || std::abs(y - y_prev) > 0.1 || std::abs(yaw_diff) > 0.2) + { + x_prev = x; + y_prev = y; + rot_prev = rot; + std::cerr << t.stamp_ << " " << x << " " << y << " " << tf2::getYaw(rot) << std::endl; + } + } + } + + void waitForPlannerStatus(const std::string& name, const int expected_error) + { + ros::spinOnce(); + ros::Duration(0.2).sleep(); + + ros::Rate wait(10); + ros::Time deadline = ros::Time::now() + ros::Duration(10); + while (ros::ok()) + { + ros::spinOnce(); + wait.sleep(); + + const ros::Time now = ros::Time::now(); + + if (now > deadline) + { + dumpRobotTrajectory(); + FAIL() + << test_scope_ << "/" << name << ": Navigation timeout." << std::endl + << "now: " << now << std::endl + << "status: " << planner_status_ << " (expected: " << expected_error << ")"; + } + + if (planner_status_->error == expected_error) + { + return; + } + } + } +}; + +TEST_F(Navigate, Navigate) +{ + ros::spinOnce(); + ros::Duration(0.2).sleep(); + + nav_msgs::Path path; + path.poses.resize(1); + path.header.frame_id = "map"; + path.poses[0].header.frame_id = path.header.frame_id; + path.poses[0].pose.position.x = 1.5; + path.poses[0].pose.position.y = 5.6; + path.poses[0].pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), 1.57)); + pub_patrol_nodes_.publish(path); + + tf2::Transform goal; + tf2::fromMsg(path.poses.back().pose, goal); + + ros::Rate wait(10); + const ros::Time deadline = ros::Time::now() + ros::Duration(120); + while (ros::ok()) + { + ros::spinOnce(); + wait.sleep(); + + const ros::Time now = ros::Time::now(); + + if (now > deadline) + { + dumpRobotTrajectory(); + FAIL() + << test_scope_ << "Navigation timeout." << std::endl + << "now: " << now << std::endl + << "status: " << planner_status_; + break; + } + + tf2::Stamped trans; + try + { + trans = lookupRobotTrans(now); + } + catch (tf2::TransformException& e) + { + std::cerr << test_scope_ << e.what() << std::endl; + continue; + } + + auto goal_rel = trans.inverse() * goal; + if (goal_rel.getOrigin().length() < 0.2 && + std::abs(tf2::getYaw(goal_rel.getRotation())) < 0.2) + { + std::cerr << test_scope_ << "Navagation success." << std::endl; + dumpRobotTrajectory(); + ros::Duration(2.0).sleep(); + return; + } + } + ASSERT_TRUE(false); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_navigate_remember"); + + return RUN_ALL_TESTS(); +} diff --git a/planner_cspace/test/test/navigation_remember_rostest.test b/planner_cspace/test/test/navigation_remember_rostest.test new file mode 100644 index 000000000..e8cd21dc7 --- /dev/null +++ b/planner_cspace/test/test/navigation_remember_rostest.test @@ -0,0 +1,87 @@ + + + + + + + + + + + + [[0.2, -0.1], [0.2, 0.1], [-0.2, 0.1], [-0.2, -0.1]] + + + + + static_layers: + - name: unknown + type: Costmap3dLayerUnknownHandle + unknown_cost: 100 + layers: + - name: overlay + type: Costmap3dLayerFootprint + overlay_mode: max + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 6e7fb22f55d1992070df7c6f2e6f5c5b59d54d3f Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 4 Dec 2024 15:30:42 +0900 Subject: [PATCH 2/8] Add debug log for remember updates --- planner_cspace/src/planner_3d.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp index eee4b91b5..0504dc03b 100644 --- a/planner_cspace/src/planner_3d.cpp +++ b/planner_cspace/src/planner_3d.cpp @@ -851,12 +851,15 @@ class Planner3dNode if (remember_updates_) { + const auto ts = boost::chrono::high_resolution_clock::now(); bbf_costmap_.remember( &cm_updates_, s, remember_hit_odds_, remember_miss_odds_, hist_ignore_range_, hist_ignore_range_max_); publishRememberedMap(); bbf_costmap_.updateCostmap(); + const float dur = boost::chrono::duration(tnow - ts).count(); + ROS_DEBUG("Remembered costmap updated (%0.4f sec.)", dur); } if (!has_goal_) return; From 9f0dd1fe49f411138d9cd58116c781239444722f Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 4 Dec 2024 16:07:06 +0900 Subject: [PATCH 3/8] Fix duration calculation --- planner_cspace/src/planner_3d.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp index 0504dc03b..5647a4f51 100644 --- a/planner_cspace/src/planner_3d.cpp +++ b/planner_cspace/src/planner_3d.cpp @@ -858,6 +858,7 @@ class Planner3dNode hist_ignore_range_, hist_ignore_range_max_); publishRememberedMap(); bbf_costmap_.updateCostmap(); + const auto tnow = boost::chrono::high_resolution_clock::now(); const float dur = boost::chrono::duration(tnow - ts).count(); ROS_DEBUG("Remembered costmap updated (%0.4f sec.)", dur); } From 755e751529e18e4bdf5e3ef3c5b44a9540e24cbe Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 4 Dec 2024 16:57:52 +0900 Subject: [PATCH 4/8] Update test map --- .../test/data/global_map_remember.png | Bin 998 -> 982 bytes planner_cspace/test/data/local_map_remember.png | Bin 982 -> 976 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/planner_cspace/test/data/global_map_remember.png b/planner_cspace/test/data/global_map_remember.png index b23344158e479fc5a5abf92925fa74df69a5ea10..aef11b0876e3dcbffe77c3bc788798abbe3bbccc 100755 GIT binary patch delta 84 zcmaFHevN%Y7qgGKr;B4q#jT_S1qKPpe~+i9E2SJ-!4##LkRrp|9>}_w%T4rxFC(|q o|Nj{dyy>hgj0_A6|Nk>Hm_#wve?QCd8pvhvboFyt=akR{0Pd+ASO5S3 delta 100 zcmV-q0Gt2T2j&N`iUV9nNkl8yd3I>M)7Bv3<(SV|DP&49Iz&Hr%ZomMA3I!vr&-54o0000D$`7s_mq2$EqsO=`&qa_h)uqt7ThTKFh e4h9hT|DTz`B=vS>j>a}C5Z}|)&t;ucLK6T9a2Ub> delta 84 zcmcb>evN%Y7qgGKr;B4q#jPX@Zw7-4o{Y?V4FMnIwIxn+aj=O9a&bf%u1W|wxS}bL o^}l=fqs9P-6;Uz|c1asB#I38m7+~}?fB^_RUHx3vIVCg!0L{Q1Qvd(} From 784d3920b4fe3d86ff782ef9e23c96ad0a3346ac Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 4 Dec 2024 16:58:28 +0900 Subject: [PATCH 5/8] Partially update remembered costmap --- .../planner_cspace/planner_3d/costmap_bbf.h | 5 +++++ planner_cspace/src/costmap_bbf.cpp | 15 ++++++++++++--- 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/planner_cspace/include/planner_cspace/planner_3d/costmap_bbf.h b/planner_cspace/include/planner_cspace/planner_3d/costmap_bbf.h index d8384ea90..eb36aab6c 100644 --- a/planner_cspace/include/planner_cspace/planner_3d/costmap_bbf.h +++ b/planner_cspace/include/planner_cspace/planner_3d/costmap_bbf.h @@ -49,6 +49,8 @@ class CostmapBBF BlockMemGridmap cm_hist_bbf_; BlockMemGridmap cm_hist_; Vec size_; + VecInternal updated_min_; + VecInternal updated_max_; public: inline CostmapBBF() @@ -60,11 +62,14 @@ class CostmapBBF size_ = size; cm_hist_bbf_.reset(VecInternal(size[0], size[1])); cm_hist_.reset(VecInternal(size[0], size[1])); + clear(); } inline void clear() { cm_hist_bbf_.clear(bbf::BinaryBayesFilter(bbf::MIN_ODDS)); cm_hist_.clear(0); + updated_min_ = VecInternal(0, 0); + updated_max_ = VecInternal(size_[0] - 1, size_[1] - 1); } inline char getCost(const Vec& p) const { diff --git a/planner_cspace/src/costmap_bbf.cpp b/planner_cspace/src/costmap_bbf.cpp index 73a9a80b7..6e8785733 100644 --- a/planner_cspace/src/costmap_bbf.cpp +++ b/planner_cspace/src/costmap_bbf.cpp @@ -27,6 +27,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include #include #include @@ -39,14 +40,15 @@ namespace planner_3d { void CostmapBBF::updateCostmap() { - cm_hist_.clear(0); - for (VecInternal p(0, 0); p[1] < size_[1]; p[1]++) + for (VecInternal p = updated_min_; p[1] <= updated_max_[1]; p[1]++) { - for (p[0] = 0; p[0] < size_[0]; p[0]++) + for (p[0] = updated_min_[0]; p[0] <= updated_max_[0]; p[0]++) { cm_hist_[p] = std::lround(cm_hist_bbf_[p].getNormalizedProbability() * 100.0); } } + updated_min_ = VecInternal(size_[0], size_[1]); + updated_max_ = VecInternal(-1, -1); } void CostmapBBF::remember( const BlockMemGridmapBase* costmap, @@ -54,6 +56,13 @@ void CostmapBBF::remember( const float remember_hit_odds, const float remember_miss_odds, const int range_min, const int range_max) { + updated_min_ = VecInternal( + std::min(updated_min_[0], std::max(0, center[0] - range_max)), + std::min(updated_min_[1], std::max(0, center[1] - range_max))); + updated_max_ = VecInternal( + std::max(updated_max_[0], std::min(size_[0] - 1, center[0] + range_max)), + std::max(updated_max_[1], std::min(size_[1] - 1, center[1] + range_max))); + const size_t width = size_[0]; const size_t height = size_[1]; const int range_min_sq = range_min * range_min; From 7c2c3ec77cf97852768a80e894a1193eba410ecf Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 4 Dec 2024 17:38:30 +0900 Subject: [PATCH 6/8] Revert debug logging --- planner_cspace/test/src/test_navigate_remember.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/planner_cspace/test/src/test_navigate_remember.cpp b/planner_cspace/test/src/test_navigate_remember.cpp index 652553291..0601cb428 100644 --- a/planner_cspace/test/src/test_navigate_remember.cpp +++ b/planner_cspace/test/src/test_navigate_remember.cpp @@ -293,7 +293,6 @@ TEST_F(Navigate, Navigate) std::abs(tf2::getYaw(goal_rel.getRotation())) < 0.2) { std::cerr << test_scope_ << "Navagation success." << std::endl; - dumpRobotTrajectory(); ros::Duration(2.0).sleep(); return; } From cc1c0a3737a5a885c88b80f7a34fdb88ef04efbf Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 4 Dec 2024 18:14:12 +0900 Subject: [PATCH 7/8] Update planner_cspace/test/src/test_navigate_remember.cpp --- planner_cspace/test/src/test_navigate_remember.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planner_cspace/test/src/test_navigate_remember.cpp b/planner_cspace/test/src/test_navigate_remember.cpp index 0601cb428..b610da8d5 100644 --- a/planner_cspace/test/src/test_navigate_remember.cpp +++ b/planner_cspace/test/src/test_navigate_remember.cpp @@ -241,7 +241,7 @@ class Navigate : public ::testing::Test } }; -TEST_F(Navigate, Navigate) +TEST_F(NavigateWithRememberUpdates, Navigate) { ros::spinOnce(); ros::Duration(0.2).sleep(); From 71747298b3abd448f855b508540efafa8883e778 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 4 Dec 2024 18:47:05 +0900 Subject: [PATCH 8/8] Fix test --- .../include/planner_cspace/planner_status.h | 55 +++++++++++++++++++ planner_cspace/test/src/test_navigate.cpp | 21 +------ .../test/src/test_navigate_remember.cpp | 31 +++-------- 3 files changed, 64 insertions(+), 43 deletions(-) create mode 100644 planner_cspace/test/include/planner_cspace/planner_status.h diff --git a/planner_cspace/test/include/planner_cspace/planner_status.h b/planner_cspace/test/include/planner_cspace/planner_status.h new file mode 100644 index 000000000..fac2e34dc --- /dev/null +++ b/planner_cspace/test/include/planner_cspace/planner_status.h @@ -0,0 +1,55 @@ +/* + * Copyright (c) 2024, the neonavigation authors + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef PLANNER_CSPACE_PLANNER_STATUS_H +#define PLANNER_CSPACE_PLANNER_STATUS_H + +#include +#include + +namespace planner_cspace_msgs +{ +std::ostream& operator<<(std::ostream& os, const PlannerStatus::ConstPtr& msg) +{ + if (!msg) + { + os << "nullptr"; + } + else + { + os << std::endl + << " header: " << msg->header.stamp << " " << msg->header.frame_id << std::endl + << " status: " << static_cast(msg->status) << std::endl + << " error: " << static_cast(msg->error); + } + return os; +} +} // namespace planner_cspace_msgs + +#endif // PLANNER_CSPACE_PLANNER_STATUS_H diff --git a/planner_cspace/test/src/test_navigate.cpp b/planner_cspace/test/src/test_navigate.cpp index aa52c62f1..eb6008104 100644 --- a/planner_cspace/test/src/test_navigate.cpp +++ b/planner_cspace/test/src/test_navigate.cpp @@ -45,26 +45,9 @@ #include #include -#include +#include -namespace planner_cspace_msgs -{ -std::ostream& operator<<(std::ostream& os, const PlannerStatus::ConstPtr& msg) -{ - if (!msg) - { - os << "nullptr"; - } - else - { - os << std::endl - << " header: " << msg->header.stamp << " " << msg->header.frame_id << std::endl - << " status: " << static_cast(msg->status) << std::endl - << " error: " << static_cast(msg->error); - } - return os; -} -} // namespace planner_cspace_msgs +#include class Navigate : public ::testing::Test { diff --git a/planner_cspace/test/src/test_navigate_remember.cpp b/planner_cspace/test/src/test_navigate_remember.cpp index b610da8d5..51a60d777 100644 --- a/planner_cspace/test/src/test_navigate_remember.cpp +++ b/planner_cspace/test/src/test_navigate_remember.cpp @@ -45,28 +45,11 @@ #include #include -#include +#include -namespace planner_cspace_msgs -{ -std::ostream& operator<<(std::ostream& os, const PlannerStatus::ConstPtr& msg) -{ - if (!msg) - { - os << "nullptr"; - } - else - { - os << std::endl - << " header: " << msg->header.stamp << " " << msg->header.frame_id << std::endl - << " status: " << static_cast(msg->status) << std::endl - << " error: " << static_cast(msg->error); - } - return os; -} -} // namespace planner_cspace_msgs +#include -class Navigate : public ::testing::Test +class NavigateWithRememberUpdates : public ::testing::Test { protected: ros::NodeHandle nh_; @@ -84,13 +67,13 @@ class Navigate : public ::testing::Test std::vector> traj_; std::string test_scope_; - Navigate() + NavigateWithRememberUpdates() : tfl_(tfbuf_) { - sub_costmap_ = nh_.subscribe("costmap", 1, &Navigate::cbCostmap, this); + sub_costmap_ = nh_.subscribe("costmap", 1, &NavigateWithRememberUpdates::cbCostmap, this); sub_status_ = nh_.subscribe( - "/planner_3d/status", 10, &Navigate::cbStatus, this); - sub_path_ = nh_.subscribe("path", 1, &Navigate::cbPath, this); + "/planner_3d/status", 10, &NavigateWithRememberUpdates::cbStatus, this); + sub_path_ = nh_.subscribe("path", 1, &NavigateWithRememberUpdates::cbPath, this); srv_forget_ = nh_.serviceClient( "forget_planning_cost");