ros-navigation · SteveMacenski · Jan 24, 2024 · Sep 21, 2023
diff --git a/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp b/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp
@@ -49,7 +49,7 @@ typedef struct
 
 
 // Return a zero vector
-pf_vector_t pf_vector_zero();
+pf_vector_t pf_vector_zero(void);
 
 // Check for NAN or INF in any component
 // int pf_vector_finite(pf_vector_t a);
@@ -71,7 +71,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b);
 
 
 // Return a zero matrix
-pf_matrix_t pf_matrix_zero();
+pf_matrix_t pf_matrix_zero(void);
 
 // Check for NAN or INF in any component
 // int pf_matrix_finite(pf_matrix_t a);

diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c
@@ -463,7 +463,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
 
   // Workspace
   double m[4], c[2][2];
-  size_t count;
   double weight;
 
   // Cluster the samples
@@ -474,7 +473,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
 
   for (i = 0; i < set->cluster_max_count; i++) {
     cluster = set->clusters + i;
-    cluster->count = 0;
     cluster->weight = 0;
     cluster->mean = pf_vector_zero();
     cluster->cov = pf_matrix_zero();
@@ -490,7 +488,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
   }
 
   // Initialize overall filter stats
-  count = 0;
   weight = 0.0;
   set->mean = pf_vector_zero();
   set->cov = pf_matrix_zero();
@@ -521,10 +518,8 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
 
     cluster = set->clusters + cidx;
 
-    cluster->count += 1;
     cluster->weight += sample->weight;
 
-    count += 1;
     weight += sample->weight;
 
     // Compute mean

diff --git a/nav2_amcl/src/pf/pf_vector.c b/nav2_amcl/src/pf/pf_vector.c
@@ -35,7 +35,7 @@
 
 
 // Return a zero vector
-pf_vector_t pf_vector_zero()
+pf_vector_t pf_vector_zero(void)
 {
   pf_vector_t c;
 
@@ -130,7 +130,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)
 
 
 // Return a zero matrix
