From 2702a47669022bb3b5ab5101baef7d95797f9bb9 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 5 Dec 2024 14:23:36 +0900 Subject: [PATCH] planner_cspace: reduce remember_updates calculation cost (#742) --- .../planner_cspace/planner_3d/costmap_bbf.h | 5 + planner_cspace/src/costmap_bbf.cpp | 15 +- planner_cspace/src/planner_3d.cpp | 4 + planner_cspace/test/CMakeLists.txt | 7 + .../test/data/global_map_remember.png | Bin 0 -> 982 bytes .../test/data/global_map_remember.yaml | 7 + .../test/data/local_map_remember.png | Bin 0 -> 976 bytes .../test/data/local_map_remember.yaml | 7 + .../include/planner_cspace/planner_status.h | 55 ++++ planner_cspace/test/src/test_navigate.cpp | 21 +- .../test/src/test_navigate_remember.cpp | 292 ++++++++++++++++++ .../test/navigation_remember_rostest.test | 87 ++++++ 12 files changed, 478 insertions(+), 22 deletions(-) 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/include/planner_cspace/planner_status.h 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/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; diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp index eee4b91b5..5647a4f51 100644 --- a/planner_cspace/src/planner_3d.cpp +++ b/planner_cspace/src/planner_3d.cpp @@ -851,12 +851,16 @@ 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 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); } if (!has_goal_) return; 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..aef11b0876e3dcbffe77c3bc788798abbe3bbccc GIT binary patch literal 982 zcmV;{11bE8P)KLZ*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!VqhQu7#aTKmuDhm!l(jT zDPUwA4Tn($qY6e9&`kjg8vp-jKn-UF1OWg50RR6305ni_@%07*qoM6N<$ Ef>C0@;{X5v literal 0 HcmV?d00001 diff --git a/planner_cspace/test/data/global_map_remember.yaml b/planner_cspace/test/data/global_map_remember.yaml new file mode 100755 index 000000000..232f8a69d --- /dev/null +++ b/planner_cspace/test/data/global_map_remember.yaml @@ -0,0 +1,7 @@ +image: global_map_remember.png +resolution: 0.1 +origin: [0.0, 0.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/planner_cspace/test/data/local_map_remember.png b/planner_cspace/test/data/local_map_remember.png new file mode 100755 index 0000000000000000000000000000000000000000..42b1ad93da712d9bd3fa800e8b354ee0eb45634e GIT binary patch literal 976 zcmV;>126oEP)KLZ*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-C05M5KK~#9!VqhQ${^JFs3I +#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 new file mode 100644 index 000000000..51a60d777 --- /dev/null +++ b/planner_cspace/test/src/test_navigate_remember.cpp @@ -0,0 +1,292 @@ +/* + * 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. + */ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +class NavigateWithRememberUpdates : 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_; + + NavigateWithRememberUpdates() + : tfl_(tfbuf_) + { + sub_costmap_ = nh_.subscribe("costmap", 1, &NavigateWithRememberUpdates::cbCostmap, this); + sub_status_ = nh_.subscribe( + "/planner_3d/status", 10, &NavigateWithRememberUpdates::cbStatus, this); + sub_path_ = nh_.subscribe("path", 1, &NavigateWithRememberUpdates::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(NavigateWithRememberUpdates, 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; + 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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +