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 000000000..b23344158 Binary files /dev/null and b/planner_cspace/test/data/global_map_remember.png differ 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 000000000..db775703b Binary files /dev/null and b/planner_cspace/test/data/local_map_remember.png differ diff --git a/planner_cspace/test/data/local_map_remember.yaml b/planner_cspace/test/data/local_map_remember.yaml new file mode 100755 index 000000000..14e83a070 --- /dev/null +++ b/planner_cspace/test/data/local_map_remember.yaml @@ -0,0 +1,7 @@ +image: local_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/src/test_navigate_remember.cpp b/planner_cspace/test/src/test_navigate_remember.cpp new file mode 100644 index 000000000..652553291 --- /dev/null +++ b/planner_cspace/test/src/test_navigate_remember.cpp @@ -0,0 +1,310 @@ +/* + * 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 + +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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +