add humble-navigation2

navigation2/nav2_costmap_2d/src/layer.cpp

@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) 2013, Willow Garage, Inc.
+ * 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 Willow Garage, Inc. 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 "nav2_costmap_2d/layer.hpp"
+
+#include <string>
+#include <vector>
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_costmap_2d
+{
+
+Layer::Layer()
+: layered_costmap_(nullptr),
+  name_(),
+  tf_(nullptr),
+  current_(false),
+  enabled_(false)
+{}
+
+void
+Layer::initialize(
+  LayeredCostmap * parent,
+  std::string name,
+  tf2_ros::Buffer * tf,
+  const nav2_util::LifecycleNode::WeakPtr & node,
+  rclcpp::CallbackGroup::SharedPtr callback_group)
+{
+  layered_costmap_ = parent;
+  name_ = name;
+  tf_ = tf;
+  node_ = node;
+  callback_group_ = callback_group;
+  {
+    auto node_shared_ptr = node_.lock();
+    logger_ = node_shared_ptr->get_logger();
+    clock_ = node_shared_ptr->get_clock();
+  }
+
+  onInitialize();
+}
+
+const std::vector<geometry_msgs::msg::Point> &
+Layer::getFootprint() const
+{
+  return layered_costmap_->getFootprint();
+}
+
+void
+Layer::declareParameter(
+  const std::string & param_name,
+  const rclcpp::ParameterValue & value)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+  local_params_.insert(param_name);
+  nav2_util::declare_parameter_if_not_declared(
+    node, getFullName(param_name), value);
+}
+
+void
+Layer::declareParameter(
+  const std::string & param_name,
+  const rclcpp::ParameterType & param_type)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+  local_params_.insert(param_name);
+  nav2_util::declare_parameter_if_not_declared(
+    node, getFullName(param_name), param_type);
+}
+
+bool
+Layer::hasParameter(const std::string & param_name)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+  return node->has_parameter(getFullName(param_name));
+}
+
+std::string
+Layer::getFullName(const std::string & param_name)
+{
+  return std::string(name_ + "." + param_name);
+}
+
+}  // end namespace nav2_costmap_2d