diff --git a/README.md b/README.md index 488eb280427..fe0364bc3b8 100644 --- a/README.md +++ b/README.md @@ -9,8 +9,8 @@ For detailed instructions on how to: - [Getting Started](https://navigation.ros.org/getting_started/index.html) - [Concepts](https://navigation.ros.org/concepts/index.html) -- [Build](https://navigation.ros.org/build_instructions/index.html#build) -- [Install](https://navigation.ros.org/build_instructions/index.html#install) +- [Build](https://navigation.ros.org/development_guides/build_docs/index.html#build) +- [Install](https://navigation.ros.org/development_guides/build_docs/index.html#install) - [General Tutorials](https://navigation.ros.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://navigation.ros.org/plugin_tutorials/index.html) - [Configure](https://navigation.ros.org/configuration/index.html) - [Navigation Plugins](https://navigation.ros.org/plugins/index.html) @@ -20,6 +20,8 @@ For detailed instructions on how to: Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate). +If you need professional services related to Nav2, please contact Open Navigation at info@opennav.org. + ## Our Sponsors Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community. diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 2360d09c253..030a85f38cd 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -35,6 +35,7 @@ #include "nav2_amcl/sensors/laser/laser.hpp" #include "nav2_msgs/msg/particle.hpp" #include "nav2_msgs/msg/particle_cloud.hpp" +#include "nav2_msgs/srv/set_initial_pose.hpp" #include "nav_msgs/srv/set_map.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "std_srvs/srv/empty.hpp" @@ -210,6 +211,16 @@ class AmclNode : public nav2_util::LifecycleNode const std::shared_ptr request, std::shared_ptr response); + // service server for providing an initial pose guess + rclcpp::Service::SharedPtr initial_guess_srv_; + /* + * @brief Service callback for an initial pose guess request + */ + void initialPoseReceivedSrv( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + // Let amcl update samples without requiring motion rclcpp::Service::SharedPtr nomotion_update_srv_; /* diff --git a/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp b/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp index c3b406cfd43..713478d24d3 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/package.xml b/nav2_amcl/package.xml index 5725a6e6345..1f5199979a0 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.1.12 + 1.1.14

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 65df56dac15..ba2010d507e 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -325,13 +325,17 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // Get rid of the inputs first (services and message filter input), so we // don't continue to process incoming messages global_loc_srv_.reset(); + initial_guess_srv_.reset(); nomotion_update_srv_.reset(); + executor_thread_.reset(); // to make sure initial_pose_sub_ completely exit initial_pose_sub_.reset(); laser_scan_connection_.disconnect(); + tf_listener_.reset(); // listener may access lase_scan_filter_, so it should be reset earlier laser_scan_filter_.reset(); laser_scan_sub_.reset(); // Map + map_sub_.reset(); // map_sub_ may access map_, so it should be reset earlier if (map_ != NULL) { map_free(map_); map_ = nullptr; @@ -341,7 +345,6 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // Transforms tf_broadcaster_.reset(); - tf_listener_.reset(); tf_buffer_.reset(); // PubSub @@ -493,6 +496,15 @@ AmclNode::globalLocalizationCallback( pf_init_ = false; } +void +AmclNode::initialPoseReceivedSrv( + const std::shared_ptr/*request_header*/, + const std::shared_ptr req, + std::shared_ptr/*res*/) +{ + initialPoseReceived(std::make_shared(req->pose)); +} + // force nomotion updates (amcl updating without requiring motion) void AmclNode::nomotionUpdateCallback( @@ -1093,20 +1105,20 @@ AmclNode::initParameters() // Semantic checks if (laser_likelihood_max_dist_ < 0) { RCLCPP_WARN( - get_logger(), "You've set laser_likelihood_max_dist to be negtive," + get_logger(), "You've set laser_likelihood_max_dist to be negative," " this isn't allowed so it will be set to default value 2.0."); laser_likelihood_max_dist_ = 2.0; } if (max_particles_ < 0) { RCLCPP_WARN( - get_logger(), "You've set max_particles to be negtive," + get_logger(), "You've set max_particles to be negative," " this isn't allowed so it will be set to default value 2000."); max_particles_ = 2000; } if (min_particles_ < 0) { RCLCPP_WARN( - get_logger(), "You've set min_particles to be negtive," + get_logger(), "You've set min_particles to be negative," " this isn't allowed so it will be set to default value 500."); min_particles_ = 500; } @@ -1120,7 +1132,7 @@ AmclNode::initParameters() if (resample_interval_ <= 0) { RCLCPP_WARN( - get_logger(), "You've set resample_interval to be zero or negtive," + get_logger(), "You've set resample_interval to be zero or negative," " this isn't allowed so it will be set to default value to 1."); resample_interval_ = 1; } @@ -1533,6 +1545,10 @@ AmclNode::initServices() "reinitialize_global_localization", std::bind(&AmclNode::globalLocalizationCallback, this, _1, _2, _3)); + initial_guess_srv_ = create_service( + "set_initial_pose", + std::bind(&AmclNode::initialPoseReceivedSrv, this, _1, _2, _3)); + nomotion_update_srv_ = create_service( "request_nomotion_update", std::bind(&AmclNode::nomotionUpdateCallback, this, _1, _2, _3)); diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c index de4e3247d10..9d1f5a82892 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 a7a5cd39c61..00fa3840607 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_amcl/src/sensors/laser/likelihood_field_model.cpp b/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp index 491e64ecf1e..1148c2828db 100644 --- a/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp +++ b/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp @@ -53,6 +53,17 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set) self = reinterpret_cast(data->laser); + // Pre-compute a couple of things + double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_; + double z_rand_mult = 1.0 / data->range_max; + + step = (data->range_count - 1) / (self->max_beams_ - 1); + + // Step size must be at least 1 + if (step < 1) { + step = 1; + } + total_weight = 0.0; // Compute the sample weights @@ -65,17 +76,6 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set) p = 1.0; - // Pre-compute a couple of things - double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_; - double z_rand_mult = 1.0 / data->range_max; - - step = (data->range_count - 1) / (self->max_beams_ - 1); - - // Step size must be at least 1 - if (step < 1) { - step = 1; - } - for (i = 0; i < data->range_count; i += step) { obs_range = data->ranges[i][0]; obs_bearing = data->ranges[i][1]; diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 944ca7bce47..29d0ecbca70 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -204,6 +204,9 @@ list(APPEND plugin_libs nav2_smoother_selector_bt_node) add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp) list(APPEND plugin_libs nav2_goal_checker_selector_bt_node) +add_library(nav2_progress_checker_selector_bt_node SHARED plugins/action/progress_checker_selector_node.cpp) +list(APPEND plugin_libs nav2_progress_checker_selector_bt_node) + add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp) list(APPEND plugin_libs nav2_goal_updated_controller_bt_node) diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index b2c30c432f0..84e16e7d4e9 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -63,4 +63,4 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine. -For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/learn-the-basics/bt_basics/ +For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/3.8/learn-the-basics/BT_basics 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 d7cd7f0b15c..8851ab660e0 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 @@ -163,7 +163,7 @@ class BtActionNode : public BT::ActionNodeBase } /** - * @brief Function to perform some user-defined operation whe the action is aborted. + * @brief Function to perform some user-defined operation when the action is aborted. * @return BT::NodeStatus Returns FAILURE by default, user may override return another value */ virtual BT::NodeStatus on_aborted() @@ -207,7 +207,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_) { @@ -237,7 +237,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; @@ -266,7 +266,7 @@ class BtActionNode : public BT::ActionNodeBase // Action related failure that should not fail the tree, but the node return BT::NodeStatus::FAILURE; } else { - // Internal exception to propogate to the tree + // Internal exception to propagate to the tree throw e; } } @@ -300,6 +300,7 @@ class BtActionNode : public BT::ActionNodeBase void halt() override { if (should_cancel_goal()) { + auto future_result = action_client_->async_get_result(goal_handle_); auto future_cancel = action_client_->async_cancel_goal(goal_handle_); if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) @@ -308,6 +309,16 @@ class BtActionNode : public BT::ActionNodeBase node_->get_logger(), "Failed to cancel action server for %s", action_name_.c_str()); } + + if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR( + node_->get_logger(), + "Failed to get result for %s in node halt!", action_name_.c_str()); + } + + on_cancelled(); } setStatus(BT::NodeStatus::IDLE); 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 e87ffbfac48..42d27c94ec8 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 e050ab30382..3537f9e9a2a 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/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp new file mode 100644 index 00000000000..e996bda55bf --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp @@ -0,0 +1,96 @@ +// Copyright (c) 2024 Open Navigation LLC +// +// 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. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_HPP_ + +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "behaviortree_cpp_v3/action_node.h" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief The ProgressCheckerSelector behavior is used to switch the progress checker + * of the controller server. It subscribes to a topic "progress_checker_selector" + * to get the decision about what progress_checker must be used. It is usually used before of + * the FollowPath. The selected_progress_checker output port is passed to progress_checker_id + * input port of the FollowPath + */ +class ProgressCheckerSelector : public BT::SyncActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::ProgressCheckerSelector + * + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ + ProgressCheckerSelector( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort( + "default_progress_checker", + "the default progress_checker to use if there is not any external topic message received."), + + BT::InputPort( + "topic_name", + "progress_checker_selector", + "the input topic name to select the progress_checker"), + + BT::OutputPort( + "selected_progress_checker", + "Selected progress_checker by subscription") + }; + } + +private: + /** + * @brief Function to perform some user-defined operation on tick + */ + BT::NodeStatus tick() override; + + /** + * @brief callback function for the progress_checker_selector topic + * + * @param msg the message with the id of the progress_checker_selector + */ + void callbackProgressCheckerSelect(const std_msgs::msg::String::SharedPtr msg); + + rclcpp::Subscription::SharedPtr progress_checker_selector_sub_; + + std::string last_selected_progress_checker_; + + rclcpp::Node::SharedPtr node_; + + std::string topic_name_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index a96693112d1..360a3bea608 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -181,6 +181,12 @@ Name of the selected goal checker received from the topic subcription + + Name of the topic to receive progress checker selection commands + Default progress checker of the controller selector + Name of the selected progress checker received from the topic subcription + + Spin distance Allowed time for spinning diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index c841446540e..b0fe9257fd2 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.12 + 1.1.14 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp new file mode 100644 index 00000000000..e8c418ddc17 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp @@ -0,0 +1,81 @@ +// Copyright (c) 2024 Open Navigation LLC +// +// 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 +#include + +#include "std_msgs/msg/string.hpp" + +#include "nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +using std::placeholders::_1; + +ProgressCheckerSelector::ProgressCheckerSelector( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::SyncActionNode(name, conf) +{ + node_ = config().blackboard->get("node"); + + getInput("topic_name", topic_name_); + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + progress_checker_selector_sub_ = node_->create_subscription( + topic_name_, qos, std::bind(&ProgressCheckerSelector::callbackProgressCheckerSelect, this, _1)); +} + +BT::NodeStatus ProgressCheckerSelector::tick() +{ + rclcpp::spin_some(node_); + + // This behavior always use the last selected progress checker received from the topic input. + // When no input is specified it uses the default goaprogressl checker. + // If the default progress checker is not specified then we work in + // "required progress checker mode": In this mode, the behavior returns failure if the progress + // checker selection is not received from the topic input. + if (last_selected_progress_checker_.empty()) { + std::string default_progress_checker; + getInput("default_progress_checker", default_progress_checker); + if (default_progress_checker.empty()) { + return BT::NodeStatus::FAILURE; + } else { + last_selected_progress_checker_ = default_progress_checker; + } + } + + setOutput("selected_progress_checker", last_selected_progress_checker_); + + return BT::NodeStatus::SUCCESS; +} + +void +ProgressCheckerSelector::callbackProgressCheckerSelect(const std_msgs::msg::String::SharedPtr msg) +{ + last_selected_progress_checker_ = msg->data; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("ProgressCheckerSelector"); +} diff --git a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp index 211254dafe9..a898dec9473 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 e5c96eba2c2..1ab530dab1d 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_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index c640e824244..4aaf185306b 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -107,3 +107,7 @@ ament_target_dependencies(test_smoother_selector_node ${dependencies}) ament_add_gtest(test_goal_checker_selector_node test_goal_checker_selector_node.cpp) target_link_libraries(test_goal_checker_selector_node nav2_goal_checker_selector_bt_node) ament_target_dependencies(test_goal_checker_selector_node ${dependencies}) + +ament_add_gtest(test_progress_checker_selector_node test_progress_checker_selector_node.cpp) +target_link_libraries(test_progress_checker_selector_node nav2_progress_checker_selector_bt_node) +ament_target_dependencies(test_progress_checker_selector_node ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index d95cbd4a848..12c25d30d6f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -54,11 +54,17 @@ class FibonacciActionServer : public rclcpp::Node sleep_duration_ = sleep_duration; } + void setServerLoopRate(std::chrono::nanoseconds server_loop_rate) + { + server_loop_rate_ = server_loop_rate; + } + protected: rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID &, std::shared_ptr) { + RCLCPP_INFO(this->get_logger(), "Goal is received.."); if (sleep_duration_ > 0ms) { std::this_thread::sleep_for(sleep_duration_); } @@ -73,6 +79,13 @@ class FibonacciActionServer : public rclcpp::Node void handle_accepted( const std::shared_ptr> handle) + { + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&FibonacciActionServer::execute, this, _1), handle}.detach(); + } + + void execute( + const std::shared_ptr> handle) { // this needs to return quickly to avoid blocking the executor, so spin up a new thread if (handle) { @@ -88,8 +101,17 @@ class FibonacciActionServer : public rclcpp::Node sequence.push_back(0); sequence.push_back(1); + rclcpp::Rate rate(server_loop_rate_); for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) { + if (handle->is_canceling()) { + RCLCPP_INFO(this->get_logger(), "Goal is canceling."); + handle->canceled(result); + return; + } + + RCLCPP_INFO(this->get_logger(), "Goal is feedbacking."); sequence.push_back(sequence[i] + sequence[i - 1]); + rate.sleep(); } handle->succeed(result); @@ -99,6 +121,7 @@ class FibonacciActionServer : public rclcpp::Node protected: rclcpp_action::Server::SharedPtr action_server_; std::chrono::milliseconds sleep_duration_; + std::chrono::nanoseconds server_loop_rate_; }; class FibonacciAction : public nav2_behavior_tree::BtActionNode @@ -121,6 +144,13 @@ class FibonacciAction : public nav2_behavior_tree::BtActionNodeset>("sequence", result_.result->sequence); + config().blackboard->set("on_cancelled_triggered", true); + return BT::NodeStatus::SUCCESS; + } + static BT::PortsList providedPorts() { return providedBasicPorts({BT::InputPort("order", "Fibonacci order")}); @@ -144,6 +174,7 @@ class BTActionNodeTestFixture : public ::testing::Test config_->blackboard->set("server_timeout", 20ms); config_->blackboard->set("bt_loop_duration", 10ms); config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("on_cancelled_triggered", false); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) @@ -220,6 +251,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // setting a small action server goal handling duration action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(10ns); // to keep track of the number of ticks it took to reach a terminal result int ticks = 0; @@ -255,15 +287,22 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // start a new execution cycle with the previous BT to ensure previous state doesn't leak into // the new cycle - // halt BT for a new execution cycle + // halt BT for a new execution cycle, + // get if the on_cancelled is triggered from blackboard and assert + // that the on_cancelled triggers after halting node + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); tree_->haltTree(); + bool on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, false); // setting a large action server goal handling duration action_server_->setHandleGoalSleepDuration(100ms); + action_server_->setServerLoopRate(10ns); // reset state variables ticks = 0; result = BT::NodeStatus::RUNNING; + config_->blackboard->set("on_cancelled_triggered", false); // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { @@ -300,6 +339,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // the action server will take 100ms before accepting the goal action_server_->setHandleGoalSleepDuration(100ms); + action_server_->setServerLoopRate(10ns); // to keep track of the number of ticks it took to reach a terminal result int ticks = 0; @@ -327,14 +367,21 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // the new cycle // halt BT for a new execution cycle + // get if the on_cancel is triggered from blackboard and assert + // that the on_cancelled never can trigger after halting node + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); tree_->haltTree(); + bool on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, false); // setting a small action server goal handling duration action_server_->setHandleGoalSleepDuration(25ms); + action_server_->setServerLoopRate(10ns); // reset state variables ticks = 0; result = BT::NodeStatus::RUNNING; + config_->blackboard->set("on_cancelled_triggered", false); // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { @@ -348,6 +395,90 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) EXPECT_EQ(result, BT::NodeStatus::SUCCESS); } +TEST_F(BTActionNodeTestFixture, test_server_cancel) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + // setting a server timeout smaller than the time the action server will take to accept the goal + // to simulate a server timeout scenario + config_->blackboard->set("server_timeout", 100ms); + config_->blackboard->set("bt_loop_duration", 10ms); + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // the action server will take 2ms before accepting the goal + // and the feedback period of the action server will be 50ms + action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(50ms); + + // to keep track of the number of ticks it took to reach expected tick count + int ticks = 0; + + BT::NodeStatus result = BT::NodeStatus::RUNNING; + + // BT loop execution rate + rclcpp::WallRate loopRate(100ms); + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 5) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // halt BT for testing if the action node cancels the goal correctly + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); + tree_->haltTree(); + + // get if the on_cancel is triggered from blackboard and assert + // that the on_cancel is triggered after halting node + bool on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, true); + + // ticks variable must be 5 because execution time of the action server + // is at least 1000000 x 50 ms + EXPECT_EQ(ticks, 5); + + // send new goal to the action server for a new execution cycle + + // the action server will take 2ms before accepting the goal + // and the feedback period of the action server will be 1000ms + action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(50ms); + + // reset state variable + ticks = 0; + config_->blackboard->set("on_cancelled_triggered", false); + result = BT::NodeStatus::RUNNING; + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 7) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // halt BT for testing if the action node cancels the goal correctly + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); + tree_->haltTree(); + + // get if the on_cancel is triggered from blackboard and assert + // that the on_cancel is triggered after halting node + on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, true); + + // ticks variable must be 7 because execution time of the action server + // is at least 1000000 x 50 ms + EXPECT_EQ(ticks, 7); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp new file mode 100644 index 00000000000..08d589ac031 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp @@ -0,0 +1,187 @@ +// Copyright (c) 2024 Open Navigation LLC +// +// 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 + +#include +#include +#include + +#include "../../test_action_server.hpp" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/string.hpp" + +class ProgressCheckerSelectorTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("progress_checker_selector_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set("node", node_); + + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, config); + }; + + factory_->registerBuilder( + "ProgressCheckerSelector", + builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr ProgressCheckerSelectorTestFixture::node_ = nullptr; + +BT::NodeConfiguration * ProgressCheckerSelectorTestFixture::config_ = nullptr; +std::shared_ptr ProgressCheckerSelectorTestFixture::factory_ = nullptr; +std::shared_ptr ProgressCheckerSelectorTestFixture::tree_ = nullptr; + +TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_progress_checker_result; + config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + + EXPECT_EQ(selected_progress_checker_result, "SimpleProgressCheck"); + + std_msgs::msg::String selected_progress_checker_cmd; + + selected_progress_checker_cmd.data = "AngularProgressChecker"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto progress_checker_selector_pub = + node_->create_publisher( + "progress_checker_selector_custom_topic_name", qos); + + // publish a few updates of the selected_progress_checker + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + progress_checker_selector_pub->publish(selected_progress_checker_cmd); + + rclcpp::spin_some(node_); + } + + // check progress_checker updated + config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + EXPECT_EQ("AngularProgressChecker", selected_progress_checker_result); +} + +TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_progress_checker_result; + config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + + EXPECT_EQ(selected_progress_checker_result, "GridBased"); + + std_msgs::msg::String selected_progress_checker_cmd; + + selected_progress_checker_cmd.data = "RRT"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto progress_checker_selector_pub = + node_->create_publisher("progress_checker_selector", qos); + + // publish a few updates of the selected_progress_checker + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + progress_checker_selector_pub->publish(selected_progress_checker_cmd); + + rclcpp::spin_some(node_); + } + + // check goal_checker updated + config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + EXPECT_EQ("RRT", selected_progress_checker_result); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 898573b9a3b..0d6ca68ffc7 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.12 + 1.1.14 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 559608e61f6..0340705620e 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.12 + 1.1.14 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index 2605fc86497..615ce5ef64b 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -32,7 +32,7 @@ namespace nav2_bt_navigator { /** - * @class NavigateToPoseNavigator + * @class NavigateThroughPosesNavigator * @brief A navigator for navigating to a a bunch of intermediary poses */ class NavigateThroughPosesNavigator 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 98287b5be83..32dc1cd1447 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_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 2540ec06a78..f8209fcfc29 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.12 + 1.1.14 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 0672683c27e..9ffc92422e4 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.12 + 1.1.14 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index a6e3f86806b..8e9914a400f 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -389,7 +389,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) printAction(robot_action, action_polygon); } - // Publish requred robot velocity + // Publish required robot velocity publishVelocity(robot_action); // Publish polygons for better visualization diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 42b183c6e98..631a50e658e 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.12 + 1.1.14 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index b1879c57434..1e456a608c4 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.12 + 1.1.14 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index 463af33fd08..1ccae77f774 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/package.xml b/nav2_controller/package.xml index 8c1208f9ccc..efd36e841f6 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.12 + 1.1.14 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index a0bd0cec146..6c96915c9bf 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -534,7 +534,7 @@ void ControllerServer::computeAndPublishVelocity() // other types will not be resolved with more attempts } catch (nav2_core::NoValidControl & 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_core/package.xml b/nav2_core/package.xml index 8f09d1b42eb..d1d580e4396 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.12 + 1.1.14 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index f039cf5d42b..3be30b28539 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -26,17 +26,10 @@ find_package(visualization_msgs REQUIRED) find_package(angles REQUIRED) remove_definitions(-DDISABLE_LIBUSB-1.0) -find_package(Eigen3 REQUIRED) +find_package(Eigen3 3.3 REQUIRED) nav2_package() -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} -) - -add_definitions(${EIGEN3_DEFINITIONS}) - add_library(nav2_costmap_2d_core SHARED src/array_parser.cpp src/costmap_2d.cpp @@ -52,6 +45,13 @@ add_library(nav2_costmap_2d_core SHARED src/footprint_collision_checker.cpp plugins/costmap_filters/costmap_filter.cpp ) +add_library(${PROJECT_NAME}::nav2_costmap_2d_core ALIAS nav2_costmap_2d_core) + +target_include_directories(nav2_costmap_2d_core + PUBLIC + "$" + "$" +) set(dependencies geometry_msgs @@ -79,6 +79,7 @@ set(dependencies ament_target_dependencies(nav2_costmap_2d_core ${dependencies} ) +target_link_libraries(nav2_costmap_2d_core Eigen3::Eigen) add_library(layers SHARED plugins/inflation_layer.cpp @@ -89,11 +90,12 @@ add_library(layers SHARED plugins/range_sensor_layer.cpp plugins/denoise_layer.cpp ) +add_library(${PROJECT_NAME}::layers ALIAS layers) ament_target_dependencies(layers ${dependencies} ) target_link_libraries(layers - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_library(filters SHARED @@ -101,11 +103,14 @@ add_library(filters SHARED plugins/costmap_filters/speed_filter.cpp plugins/costmap_filters/binary_filter.cpp ) +add_library(${PROJECT_NAME}::filters ALIAS filters) + + ament_target_dependencies(filters ${dependencies} ) target_link_libraries(filters - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_library(nav2_costmap_2d_client SHARED @@ -113,18 +118,19 @@ add_library(nav2_costmap_2d_client SHARED src/costmap_subscriber.cpp src/costmap_topic_collision_checker.cpp ) +add_library(${PROJECT_NAME}::nav2_costmap_2d_client ALIAS nav2_costmap_2d_client) ament_target_dependencies(nav2_costmap_2d_client ${dependencies} ) target_link_libraries(nav2_costmap_2d_client - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_executable(nav2_costmap_2d_markers src/costmap_2d_markers.cpp) target_link_libraries(nav2_costmap_2d_markers - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_target_dependencies(nav2_costmap_2d_markers @@ -133,7 +139,7 @@ ament_target_dependencies(nav2_costmap_2d_markers add_executable(nav2_costmap_2d_cloud src/costmap_2d_cloud.cpp) target_link_libraries(nav2_costmap_2d_cloud - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_executable(nav2_costmap_2d src/costmap_2d_node.cpp) @@ -142,26 +148,24 @@ ament_target_dependencies(nav2_costmap_2d ) target_link_libraries(nav2_costmap_2d - nav2_costmap_2d_core - layers - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers + ${PROJECT_NAME}::filters ) install(TARGETS - nav2_costmap_2d_core layers filters + nav2_costmap_2d_core nav2_costmap_2d_client + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -install(TARGETS - nav2_costmap_2d - nav2_costmap_2d_markers - nav2_costmap_2d_cloud - RUNTIME DESTINATION lib/${PROJECT_NAME} +install(TARGETS nav2_costmap_2d_markers nav2_costmap_2d_cloud nav2_costmap_2d + DESTINATION lib/${PROJECT_NAME} ) install(FILES costmap_plugins.xml @@ -169,7 +173,7 @@ install(FILES costmap_plugins.xml ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) @@ -184,8 +188,7 @@ if(BUILD_TESTING) pluginlib_export_plugin_description_file(nav2_costmap_2d test/regression/order_layer.xml) endif() -ament_export_include_directories(include) -ament_export_libraries(layers filters nav2_costmap_2d_core nav2_costmap_2d_client) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies(${dependencies}) pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) ament_package() 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 ee798948475..75c720cbaa3 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