add humble-navigation2

navigation2/nav2_system_tests/src/dummy_controller/CMakeLists.txt

@@ -0,0 +1,13 @@
+add_executable(dummy_controller_node
+  src/dummy_controller/main.cpp
+  src/dummy_controller/dummy_controller.cpp
+)
+
+ament_target_dependencies(dummy_controller_node
+  rclcpp
+  std_msgs
+  nav2_util
+  nav2_behavior_tree
+  nav2_msgs
+  nav_msgs
+)

navigation2/nav2_system_tests/src/dummy_controller/dummy_controller.cpp

@@ -0,0 +1,104 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// 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.
+
+#include <chrono>
+#include <ctime>
+#include <thread>
+#include <memory>
+#include <utility>
+
+#include "dummy_controller.hpp"
+
+using namespace std::chrono_literals;
+
+namespace nav2_system_tests
+{
+
+DummyController::DummyController()
+: Node("DummyController")
+{
+  RCLCPP_INFO(get_logger(), "Initializing DummyController...");
+
+  auto temp_node = std::shared_ptr<rclcpp::Node>(this, [](auto) {});
+
+  vel_pub_ =
+    this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
+
+  task_server_ = std::make_unique<nav2_behavior_tree::FollowPathTaskServer>(temp_node, false),
+  task_server_->setExecuteCallback(
+    std::bind(&DummyController::followPath, this, std::placeholders::_1));
+
+  // Start listening for incoming ComputePathToPose action server requests
+  task_server_->start();
+
+  RCLCPP_INFO(get_logger(), "Initialized DummyController");
+}
+
+DummyController::~DummyController()
+{
+  RCLCPP_INFO(get_logger(), "Shutting down DummyController");
+}
+
+void
+DummyController::followPath(const nav2_behavior_tree::FollowPathCommand::SharedPtr /*command*/)
+{
+  RCLCPP_INFO(get_logger(), "Starting controller ");
+
+  auto start_time = std::chrono::system_clock::now();
+  auto time_since_msg = std::chrono::system_clock::now();
+
+  while (true) {
+    // Dummy controller computation time
+    std::this_thread::sleep_for(50ms);
+
+    if (task_server_->cancelRequested()) {
+      RCLCPP_INFO(get_logger(), "Task cancelled");
+      setZeroVelocity();
+      task_server_->setCanceled();
+      return;
+    }
+
+    // Log a message every second
+    auto current_time = std::chrono::system_clock::now();
+    if (current_time - time_since_msg >= 1s) {
+      RCLCPP_INFO(get_logger(), "Following path");
+      time_since_msg = std::chrono::system_clock::now();
+    }
+
+    // Output control command
+    auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
+    cmd_vel->linear.x = 0.1;
+    vel_pub_->publish(std::move(cmd_vel));
+
+    if (current_time - start_time >= 30s) {
+      RCLCPP_INFO(get_logger(), "Reached end point");
+      setZeroVelocity();
+      break;
+    }
+  }
+
+  nav2_behavior_tree::FollowPathResult result;
+  task_server_->setResult(result);
+}
+
+void DummyController::setZeroVelocity()
+{
+  auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
+  cmd_vel->linear.x = 0.0;
+  cmd_vel->linear.y = 0.0;
+  cmd_vel->angular.z = 0.0;
+  vel_pub_->publish(std::move(cmd_vel));
+}
+
+}  // namespace nav2_system_tests

navigation2/nav2_system_tests/src/dummy_controller/dummy_controller.hpp

@@ -0,0 +1,45 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// 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.
+
+#ifndef DUMMY_CONTROLLER__DUMMY_CONTROLLER_HPP_
+#define DUMMY_CONTROLLER__DUMMY_CONTROLLER_HPP_
+
+#include <memory>
+
+#include "nav2_behavior_tree/follow_path_task.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+
+namespace nav2_system_tests
+{
+
+class DummyController : public rclcpp::Node
+{
+public:
+  DummyController();
+  ~DummyController();
+
+  nav2_behavior_tree::TaskStatus followPath(
+    const nav2_behavior_tree::FollowPathCommand::SharedPtr command);
+
+private:
+  void setZeroVelocity();
+
+  std::unique_ptr<nav2_behavior_tree::FollowPathTaskServer> task_server_;
+
+  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::Twist>> vel_pub_;
+};
+
+}  // namespace nav2_system_tests
+
+#endif  // DUMMY_CONTROLLER__DUMMY_CONTROLLER_HPP_

navigation2/nav2_system_tests/src/dummy_controller/main.cpp

@@ -0,0 +1,26 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// 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 <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "dummy_controller.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<nav2_system_tests::DummyController>());
+  rclcpp::shutdown();
+
+  return 0;
+}