From 9bcdc14ecf627b77800fb4da14324bd1c0125f04 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 21 Sep 2023 13:51:40 +0100 Subject: [PATCH] Fixes for compiling with clang on macOS amcl: declare void parameter for functions with no args Signed-off-by: Rhys Mainwaring amcl: remove unused variables Signed-off-by: Rhys Mainwaring behavior_tree: address clang compilation issues Signed-off-by: Rhys Mainwaring behaviors: add missing override specifier Signed-off-by: Rhys Mainwaring bt_navigator: add missing override specifier Signed-off-by: Rhys Mainwaring collision_monitor: address clang compilation issues Signed-off-by: Rhys Mainwaring constrained_smoother: address clang compilation issues Signed-off-by: Rhys Mainwaring controller: address clang compilation issues Signed-off-by: Rhys Mainwaring costmap_2d: add missing override specifier Signed-off-by: Rhys Mainwaring costmap_2d: address clang compilation issues Signed-off-by: Rhys Mainwaring costmap_2d: fix link issue for order_layer Signed-off-by: Rhys Mainwaring dwb_controller: fix clang compile issue, replace ulong with uint32_t Signed-off-by: Rhys Mainwaring map_server: replace std::experimental::filesystem Signed-off-by: Rhys Mainwaring map_server: remove dependency on stdc++fs Signed-off-by: Rhys Mainwaring waypoint_follower: address clang compilation issues Signed-off-by: Rhys Mainwaring waypoint_follower: remove dependency on stdc++fs Signed-off-by: Rhys Mainwaring waypoint_follower: replace std::experimental::filesystem Signed-off-by: Rhys Mainwaring smoother: address clang compilation issues Signed-off-by: Rhys Mainwaring smoother: remove unused variables Signed-off-by: Rhys Mainwaring system_tests: remove dependency on stdc++fs Signed-off-by: Rhys Mainwaring rotation_shim_controller: update percentage arg in setSpeedLimit to boolean Signed-off-by: Rhys Mainwaring planner: remove unused variables Signed-off-by: Rhys Mainwaring costmap_2d: address clang compilation issues Signed-off-by: Rhys Mainwaring mppi_controller: replace use of auto as function param with templates Signed-off-by: Rhys Mainwaring mppi_controller: address clang compilation issues Signed-off-by: Rhys Mainwaring costmap_2d: resolve clang issue with std::pair non-const copy Signed-off-by: Rhys Mainwaring --- nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp | 4 +- nav2_amcl/src/pf/pf.c | 5 -- nav2_amcl/src/pf/pf_vector.c | 4 +- .../nav2_behavior_tree/bt_action_node.hpp | 4 +- .../bt_cancel_action_node.hpp | 2 +- .../nav2_behavior_tree/bt_service_node.hpp | 4 +- .../plugins/condition/is_stuck_condition.cpp | 2 +- .../plugins/decorator/speed_controller.cpp | 2 +- .../plugins/drive_on_heading.hpp | 2 +- .../navigators/navigate_to_pose.hpp | 2 +- .../test/collision_monitor_node_test.cpp | 2 +- .../test/test_constrained_smoother.cpp | 2 +- nav2_controller/src/controller_server.cpp | 4 +- .../denoise/image_processing.hpp | 19 +++-- .../nav2_costmap_2d/inflation_layer.hpp | 2 +- nav2_costmap_2d/plugins/denoise_layer.cpp | 2 +- nav2_costmap_2d/src/clear_costmap_service.cpp | 6 +- .../test/regression/CMakeLists.txt | 3 + .../test/unit/denoise_layer_test.cpp | 3 +- .../test/unit/image_processing_test.cpp | 77 +++++++++++-------- .../nav_2d_utils/test/path_ops_test.cpp | 2 +- nav2_map_server/test/component/CMakeLists.txt | 3 - .../test/component/test_map_saver_node.cpp | 4 +- .../component/test_map_saver_publisher.cpp | 4 +- .../test/component/test_map_server_node.cpp | 4 +- .../test/map_saver_cli/CMakeLists.txt | 1 - .../test/map_saver_cli/test_map_saver_cli.cpp | 20 ++--- nav2_map_server/test/unit/CMakeLists.txt | 1 - nav2_map_server/test/unit/test_map_io.cpp | 4 +- .../test/optimizer_unit_tests.cpp | 2 +- nav2_mppi_controller/test/utils/factory.hpp | 34 +++++--- nav2_mppi_controller/test/utils/utils.hpp | 12 ++- nav2_mppi_controller/test/utils_test.cpp | 2 +- nav2_planner/src/planner_server.cpp | 1 - .../test/test_shim_controller.cpp | 2 +- nav2_smoother/src/nav2_smoother.cpp | 2 +- .../test/test_savitzky_golay_smoother.cpp | 7 +- nav2_smoother/test/test_smoother_server.cpp | 4 +- nav2_system_tests/src/planning/CMakeLists.txt | 2 +- nav2_waypoint_follower/CMakeLists.txt | 2 - .../plugins/photo_at_waypoint.hpp | 4 +- .../plugins/photo_at_waypoint.cpp | 8 +- .../test/test_task_executors.cpp | 4 +- 43 files changed, 149 insertions(+), 131 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp b/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp index c3b406cfd4..713478d24d 100644 --- 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 index de4e3247d1..9d1f5a8289 100644 --- 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 index a7a5cd39c6..00fa384060 100644 --- 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 index 44fe8b374e..7b0286dc3c 100644 --- 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(); + auto elapsed = (node_->now() - time_goal_sent_).template to_chrono(); 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(); + auto elapsed = (node_->now() - time_goal_sent_).template to_chrono(); 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 index e87ffbfac4..42d27c94ec 100644 --- 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 index e050ab3038..3537f9e9a2 100644 --- 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(); + auto elapsed = (node_->now() - sent_time_).template to_chrono(); 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(); + elapsed = (node_->now() - sent_time_).template to_chrono(); 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 index 211254dafe..a898dec947 100644 --- 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 index e5c96eba2c..1ab530dab1 100644 --- 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 index 79c8d81c71..7ffba907e5 100644 --- 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 * @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 index 20230aa10e..fbcfa2c39c 100644 --- 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 index 472530f786..c25b713f10 100644 --- 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{"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 index 463af33fd0..1ccae77f77 100644 --- 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( diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index baa9775348..9c6cb01a29 100644 --- 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 index ee79894847..75c720cbaa 100644 --- 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(image, buffer, label_trees, is_background); - const Label groups_count = components.second; - const Image