add humble-navigation2

2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
@@ -0,0 +1,21 @@
+# Test test executors
+ament_add_gtest(test_task_executors
+  test_task_executors.cpp
+)
+ament_target_dependencies(test_task_executors
+  ${dependencies}
+)
+target_link_libraries(test_task_executors
+  ${library_name} wait_at_waypoint photo_at_waypoint input_at_waypoint
+)
+
+# Test dynamic parameters
+ament_add_gtest(test_dynamic_parameters
+  test_dynamic_parameters.cpp
+)
+ament_target_dependencies(test_dynamic_parameters
+  ${dependencies}
+)
+target_link_libraries(test_dynamic_parameters
+  ${library_name}
+)
@@ -0,0 +1,76 @@
+// Copyright (c) 2021, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <math.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_waypoint_follower/waypoint_follower.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+class WPShim : public nav2_waypoint_follower::WaypointFollower
+{
+public:
+  WPShim()
+  : nav2_waypoint_follower::WaypointFollower(rclcpp::NodeOptions())
+  {
+  }
+
+  void configure()
+  {
+    rclcpp_lifecycle::State state;
+    this->on_configure(state);
+  }
+
+  void activate()
+  {
+    rclcpp_lifecycle::State state;
+    this->on_activate(state);
+  }
+};
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(WPTest, test_dynamic_parameters)
+{
+  auto follower = std::make_shared<WPShim>();
+  follower->configure();
+  follower->activate();
+
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    follower->get_node_base_interface(), follower->get_node_topics_interface(),
+    follower->get_node_graph_interface(),
+    follower->get_node_services_interface());
+
+  auto results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("loop_rate", 100),
+      rclcpp::Parameter("stop_on_failure", false)});
+
+  rclcpp::spin_until_future_complete(
+    follower->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(follower->get_parameter("loop_rate").as_int(), 100);
+  EXPECT_EQ(follower->get_parameter("stop_on_failure").as_bool(), false);
+}
@@ -0,0 +1,164 @@
+// Copyright (c) 2021, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <math.h>
+#include <condition_variable>
+#include <memory>
+#include <string>
+#include <vector>
+#include <utility>
+#include <thread>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp"
+#include "nav2_waypoint_follower/plugins/wait_at_waypoint.hpp"
+#include "nav2_waypoint_follower/plugins/input_at_waypoint.hpp"
+
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(WaypointFollowerTest, WaitAtWaypoint)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testWaypointNode");
+
+  node->declare_parameter("WAW.waypoint_pause_duration", 50);
+
+  std::unique_ptr<nav2_waypoint_follower::WaitAtWaypoint> waw(
+    new nav2_waypoint_follower::WaitAtWaypoint
+  );
+  waw->initialize(node, std::string("WAW"));
+
+  auto start_time = node->now();
+
+  // should wait 50ms
+  geometry_msgs::msg::PoseStamped pose;
+  waw->processAtWaypoint(pose, 0);
+
+  auto end_time = node->now();
+
+  EXPECT_NEAR((end_time - start_time).seconds(), 0.05, 0.01);
+
+  waw.reset(new nav2_waypoint_follower::WaitAtWaypoint);
+  node->set_parameter(rclcpp::Parameter("WAW.enabled", false));
+  waw->initialize(node, std::string("WAW"));
+
+  // plugin is not enabled, should exit
+  EXPECT_TRUE(waw->processAtWaypoint(pose, 0));
+}
+
+TEST(WaypointFollowerTest, InputAtWaypoint)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testWaypointNode");
+  auto pub = node->create_publisher<std_msgs::msg::Empty>("input_at_waypoint/input", 1);
+  pub->on_activate();
+  auto publish_message =
+    [&]() -> void
+    {
+      rclcpp::Rate(5).sleep();
+      auto msg = std::make_unique<std_msgs::msg::Empty>();
+      pub->publish(std::move(msg));
+      rclcpp::spin_some(node->shared_from_this()->get_node_base_interface());
+    };
+
+  std::unique_ptr<nav2_waypoint_follower::InputAtWaypoint> iaw(
+    new nav2_waypoint_follower::InputAtWaypoint
+  );
+  iaw->initialize(node, std::string("IAW"));
+
+  auto start_time = node->now();
+
+  // no input, should timeout
+  geometry_msgs::msg::PoseStamped pose;
+  EXPECT_FALSE(iaw->processAtWaypoint(pose, 0));
+
+  auto end_time = node->now();
+
+  EXPECT_NEAR((end_time - start_time).seconds(), 10.0, 0.1);
+
+  // has input now, should work
+  std::thread t1(publish_message);
+  EXPECT_TRUE(iaw->processAtWaypoint(pose, 0));
+  t1.join();
+
+  iaw.reset(new nav2_waypoint_follower::InputAtWaypoint);
+  node->set_parameter(rclcpp::Parameter("IAW.enabled", false));
+  iaw->initialize(node, std::string("IAW"));
+
+  // plugin is not enabled, should exit
+  EXPECT_TRUE(iaw->processAtWaypoint(pose, 0));
+}
+
+TEST(WaypointFollowerTest, PhotoAtWaypoint)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testWaypointNode");
+  auto pub = node->create_publisher<sensor_msgs::msg::Image>("/camera/color/image_raw", 1);
+  pub->on_activate();
+  std::condition_variable cv;
+  std::mutex mtx;
+  std::unique_lock<std::mutex> lck(mtx, std::defer_lock);
+  bool data_published = false;
+  auto publish_message =
+    [&]() -> void
+    {
+      rclcpp::Rate(5).sleep();
+      auto msg = std::make_unique<sensor_msgs::msg::Image>();
+      // fill image msg data.
+      msg->encoding = "rgb8";
+      msg->height = 240;
+      msg->width = 320;
+      msg->step = 960;
+      auto size = msg->height * msg->width * 3;
+      msg->data.reserve(size);
+      int fake_data = 0;
+      for (size_t i = 0; i < size; i++) {
+        msg->data.push_back(fake_data++);
+      }
+      pub->publish(std::move(msg));
+      rclcpp::spin_some(node->shared_from_this()->get_node_base_interface());
+      lck.lock();
+      data_published = true;
+      cv.notify_one();
+      lck.unlock();
+    };
+
+  std::unique_ptr<nav2_waypoint_follower::PhotoAtWaypoint> paw(
+    new nav2_waypoint_follower::PhotoAtWaypoint
+  );
+  paw->initialize(node, std::string("PAW"));
+
+  // no images, throws because can't write
+  geometry_msgs::msg::PoseStamped pose;
+  EXPECT_FALSE(paw->processAtWaypoint(pose, 0));
+
+  std::thread t1(publish_message);
+  cv.wait(lck);
+  // has image now, since we force waiting until image is published
+  EXPECT_TRUE(paw->processAtWaypoint(pose, 0));
+  t1.join();
+
+  paw.reset(new nav2_waypoint_follower::PhotoAtWaypoint);
+  node->set_parameter(rclcpp::Parameter("PAW.enabled", false));
+  paw->initialize(node, std::string("PAW"));
+
+  // plugin is not enabled, should exit
+  EXPECT_TRUE(paw->processAtWaypoint(pose, 0));
+}