-pf_matrix_t pf_matrix_zero()
+pf_matrix_t pf_matrix_zero(void)
 {
   int i, j;
   pf_matrix_t c;

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
@@ -206,7 +206,7 @@ class BtActionNode : public BT::ActionNodeBase
       // if new goal was sent and action server has not yet responded
       // check the future goal handle
       if (future_goal_handle_) {
-        auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
+        auto elapsed = (node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>();
         if (!is_future_goal_handle_complete(elapsed)) {
           // return RUNNING if there is still some time before timeout happens
           if (elapsed < server_timeout_) {
@@ -236,7 +236,7 @@ class BtActionNode : public BT::ActionNodeBase
         {
           goal_updated_ = false;
           send_new_goal();
-          auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
+          auto elapsed = (node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>();
           if (!is_future_goal_handle_complete(elapsed)) {
             if (elapsed < server_timeout_) {
               return BT::NodeStatus::RUNNING;

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp
@@ -116,7 +116,7 @@ class BtCancelActionNode : public BT::ActionNodeBase
     return basic;
   }
 
-  void halt()
+  void halt() override
   {
   }
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
@@ -183,7 +183,7 @@ class BtServiceNode : public BT::ActionNodeBase
    */
   virtual BT::NodeStatus check_future()
   {
-    auto elapsed = (node_->now() - sent_time_).to_chrono<std::chrono::milliseconds>();
+    auto elapsed = (node_->now() - sent_time_).template to_chrono<std::chrono::milliseconds>();
     auto remaining = server_timeout_ - elapsed;
 
     if (remaining > std::chrono::milliseconds(0)) {
@@ -199,7 +199,7 @@ class BtServiceNode : public BT::ActionNodeBase
 
       if (rc == rclcpp::FutureReturnCode::TIMEOUT) {
         on_wait_for_result();
-        elapsed = (node_->now() - sent_time_).to_chrono<std::chrono::milliseconds>();
+        elapsed = (node_->now() - sent_time_).template to_chrono<std::chrono::milliseconds>();
         if (elapsed < server_timeout_) {
           return BT::NodeStatus::RUNNING;
         }

diff --git a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp
@@ -95,7 +95,7 @@ void IsStuckCondition::logStuck(const std::string & msg) const
     return;
   }
 
-  RCLCPP_INFO(node_->get_logger(), msg.c_str());
+  RCLCPP_INFO(node_->get_logger(), "%s", msg.c_str());
   prev_msg = msg;
 }
 

diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp
@@ -43,7 +43,7 @@ SpeedController::SpeedController(
 
   if (min_rate_ <= 0.0 || max_rate_ <= 0.0) {
     std::string err_msg = "SpeedController node cannot have rate <= 0.0";
-    RCLCPP_FATAL(node_->get_logger(), err_msg.c_str());
+    RCLCPP_FATAL(node_->get_logger(), "%s" ,err_msg.c_str());
     throw BT::BehaviorTreeException(err_msg);
   }
 

diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp
@@ -91,7 +91,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
    * @brief Loop function to run behavior
    * @return Status of behavior
    */
-  Status onCycleUpdate()
+  Status onCycleUpdate() override
   {
     rclcpp::Duration time_remaining = end_time_ - this->steady_clock_.now();
     if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {

diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp
@@ -72,7 +72,7 @@ class NavigateToPoseNavigator
    * @brief Get action name for this navigator
    * @return string Name of action server
    */
-  std::string getName() {return std::string("navigate_to_pose");}
+  std::string getName() override {return std::string("navigate_to_pose");}
 
   /**
    * @brief Get navigator's default BT

diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
@@ -910,7 +910,7 @@ TEST_F(Tester, testSourcesNotSet)
   setCommonParameters();
   addPolygon("Stop", POLYGON, 1.0, "stop");
   addSource(SCAN_NAME, SCAN);
-  cm_->declare_parameter("polygons", rclcpp::ParameterValue({"Stop"}));
+  cm_->declare_parameter("polygons", rclcpp::ParameterValue("Stop"));
   cm_->set_parameter(rclcpp::Parameter("polygons", std::vector<std::string>{"Stop"}));
 
   // Check that Collision Monitor node can not be configured for this parameters set

diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp
@@ -100,7 +100,7 @@ class SmootherTest : public ::testing::Test
   SmootherTest() {SetUp();}
   ~SmootherTest() {}
 
-  void SetUp()
+  void SetUp() override
   {
     node_lifecycle_ =
       std::make_shared<rclcpp_lifecycle::LifecycleNode>(

diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp
@@ -424,7 +424,7 @@ void ControllerServer::computeControl()
       }
     }
   } catch (nav2_core::PlannerException & e) {
-    RCLCPP_ERROR(this->get_logger(), e.what());
+    RCLCPP_ERROR(this->get_logger(), "%s", e.what());
     publishZeroVelocity();
     action_server_->terminate_current();
     return;
@@ -490,7 +490,7 @@ void ControllerServer::computeAndPublishVelocity()
     last_valid_cmd_time_ = now();
   } catch (nav2_core::PlannerException & e) {
     if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) {
-      RCLCPP_WARN(this->get_logger(), e.what());
+      RCLCPP_WARN(this->get_logger(), "%s", e.what());
       cmd_vel_2d.twist.angular.x = 0;
       cmd_vel_2d.twist.angular.y = 0;
       cmd_vel_2d.twist.angular.z = 0;

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp
@@ -986,9 +986,9 @@ class GroupsRemover
     const IsBg & is_background) const
   {
     // Creates an image labels in which each obstacles group is labeled with a unique code
-    auto components = connectedComponents<connectivity>(image, buffer, label_trees, is_background);
-    const Label groups_count = components.second;
-    const Image<Label> & labels = components.first;
+    Label groups_count;
+    auto labels = connectedComponents<connectivity>(image, buffer, label_trees,
+        is_background, groups_count);
 
     // Calculates the size of each group.
     // Group size is equal to the number of pixels with the same label
@@ -1026,24 +1026,27 @@ class GroupsRemover
 }  // namespace imgproc_impl
 
 template<ConnectivityType connectivity, class Label, class IsBg>
-std::pair<Image<Label>, Label> connectedComponents(
+Image<Label> connectedComponents(
   const Image<uint8_t> & image, MemoryBuffer & buffer,
-  imgproc_impl::EquivalenceLabelTrees<Label> & label_trees, const IsBg & is_background)
+  imgproc_impl::EquivalenceLabelTrees<Label> & label_trees,
+  const IsBg & is_background,
+  Label & total_labels)
 {
   using namespace imgproc_impl;
   const size_t pixels = image.rows() * image.columns();
 
   if (pixels == 0) {
-    return {Image<Label>{}, 0};
+    total_labels = 0;
+    return Image<Label>{};
   }
 
   Label * image_buffer = buffer.get<Label>(pixels);
   Image<Label> labels(image.rows(), image.columns(), image_buffer, image.columns());
   label_trees.reset(image.rows(), image.columns(), connectivity);
-  const Label total_labels = connectedComponentsImpl<connectivity>(
+  total_labels = connectedComponentsImpl<connectivity>(
     image, labels, label_trees,
     is_background);
-  return std::make_pair(labels, total_labels);
+  return labels;
 }
 
 }  // namespace nav2_costmap_2d

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/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() {return false;}
+  virtual bool isClearable() override {return false;}
 
   /**
    * @brief Reset this costmap

diff --git a/nav2_costmap_2d/plugins/denoise_layer.cpp b/nav2_costmap_2d/plugins/denoise_layer.cpp
@@ -122,7 +122,7 @@ DenoiseLayer::updateCosts(
   try {
     denoise(roi_image);
   } catch (std::exception & ex) {
-    RCLCPP_ERROR(logger_, (std::string("Inner error: ") + ex.what()).c_str());
+    RCLCPP_ERROR(logger_, "%s", (std::string("Inner error: ") + ex.what()).c_str());
   }
 
   current_ = true;

diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp
@@ -67,7 +67,7 @@ void ClearCostmapService::clearExceptRegionCallback(
   const shared_ptr<ClearExceptRegion::Response>/*response*/)
 {
   RCLCPP_INFO(
-    logger_,
+    logger_, "%s",
     ("Received request to clear except a region the " + costmap_.getName()).c_str());
 
   clearRegion(request->reset_distance, true);
@@ -87,7 +87,7 @@ void ClearCostmapService::clearEntireCallback(
   const std::shared_ptr<ClearEntirely::Response>/*response*/)
 {
   RCLCPP_INFO(
-    logger_,
+    logger_, "%s",
     ("Received request to clear entirely the " + costmap_.getName()).c_str());
 
   clearEntirely();
@@ -99,7 +99,7 @@ void ClearCostmapService::clearRegion(const double reset_distance, bool invert)
 
   if (!getPosition(x, y)) {
     RCLCPP_ERROR(
-      logger_,
+      logger_, "%s",
       "Cannot clear map because robot pose cannot be retrieved.");
     return;
   }

diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt
@@ -10,6 +10,9 @@ add_library(order_layer SHARED
 ament_target_dependencies(order_layer
   ${dependencies}
 )
+target_link_libraries(order_layer
+  nav2_costmap_2d_core
+)
 install(TARGETS
   order_layer
   ARCHIVE DESTINATION lib

diff --git a/nav2_costmap_2d/test/unit/denoise_layer_test.cpp b/nav2_costmap_2d/test/unit/denoise_layer_test.cpp
@@ -407,7 +407,8 @@ TEST_F(DenoiseLayerTester, denoiseNothing) {
 
 TEST_F(DenoiseLayerTester, constructorAndDestructor) {
   ASSERT_NO_THROW(
-    []() {
+    // []()
+    {
       nav2_costmap_2d::DenoiseLayer layer;
     });
 }