Merge remote-tracking branch 'upstream/main' into add_polygon_velocity

.github/workflows/lint.yml

@@ -0,0 +1,21 @@
+name: Lint
+on:
+  pull_request:
+
+jobs:
+  ament_lint_general:
+    name: ament_${{ matrix.linter }}
+    runs-on: ubuntu-latest
+    container:
+      image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest
+    strategy:
+      fail-fast: false
+      matrix:
+          linter: [xmllint, cpplint, uncrustify, pep257, flake8]
+    steps:
+    - uses: actions/checkout@v4
+    - uses: ros-tooling/[email protected]
+      with:
+        linter: ${{ matrix.linter }}
+        distribution: rolling
+        package-name: "*"

CONTRIBUTING.md

@@ -0,0 +1,18 @@
+Any contribution that you make to this repository will
+be under the Apache 2 License, as dictated by that
+[license](http://www.apache.org/licenses/LICENSE-2.0.html):
+
+~~~
+5. Submission of Contributions. Unless You explicitly state otherwise,
+   any Contribution intentionally submitted for inclusion in the Work
+   by You to the Licensor shall be under the terms and conditions of
+   this License, without any additional terms or conditions.
+   Notwithstanding the above, nothing herein shall supersede or modify
+   the terms of any separate license agreement you may have executed
+   with Licensor regarding such Contributions.
+~~~
+
+Contributors must sign-off each commit by adding a `Signed-off-by: ...`
+line to commit messages to certify that they have the right to submit
+the code they are contributing to the project according to the
+[Developer Certificate of Origin (DCO)](https://developercertificate.org/).

nav2_amcl/src/amcl_node.cpp

@@ -1167,7 +1167,7 @@ AmclNode::dynamicParametersCallback(
     if (param_type == ParameterType::PARAMETER_DOUBLE) {
       if (param_name == "alpha1") {
         alpha1_ = parameter.as_double();
-        //alpha restricted to be non-negative
+        // alpha restricted to be non-negative
         if (alpha1_ < 0.0) {
           RCLCPP_WARN(
             get_logger(), "You've set alpha1 to be negative,"
@@ -1177,7 +1177,7 @@ AmclNode::dynamicParametersCallback(
         reinit_odom = true;
       } else if (param_name == "alpha2") {
         alpha2_ = parameter.as_double();
-        //alpha restricted to be non-negative
+        // alpha restricted to be non-negative
         if (alpha2_ < 0.0) {
           RCLCPP_WARN(
             get_logger(), "You've set alpha2 to be negative,"
@@ -1187,7 +1187,7 @@ AmclNode::dynamicParametersCallback(
         reinit_odom = true;
       } else if (param_name == "alpha3") {
         alpha3_ = parameter.as_double();
-        //alpha restricted to be non-negative
+        // alpha restricted to be non-negative
         if (alpha3_ < 0.0) {
           RCLCPP_WARN(
             get_logger(), "You've set alpha3 to be negative,"
@@ -1197,7 +1197,7 @@ AmclNode::dynamicParametersCallback(
         reinit_odom = true;
       } else if (param_name == "alpha4") {
         alpha4_ = parameter.as_double();
-        //alpha restricted to be non-negative
+        // alpha restricted to be non-negative
         if (alpha4_ < 0.0) {
           RCLCPP_WARN(
             get_logger(), "You've set alpha4 to be negative,"
@@ -1207,7 +1207,7 @@ AmclNode::dynamicParametersCallback(
         reinit_odom = true;
       } else if (param_name == "alpha5") {
         alpha5_ = parameter.as_double();
-        //alpha restricted to be non-negative
+        // alpha restricted to be non-negative
         if (alpha5_ < 0.0) {
           RCLCPP_WARN(
             get_logger(), "You've set alpha5 to be negative,"

nav2_bringup/params/nav2_params.yaml

@@ -277,12 +277,34 @@ planner_server:
   ros__parameters:
     expected_planner_frequency: 20.0
     planner_plugins: ["GridBased"]
-    GridBased:
-      plugin: "nav2_navfn_planner/NavfnPlanner"
-      tolerance: 0.5
-      use_astar: false
-      allow_unknown: true
 
+    GridBased:
+      plugin: "nav2_smac_planner/SmacPlannerLattice"
+      allow_unknown: true                 # Allow traveling in unknown space
+      tolerance: 0.25                     # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
+      max_iterations: 1000000             # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
+      max_on_approach_iterations: 1000    # Maximum number of iterations after within tolerances to continue to try to find exact solution
+      max_planning_time: 5.0              # Max time in s for planner to plan, smooth
+      analytic_expansion_ratio: 3.5       # The ratio to attempt analytic expansions during search for final approach.
+      analytic_expansion_max_length: 3.0  # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
+      reverse_penalty: 2.0                # Penalty to apply if motion is reversing, must be => 1
+      change_penalty: 0.05                # Penalty to apply if motion is changing directions (L to R), must be >= 0
+      non_straight_penalty: 1.05          # Penalty to apply if motion is non-straight, must be => 1
+      cost_penalty: 2.0                   # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
+      rotation_penalty: 5.0               # Penalty to apply to in-place rotations, if minimum control set contains them
+      retrospective_penalty: 0.015
+      lookup_table_size: 20.0             # Size of the dubin/reeds-sheep distance window to cache, in meters.
+      cache_obstacle_heuristic: false     # Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
+      allow_reverse_expansion: false      # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse).
+      debug_visualizations: true
+      smooth_path: True                   # If true, does a simple and quick smoothing post-processing to the path
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.3
+        w_data: 0.2
+        tolerance: 1.0e-10
+        do_refinement: true
+        refinement_num: 2
 smoother_server:
   ros__parameters:
     smoother_plugins: ["simple_smoother"]

nav2_collision_monitor/src/polygon.cpp

@@ -409,7 +409,6 @@ bool Polygon::getParameters(
       // Do not need to proceed further, if "points" parameter is defined.
       // Static polygon will be used.
       return getPolygonFromString(poly_string, poly_);
-
     } catch (const rclcpp::exceptions::ParameterUninitializedException &) {
       RCLCPP_INFO(
         logger_,

nav2_collision_monitor/test/collision_detector_node_test.cpp

@@ -162,9 +162,9 @@ class Tester : public ::testing::Test
   nav2_msgs::msg::CollisionDetectorState::SharedPtr state_msg_;
 
   // CollisionMonitor collision points markers
-  rclcpp::Subscription<visualization_msgs::msg::MarkerArray>::SharedPtr collision_points_marker_sub_;
+  rclcpp::Subscription<visualization_msgs::msg::MarkerArray>::SharedPtr
+    collision_points_marker_sub_;
   visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_;
-
 };  // Tester
 
 Tester::Tester()

nav2_collision_monitor/test/collision_monitor_node_test.cpp

@@ -195,7 +195,8 @@ class Tester : public ::testing::Test
   nav2_msgs::msg::CollisionMonitorState::SharedPtr action_state_;
 
   // CollisionMonitor collision points markers
-  rclcpp::Subscription<visualization_msgs::msg::MarkerArray>::SharedPtr collision_points_marker_sub_;
+  rclcpp::Subscription<visualization_msgs::msg::MarkerArray>::SharedPtr
+    collision_points_marker_sub_;
   visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_;
 
   // Service client for setting CollisionMonitor parameters

nav2_common/nav2_common/launch/rewritten_yaml.py

@@ -20,6 +20,7 @@
 
 
 class DictItemReference:
+
     def __init__(self, dictionary, key):
         self.dictionary = dictionary
         self.dictKey = key

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -77,7 +77,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode
    * @brief  Constructor for the wrapper
    * @param options Additional options to control creation of the node.
    */
-  Costmap2DROS(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  explicit Costmap2DROS(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
   /**
    * @brief  Constructor for the wrapper, the node will
@@ -399,12 +399,12 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   double resolution_{0};
   std::string robot_base_frame_;            ///< The frame_id of the robot base
   double robot_radius_;
-  bool rolling_window_{false};              ///< Whether to use a rolling window version of the costmap
+  bool rolling_window_{false};          ///< Whether to use a rolling window version of the costmap
   bool track_unknown_space_{false};
   double transform_tolerance_{0};           ///< The timeout before transform errors
   double initial_transform_timeout_{0};   ///< The timeout before activation of the node errors
 
-  bool is_lifecycle_follower_{true};     ///< whether is a child-LifecycleNode or an independent node
+  bool is_lifecycle_follower_{true};   ///< whether is a child-LifecycleNode or an independent node
 
   // Derived parameters
   bool use_radius_{false};

nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp

@@ -81,7 +81,7 @@ void morphologyOperation(
 
 using ShapeBuffer3x3 = std::array<uint8_t, 9>;  // NOLINT
 inline Image<uint8_t> createShape(ShapeBuffer3x3 & buffer, ConnectivityType connectivity);
-} // namespace imgproc_impl
+}  // namespace imgproc_impl
 
 /**
  * @brief Perform morphological dilation

nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp

@@ -40,6 +40,7 @@
 
 #include <string>
 #include <vector>
+#include <utility>
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/polygon.hpp"

nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

@@ -131,7 +131,7 @@ class InflationLayer : public Layer
   /**
    * @brief If clearing operations should be processed on this layer or not
    */
-  virtual bool isClearable() override {return false;}
+  bool isClearable() override {return false;}
 
   /**
    * @brief Reset this costmap

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -289,7 +289,9 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/)
     // Check timeout
     if (now() > initial_transform_timeout_point) {
       RCLCPP_ERROR(
-        get_logger(), "Failed to activate %s because transform from %s to %s did not become available before timeout",
+        get_logger(),
+        "Failed to activate %s because "
+        "transform from %s to %s did not become available before timeout",
         get_name(), robot_base_frame_.c_str(), global_frame_.c_str());
 
       return nav2_util::CallbackReturn::FAILURE;
@@ -523,7 +525,7 @@ Costmap2DROS::mapUpdateLoop(double frequency)
         layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
         costmap_publisher_->updateBounds(x0, xn, y0, yn);
 
-        for (auto & layer_pub: layer_publishers_) {
+        for (auto & layer_pub : layer_publishers_) {
           layer_pub->updateBounds(x0, xn, y0, yn);
         }
 
@@ -535,7 +537,7 @@ Costmap2DROS::mapUpdateLoop(double frequency)
           RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str());
           costmap_publisher_->publishCostmap();
 
-          for (auto & layer_pub: layer_publishers_) {
+          for (auto & layer_pub : layer_publishers_) {
             layer_pub->publishCostmap();
           }
 

nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp

@@ -162,7 +162,7 @@ TEST_F(CostmapRosTestFixture, costmap_pub_test)
   auto costmap_raw = future.get();
 
   // Check first 20 cells of the 10by10 map
-  ASSERT_TRUE(costmap_raw->data.size() == 100);
+  ASSERT_EQ(costmap_raw->data.size(), 100u);
   unsigned int i = 0;
   for (; i < 7; ++i) {
     EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::FREE_SPACE);

nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp

@@ -1,3 +1,17 @@
+// Copyright (c) 2020 Samsung Research
+//
+// 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 <gtest/gtest.h>
 
 #include "rclcpp/rclcpp.hpp"
@@ -119,7 +133,7 @@ TEST_F(TestCostmapSubscriberShould, handleFullCostmapMsgs)
     node, costmapToSend.get(), "", topicName, always_send_full_costmap);
   costmapPublisher->on_activate();
 
-  for (const auto & mapChange :mapChanges) {
+  for (const auto & mapChange : mapChanges) {
     for (const auto & observation : mapChange.observations) {
       costmapToSend->setCost(observation.x, observation.y, observation.cost);
     }
@@ -153,7 +167,7 @@ TEST_F(TestCostmapSubscriberShould, handleCostmapUpdateMsgs)
     node, costmapToSend.get(), "", topicName, always_send_full_costmap);
   costmapPublisher->on_activate();
 
-  for (const auto & mapChange :mapChanges) {
+  for (const auto & mapChange : mapChanges) {
     for (const auto & observation : mapChange.observations) {
       costmapToSend->setCost(observation.x, observation.y, observation.cost);
     }

nav2_costmap_2d/test/regression/order_layer.hpp

@@ -12,8 +12,8 @@
 // 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_
+#ifndef REGRESSION__ORDER_LAYER_HPP_
+#define REGRESSION__ORDER_LAYER_HPP_
 
 #include "nav2_costmap_2d/layer.hpp"
 
@@ -43,4 +43,4 @@ class OrderLayer : public nav2_costmap_2d::Layer
 
 }  // namespace nav2_costmap_2d
 
-#endif  // NAV2_COSTMAP_2D__ORDER_LAYER_HPP_
+#endif  // REGRESSION__ORDER_LAYER_HPP_

nav2_costmap_2d/test/regression/plugin_api_order.cpp

@@ -16,8 +16,8 @@
 #include <vector>
 #include <memory>
 
-#include <nav2_costmap_2d/costmap_2d_ros.hpp>
-#include <gtest/gtest.h>
+#include "gtest/gtest.h"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
 
 TEST(CostmapPluginsTester, checkPluginAPIOrder)
 {

nav2_costmap_2d/test/unit/denoise_layer_test.cpp

@@ -102,9 +102,9 @@ class DenoiseLayerTester : public ::testing::Test
   nav2_costmap_2d::DenoiseLayer denoise_;
 };
 
-}
+}  // namespace nav2_costmap_2d
 
-using namespace nav2_costmap_2d;
+using namespace nav2_costmap_2d;  // NOLINT
 
 TEST_F(DenoiseLayerTester, removeSinglePixels4way) {
   const auto in = imageFromString<uint8_t>(

nav2_costmap_2d/test/unit/image_processing_test.cpp

@@ -18,8 +18,8 @@
 #include "nav2_costmap_2d/denoise/image_processing.hpp"
 #include "image_tests_helper.hpp"
 
-using namespace nav2_costmap_2d;
-using namespace imgproc_impl;
+using namespace nav2_costmap_2d;  // NOLINT
+using namespace imgproc_impl;  // NOLINT
 
 struct ImageProcTester : public ::testing::Test
 {

nav2_costmap_2d/test/unit/image_test.cpp

@@ -17,7 +17,7 @@
 #include "nav2_costmap_2d/denoise/image.hpp"
 #include "image_tests_helper.hpp"
 
-using namespace nav2_costmap_2d;
+using namespace nav2_costmap_2d;  // NOLINT
 
 struct ImageTester : public ::testing::Test
 {

nav2_costmap_2d/test/unit/image_tests_helper.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_COSTMAP_2D__IMAGE_TESTS_HELPER_HPP_
-#define NAV2_COSTMAP_2D__IMAGE_TESTS_HELPER_HPP_
+#ifndef UNIT__IMAGE_TESTS_HELPER_HPP_
+#define UNIT__IMAGE_TESTS_HELPER_HPP_
 
 #include "nav2_costmap_2d/denoise/image.hpp"
 #include <cmath>
@@ -121,4 +121,4 @@ std::ostream & operator<<(std::ostream & out, const Image<T> & image)
 
 }  // namespace nav2_costmap_2d
 
-#endif  // NAV2_COSTMAP_2D__IMAGE_TESTS_HELPER_HPP_
+#endif  // UNIT__IMAGE_TESTS_HELPER_HPP_

nav2_costmap_2d/test/unit/lifecycle_test.cpp

@@ -1,3 +1,5 @@
+// Copyright (c) 2020 Samsung Research
+//
 // 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
@@ -8,7 +10,7 @@
 // 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.
+// limitations under the License. Reserved.
 
 #include <memory>
 

nav2_mppi_controller/README.md

@@ -107,6 +107,7 @@ This process is then repeated a number of times and returns a converged solution
  | collision_cost       | double | Default 100000.0. Cost to apply to a true collision in a trajectory.                                          |
  | collision_margin_distance   | double    | Default 0.10. Margin distance from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable. |
  | near_goal_distance          | double    | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles.   
+ | inflation_layer_name        | string    | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. |
 
 #### Path Align Critic
  | Parameter                  | Type   | Definition                                                                                                                         |