Add test

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
 )

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
+

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
+

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 <cmath>
+#include <cstddef>
+#include <string>
+#include <unistd.h>
+#include <vector>
+
+#include <ros/ros.h>
+
+#include <costmap_cspace_msgs/CSpace3D.h>
+#include <nav_msgs/GetPlan.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/Path.h>
+#include <planner_cspace_msgs/PlannerStatus.h>
+#include <std_srvs/Empty.h>
+#include <tf2/utils.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <gtest/gtest.h>
+
+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<int>(msg->status) << std::endl
+       << "  error: " << static_cast<int>(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<tf2::Stamped<tf2::Transform>> 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<std_srvs::EmptyRequest, std_srvs::EmptyResponse>(
+            "forget_planning_cost");
+    pub_initial_pose_ =
+        nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1, true);
+    pub_patrol_nodes_ = nh_.advertise<nav_msgs::Path>("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<tf2::Transform> lookupRobotTrans(const ros::Time& now)
+  {
+    geometry_msgs::TransformStamped trans_tmp =
+        tfbuf_.lookupTransform("map", "base_link", now, ros::Duration(0.5));
+    tf2::Stamped<tf2::Transform> 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<tf2::Transform> 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();
+}