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,116 @@
+// Copyright (c) 2020 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.
+
+#include "nav2_waypoint_follower/plugins/input_at_waypoint.hpp"
+
+#include <string>
+#include <exception>
+
+#include "pluginlib/class_list_macros.hpp"
+
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_waypoint_follower
+{
+
+using std::placeholders::_1;
+
+InputAtWaypoint::InputAtWaypoint()
+: input_received_(false),
+  is_enabled_(true),
+  timeout_(10.0, 0.0)
+{
+}
+
+InputAtWaypoint::~InputAtWaypoint()
+{
+}
+
+void InputAtWaypoint::initialize(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  const std::string & plugin_name)
+{
+  auto node = parent.lock();
+
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node in input at waypoint plugin!"};
+  }
+
+  logger_ = node->get_logger();
+  clock_ = node->get_clock();
+
+  double timeout;
+  std::string input_topic;
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name + ".timeout",
+    rclcpp::ParameterValue(10.0));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name + ".enabled",
+    rclcpp::ParameterValue(true));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name + ".input_topic",
+    rclcpp::ParameterValue("input_at_waypoint/input"));
+  node->get_parameter(plugin_name + ".timeout", timeout);
+  node->get_parameter(plugin_name + ".enabled", is_enabled_);
+  node->get_parameter(plugin_name + ".input_topic", input_topic);
+
+  timeout_ = rclcpp::Duration(timeout, 0.0);
+
+  RCLCPP_INFO(
+    logger_, "InputAtWaypoint: Subscribing to input topic %s.", input_topic.c_str());
+  subscription_ = node->create_subscription<std_msgs::msg::Empty>(
+    input_topic, 1, std::bind(&InputAtWaypoint::Cb, this, _1));
+}
+
+void InputAtWaypoint::Cb(const std_msgs::msg::Empty::SharedPtr /*msg*/)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  input_received_ = true;
+}
+
+bool InputAtWaypoint::processAtWaypoint(
+  const geometry_msgs::msg::PoseStamped & /*curr_pose*/,
+  const int & curr_waypoint_index)
+{
+  if (!is_enabled_) {
+    return true;
+  }
+
+  input_received_ = false;
+
+  rclcpp::Time start = clock_->now();
+  rclcpp::Rate r(50);
+  bool input_received = false;
+  while (clock_->now() - start < timeout_) {
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
+      input_received = input_received_;
+    }
+
+    if (input_received) {
+      return true;
+    }
+
+    r.sleep();
+  }
+
+  RCLCPP_WARN(
+    logger_, "Unable to get external input at wp %i. Moving on.", curr_waypoint_index);
+  return false;
+}
+
+}  // namespace nav2_waypoint_follower
+
+PLUGINLIB_EXPORT_CLASS(
+  nav2_waypoint_follower::InputAtWaypoint,
+  nav2_core::WaypointTaskExecutor)
@@ -0,0 +1,159 @@
+// Copyright (c) 2020 Fetullah Atas
+//
+// 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 "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp"
+
+#include <string>
+#include <memory>
+
+#include "pluginlib/class_list_macros.hpp"
+
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_waypoint_follower
+{
+PhotoAtWaypoint::PhotoAtWaypoint()
+{
+}
+
+PhotoAtWaypoint::~PhotoAtWaypoint()
+{
+}
+
+void PhotoAtWaypoint::initialize(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  const std::string & plugin_name)
+{
+  auto node = parent.lock();
+
+  curr_frame_msg_ = std::make_shared<sensor_msgs::msg::Image>();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name + ".enabled",
+    rclcpp::ParameterValue(true));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name + ".image_topic",
+    rclcpp::ParameterValue("/camera/color/image_raw"));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name + ".save_dir",
+    rclcpp::ParameterValue("/tmp/waypoint_images"));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name + ".image_format",
+    rclcpp::ParameterValue("png"));
+
+  std::string save_dir_as_string;
+  node->get_parameter(plugin_name + ".enabled", is_enabled_);
+  node->get_parameter(plugin_name + ".image_topic", image_topic_);
+  node->get_parameter(plugin_name + ".save_dir", save_dir_as_string);
+  node->get_parameter(plugin_name + ".image_format", image_format_);
+
+  // get inputted save directory and make sure it exists, if not log and create  it
+  save_dir_ = save_dir_as_string;
+  try {
+    if (!std::filesystem::exists(save_dir_)) {
+      RCLCPP_WARN(
+        logger_,
+        "Provided save directory for photo at waypoint plugin does not exist,"
+        "provided directory is: %s, the directory will be created automatically.",
+        save_dir_.c_str()
+      );
+      if (!std::filesystem::create_directory(save_dir_)) {
+        RCLCPP_ERROR(
+          logger_,
+          "Failed to create directory!: %s required by photo at waypoint plugin, "
+          "exiting the plugin with failure!",
+          save_dir_.c_str()
+        );
+        is_enabled_ = false;
+      }
+    }
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(
+      logger_, "Exception (%s) thrown while attempting to create image capture directory."
+      " This task executor is being disabled as it cannot save images.", e.what());
+    is_enabled_ = false;
+  }
+
+  if (!is_enabled_) {
+    RCLCPP_INFO(
+      logger_, "Photo at waypoint plugin is disabled.");
+  } else {
+    RCLCPP_INFO(
+      logger_, "Initializing photo at waypoint plugin, subscribing to camera topic named; %s",
+      image_topic_.c_str());
+    camera_image_subscriber_ = node->create_subscription<sensor_msgs::msg::Image>(
+      image_topic_, rclcpp::SystemDefaultsQoS(),
+      std::bind(&PhotoAtWaypoint::imageCallback, this, std::placeholders::_1));
+  }
+}
+
+bool PhotoAtWaypoint::processAtWaypoint(
+  const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index)
+{
+  if (!is_enabled_) {
+    RCLCPP_WARN(
+      logger_,
+      "Photo at waypoint plugin is disabled. Not performing anything"
+    );
+    return true;
+  }
+  try {
+    // construct the full path to image filename
+    std::filesystem::path file_name = std::to_string(
+      curr_waypoint_index) + "_" +
+      std::to_string(curr_pose.header.stamp.sec) + "." + image_format_;
+    std::filesystem::path full_path_image_path = save_dir_ / file_name;
+
+    // save the taken photo at this waypoint to given directory
+    std::lock_guard<std::mutex> guard(global_mutex_);
+    cv::Mat curr_frame_mat;
+    deepCopyMsg2Mat(curr_frame_msg_, curr_frame_mat);
+    cv::imwrite(full_path_image_path.c_str(), curr_frame_mat);
+    RCLCPP_INFO(
+      logger_,
+      "Photo has been taken sucessfully at waypoint %i", curr_waypoint_index);
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(
+      logger_,
+      "Couldn't take photo at waypoint %i! Caught exception: %s \n"
+      "Make sure that the image topic named: %s is valid and active!",
+      curr_waypoint_index,
+      e.what(), image_topic_.c_str());
+    return false;
+  }
+  return true;
+}
+
+void PhotoAtWaypoint::imageCallback(const sensor_msgs::msg::Image::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> guard(global_mutex_);
+  curr_frame_msg_ = msg;
+}
+
+void PhotoAtWaypoint::deepCopyMsg2Mat(
+  const sensor_msgs::msg::Image::SharedPtr & msg,
+  cv::Mat & mat)
+{
+  cv_bridge::CvImageConstPtr cv_bridge_ptr = cv_bridge::toCvShare(msg, msg->encoding);
+  cv::Mat frame = cv_bridge_ptr->image;
+  if (msg->encoding == "rgb8") {
+    cv::cvtColor(frame, frame, cv::COLOR_RGB2BGR);
+  }
+  frame.copyTo(mat);
+}
+
+}      // namespace nav2_waypoint_follower
+PLUGINLIB_EXPORT_CLASS(
+  nav2_waypoint_follower::PhotoAtWaypoint,
+  nav2_core::WaypointTaskExecutor)
@@ -0,0 +1,87 @@
+// Copyright (c) 2020 Fetullah Atas
+//
+// 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 "nav2_waypoint_follower/plugins/wait_at_waypoint.hpp"
+
+#include <string>
+#include <exception>
+
+#include "pluginlib/class_list_macros.hpp"
+
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_waypoint_follower
+{
+WaitAtWaypoint::WaitAtWaypoint()
+: waypoint_pause_duration_(0),
+  is_enabled_(true)
+{
+}
+
+WaitAtWaypoint::~WaitAtWaypoint()
+{
+}
+
+void WaitAtWaypoint::initialize(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  const std::string & plugin_name)
+{
+  auto node = parent.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node in wait at waypoint plugin!"};
+  }
+  logger_ = node->get_logger();
+  clock_ = node->get_clock();
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    plugin_name + ".waypoint_pause_duration",
+    rclcpp::ParameterValue(0));
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    plugin_name + ".enabled",
+    rclcpp::ParameterValue(true));
+  node->get_parameter(
+    plugin_name + ".waypoint_pause_duration",
+    waypoint_pause_duration_);
+  node->get_parameter(
+    plugin_name + ".enabled",
+    is_enabled_);
+  if (waypoint_pause_duration_ == 0) {
+    is_enabled_ = false;
+    RCLCPP_INFO(
+      logger_,
+      "Waypoint pause duration is set to zero, disabling task executor plugin.");
+  } else if (!is_enabled_) {
+    RCLCPP_INFO(
+      logger_, "Waypoint task executor plugin is disabled.");
+  }
+}
+
+bool WaitAtWaypoint::processAtWaypoint(
+  const geometry_msgs::msg::PoseStamped & /*curr_pose*/, const int & curr_waypoint_index)
+{
+  if (!is_enabled_) {
+    return true;
+  }
+  RCLCPP_INFO(
+    logger_, "Arrived at %i'th waypoint, sleeping for %i milliseconds",
+    curr_waypoint_index,
+    waypoint_pause_duration_);
+  clock_->sleep_for(std::chrono::milliseconds(waypoint_pause_duration_));
+  return true;
+}
+}  // namespace nav2_waypoint_follower
+PLUGINLIB_EXPORT_CLASS(
+  nav2_waypoint_follower::WaitAtWaypoint,
+  nav2_core::WaypointTaskExecutor)