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,27 @@
+# Bresenham2D corner cases test
+ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp)
+target_link_libraries(costmap_bresenham_2d
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+)
+
+# OrderLayer for checking Costmap2D plugins API calling order
+add_library(order_layer SHARED
+            order_layer.cpp)
+ament_target_dependencies(order_layer
+  ${dependencies}
+)
+target_link_libraries(order_layer
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+)
+install(TARGETS
+  order_layer
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+# Costmap2D plugins API calling order test
+ament_add_gtest(plugin_api_order plugin_api_order.cpp)
+target_link_libraries(plugin_api_order
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+)
@@ -0,0 +1,159 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2022 Samsung Research Russia
+*  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 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.
+*
+* Author: Alexey Merzlyakov
+*********************************************************************/
+#include <nav2_costmap_2d/costmap_2d.hpp>
+#include <gtest/gtest.h>
+
+class CostmapAction
+{
+public:
+  explicit CostmapAction(
+    unsigned char * costmap, unsigned int size, unsigned char mark_val = 128)
+  : costmap_(costmap), size_(size), mark_val_(mark_val)
+  {
+  }
+
+  inline void operator()(unsigned int off)
+  {
+    ASSERT_TRUE(off < size_);
+    costmap_[off] = mark_val_;
+  }
+
+  inline unsigned int get(unsigned int off)
+  {
+    return costmap_[off];
+  }
+
+private:
+  unsigned char * costmap_;
+  unsigned int size_;
+  unsigned char mark_val_;
+};
+
+class CostmapTest : public nav2_costmap_2d::Costmap2D
+{
+public:
+  CostmapTest(
+    unsigned int size_x, unsigned int size_y, double resolution,
+    double origin_x, double origin_y, unsigned char default_val = 0)
+  : nav2_costmap_2d::Costmap2D(size_x, size_y, resolution, origin_x, origin_y, default_val)
+  {
+  }
+
+  unsigned char * getCostmap()
+  {
+    return costmap_;
+  }
+
+  unsigned int getSize()
+  {
+    return size_x_ * size_y_;
+  }
+
+  void raytraceLine(
+    CostmapAction ca, unsigned int x0, unsigned int y0, unsigned int x1,
+    unsigned int y1,
+    unsigned int max_length = UINT_MAX, unsigned int min_length = 0)
+  {
+    nav2_costmap_2d::Costmap2D::raytraceLine(ca, x0, y0, x1, y1, max_length, min_length);
+  }
+};
+
+TEST(costmap_2d, bresenham2DBoundariesCheck)
+{
+  const unsigned int sz_x = 60;
+  const unsigned int sz_y = 60;
+  const unsigned int max_length = 60;
+  const unsigned int min_length = 6;
+  CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0);
+  CostmapAction ca(ct.getCostmap(), ct.getSize());
+
+  // Initial point - some assymetrically standing point in order to cover most corner cases
+  const unsigned int x0 = 2;
+  const unsigned int y0 = 4;
+  // (x1, y1) point will move
+  unsigned int x1, y1;
+
+  // Running on (x, 0) edge
+  y1 = 0;
+  for (x1 = 0; x1 < sz_x; x1++) {
+    ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length);
+  }
+
+  // Running on (x, sz_y) edge
+  y1 = sz_y - 1;
+  for (x1 = 0; x1 < sz_x; x1++) {
+    ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length);
+  }
+
+  // Running on (0, y) edge
+  x1 = 0;
+  for (y1 = 0; y1 < sz_y; y1++) {
+    ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length);
+  }
+
+  // Running on (sz_x, y) edge
+  x1 = sz_x - 1;
+  for (y1 = 0; y1 < sz_y; y1++) {
+    ct.raytraceLine(ca, x0, y0, x1, y1, max_length, min_length);
+  }
+}
+
+TEST(costmap_2d, bresenham2DSamePoint)
+{
+  const unsigned int sz_x = 60;
+  const unsigned int sz_y = 60;
+  const unsigned int max_length = 60;
+  const unsigned int min_length = 0;
+  CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0);
+  CostmapAction ca(ct.getCostmap(), ct.getSize());
+
+  // Initial point
+  const double x0 = 2;
+  const double y0 = 4;
+
+  unsigned int offset = y0 * sz_x + x0;
+  unsigned char val_before = ca.get(offset);
+  // Same point to check
+  ct.raytraceLine(ca, x0, y0, x0, y0, max_length, min_length);
+  unsigned char val_after = ca.get(offset);
+  ASSERT_FALSE(val_before == val_after);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
@@ -0,0 +1,60 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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 "order_layer.hpp"
+
+#include <chrono>
+#include <stdexcept>
+
+using namespace std::chrono_literals;
+
+namespace nav2_costmap_2d
+{
+
+OrderLayer::OrderLayer()
+: activated_(false)
+{
+}
+
+void OrderLayer::activate()
+{
+  std::this_thread::sleep_for(100ms);
+  activated_ = true;
+}
+
+void OrderLayer::deactivate()
+{
+  activated_ = false;
+}
+
+void OrderLayer::updateBounds(
+  double, double, double, double *, double *, double *, double *)
+{
+  if (!activated_) {
+    throw std::runtime_error("update before activated");
+  }
+}
+
+void OrderLayer::updateCosts(
+  nav2_costmap_2d::Costmap2D &, int, int, int, int)
+{
+  if (!activated_) {
+    throw std::runtime_error("update before activated");
+  }
+}
+
+}  // namespace nav2_costmap_2d
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::OrderLayer, nav2_costmap_2d::Layer)
@@ -0,0 +1,46 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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.
+
+#ifndef NAV2_COSTMAP_2D__ORDER_LAYER_HPP_
+#define NAV2_COSTMAP_2D__ORDER_LAYER_HPP_
+
+#include "nav2_costmap_2d/layer.hpp"
+
+namespace nav2_costmap_2d
+{
+
+class OrderLayer : public nav2_costmap_2d::Layer
+{
+public:
+  OrderLayer();
+
+  virtual void activate();
+  virtual void deactivate();
+
+  virtual void reset() {}
+  virtual bool isClearable() {return false;}
+
+  virtual void updateBounds(
+    double, double, double, double *, double *, double *, double *);
+
+  virtual void updateCosts(
+    nav2_costmap_2d::Costmap2D &, int, int, int, int);
+
+private:
+  bool activated_;
+};
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__ORDER_LAYER_HPP_
@@ -0,0 +1,7 @@
+<class_libraries>
+  <library path="order_layer">
+    <class type="nav2_costmap_2d::OrderLayer" base_class_type="nav2_costmap_2d::Layer">
+      <description>Plugin checking order of activate() and updateBounds()/updateCosts() calls</description>
+    </class>
+  </library>
+</class_libraries>
@@ -0,0 +1,53 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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 <string>
+#include <vector>
+#include <memory>
+
+#include <nav2_costmap_2d/costmap_2d_ros.hpp>
+#include <gtest/gtest.h>
+
+TEST(CostmapPluginsTester, checkPluginAPIOrder)
+{
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
+    std::make_shared<nav2_costmap_2d::Costmap2DROS>("costmap_ros");
+
+  // Workaround to avoid setting base_link->map transform
+  costmap_ros->set_parameter(rclcpp::Parameter("robot_base_frame", "map"));
+  // Specifying order verification plugin in the parameters
+  std::vector<std::string> plugins_str;
+  plugins_str.push_back("order_layer");
+  costmap_ros->set_parameter(rclcpp::Parameter("plugins", plugins_str));
+  costmap_ros->declare_parameter(
+    "order_layer.plugin",
+    rclcpp::ParameterValue(std::string("nav2_costmap_2d::OrderLayer")));
+
+  // Do actual test: ensure that plugin->updateBounds()/updateCosts()
+  // will be called after plugin->activate()
+  costmap_ros->on_configure(costmap_ros->get_current_state());
+  costmap_ros->on_activate(costmap_ros->get_current_state());
+
+  // Do cleanup
+  costmap_ros->on_deactivate(costmap_ros->get_current_state());
+  costmap_ros->on_cleanup(costmap_ros->get_current_state());
+  costmap_ros->on_shutdown(costmap_ros->get_current_state());
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}