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/package.xml b/nav2_amcl/package.xml index cac7910c5ba..1f5199979a0 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.1.13 + 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/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 6522072368e..72b5c1b6f23 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -192,6 +192,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/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 ce5979929cb..1a2e3d7be66 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -174,6 +174,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 f8a2170ddb9..b0fe9257fd2 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.13 + 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/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_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 5efaa152e24..0d6ca68ffc7 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.13 + 1.1.14 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 1e41f7a1ffa..0340705620e 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.13 + 1.1.14 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 02a5b4cad28..572ebe3c641 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.13 + 1.1.14 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index db17606e374..9ffc92422e4 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.13 + 1.1.14 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 456c8167c87..631a50e658e 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.13 + 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 cde44f16960..1e456a608c4 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.13 + 1.1.14 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 307180c8bfa..efd36e841f6 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.13 + 1.1.14 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index c4f2d2e77e3..5898d781897 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.13 + 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/package.xml b/nav2_costmap_2d/package.xml index 41c31f39ac9..39e29574ec6 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.1.13 + 1.1.14 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index ef0b148440c..5d33caa63f6 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -1,67 +1,49 @@ ament_add_gtest_executable(footprint_tests_exec footprint_tests.cpp ) -ament_target_dependencies(footprint_tests_exec - ${dependencies} -) target_link_libraries(footprint_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(test_collision_checker_exec test_costmap_topic_collision_checker.cpp ) -ament_target_dependencies(test_collision_checker_exec - ${dependencies} -) target_link_libraries(test_collision_checker_exec - nav2_costmap_2d_core - nav2_costmap_2d_client - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_client + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(inflation_tests_exec inflation_tests.cpp ) -ament_target_dependencies(inflation_tests_exec - ${dependencies} -) target_link_libraries(inflation_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(obstacle_tests_exec obstacle_tests.cpp ) -ament_target_dependencies(obstacle_tests_exec - ${dependencies} -) target_link_libraries(obstacle_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(range_tests_exec range_tests.cpp ) -ament_target_dependencies(range_tests_exec - ${dependencies} -) target_link_libraries(range_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest(dyn_params_tests dyn_params_tests.cpp ) -ament_target_dependencies(dyn_params_tests - ${dependencies} -) target_link_libraries(dyn_params_tests - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_test(test_collision_checker @@ -123,6 +105,6 @@ ament_add_test(range_tests # ${dependencies} # ) # target_link_libraries(costmap_tester -# nav2_costmap_2d_core +# ${PROJECT_NAME}::nav2_costmap_2d_core # layers # ) diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt index 78fbfa6cf79..aafc4d561be 100644 --- a/nav2_costmap_2d/test/regression/CMakeLists.txt +++ b/nav2_costmap_2d/test/regression/CMakeLists.txt @@ -1,7 +1,7 @@ # Bresenham2D corner cases test ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp) target_link_libraries(costmap_bresenham_2d - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) # OrderLayer for checking Costmap2D plugins API calling order @@ -11,7 +11,7 @@ ament_target_dependencies(order_layer ${dependencies} ) target_link_libraries(order_layer - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) install(TARGETS order_layer @@ -23,5 +23,5 @@ install(TARGETS # Costmap2D plugins API calling order test ament_add_gtest(plugin_api_order plugin_api_order.cpp) target_link_libraries(plugin_api_order - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 19c2edb56fe..3f2f100d7a2 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -5,53 +5,54 @@ target_link_libraries(array_parser_test ament_add_gtest(collision_footprint_test footprint_collision_checker_test.cpp) target_link_libraries(collision_footprint_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(costmap_convesion_test costmap_conversion_test.cpp) target_link_libraries(costmap_convesion_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(declare_parameter_test declare_parameter_test.cpp) target_link_libraries(declare_parameter_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(costmap_filter_test costmap_filter_test.cpp) target_link_libraries(costmap_filter_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(keepout_filter_test keepout_filter_test.cpp) target_link_libraries(keepout_filter_test - nav2_costmap_2d_core - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::filters ) ament_add_gtest(speed_filter_test speed_filter_test.cpp) target_link_libraries(speed_filter_test - nav2_costmap_2d_core - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::filters ) ament_add_gtest(binary_filter_test binary_filter_test.cpp) target_link_libraries(binary_filter_test - nav2_costmap_2d_core - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::filters ) ament_add_gtest(copy_window_test copy_window_test.cpp) target_link_libraries(copy_window_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(costmap_filter_service_test costmap_filter_service_test.cpp) target_link_libraries(costmap_filter_service_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(denoise_layer_test denoise_layer_test.cpp image_test.cpp image_processing_test.cpp) target_link_libraries(denoise_layer_test - nav2_costmap_2d_core layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) diff --git a/nav2_dwb_controller/costmap_queue/CMakeLists.txt b/nav2_dwb_controller/costmap_queue/CMakeLists.txt index 4641d0b5471..3cb0d967f88 100644 --- a/nav2_dwb_controller/costmap_queue/CMakeLists.txt +++ b/nav2_dwb_controller/costmap_queue/CMakeLists.txt @@ -12,7 +12,7 @@ include_directories( include ) -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/costmap_queue.cpp src/limited_costmap_queue.cpp ) diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index e784e648231..eb9ecc1aeb2 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.1.13 + 1.1.14 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 56607270d26..e4d7b922567 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.1.13 + 1.1.14 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 1dbf1dbd4d1..3193de89816 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.1.13 + 1.1.14 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 7419bfd2394..9dbea374a36 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.1.13 + 1.1.14 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index a21aef7e064..92efb2a7cca 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.1.13 + 1.1.14 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 5675e9455fb..fd08166dd95 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.1.13 + 1.1.14 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 195ee76e463..486b5879494 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.1.13 + 1.1.14 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index 31adaf72788..d1b16c2212c 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.1.13 + 1.1.14 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 2fc9ef060c3..e04a65d0a7c 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.1.13 + 1.1.14 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 0852f14e228..13c58a57266 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.1.13 + 1.1.14 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 1501e4321a1..d2bb6d9c9da 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -79,6 +79,7 @@ add_library(mppi_controller SHARED add_library(mppi_critics SHARED src/critics/obstacles_critic.cpp + src/critics/cost_critic.cpp src/critics/goal_critic.cpp src/critics/goal_angle_critic.cpp src/critics/path_align_critic.cpp diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 164f20fe4cc..58de7b79830 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -99,6 +99,9 @@ This process is then repeated a number of times and returns a converged solution #### Obstacles Critic + +Uses estimated distances from obstacles using cost and inflation parameters to avoid obstacles + | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. | @@ -111,6 +114,20 @@ This process is then repeated a number of times and returns a converged solution | cost_scaling_factor | double | Default 10.0. Exponential decay factor across inflation radius. This should be the same as for your inflation layer (Humble only) | inflation_radius | double | Default 0.55. Radius to inflate costmap around lethal obstacles. This should be the same as for your inflation layer (Humble only) +#### Cost Critic + +Uses inflated costmap cost directly to avoid obstacles + + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. | + | cost_weight | double | Default 3.81. Wight to apply to critic to avoid obstacles. | + | cost_power | int | Default 1. Power order to apply to term. | + | collision_cost | double | Default 1000000.0. Cost to apply to a true collision in a trajectory. | + | critical_cost | double | Default 300.0. Cost to apply to a pose with any point in in inflated space to prefer distance from obstacles. | + | 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 | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | @@ -186,7 +203,7 @@ controller_server: time_step: 3 AckermannConstraints: min_turning_r: 0.2 - critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] ConstraintCritic: enabled: true cost_power: 1 @@ -206,15 +223,24 @@ controller_server: cost_power: 1 cost_weight: 5.0 threshold_to_consider: 0.5 - ObstaclesCritic: + # ObstaclesCritic: + # enabled: true + # cost_power: 1 + # repulsion_weight: 1.5 + # critical_weight: 20.0 + # consider_footprint: false + # collision_cost: 10000.0 + # collision_margin_distance: 0.1 + # near_goal_distance: 0.5 + CostCritic: enabled: true cost_power: 1 - repulsion_weight: 1.5 - critical_weight: 20.0 - consider_footprint: false - collision_cost: 10000.0 - collision_margin_distance: 0.1 - near_goal_distance: 0.5 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + PathAlignCritic: PathAlignCritic: enabled: true cost_power: 1 diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index 790568e7967..d578d23a9ec 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -5,6 +5,10 @@ mppi critic for obstacle avoidance + + mppi critic for obstacle avoidance using costmap score + + mppi critic for driving towards the goal diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp new file mode 100644 index 00000000000..77df20adcb5 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -0,0 +1,98 @@ +// Copyright (c) 2023 Robocc Brice Renaudeau +// +// 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_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ + +#include +#include + +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" + +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::CostCritic + * @brief Critic objective function for avoiding obstacles using costmap's inflated cost + */ +class CostCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to obstacle avoidance + * + * @param costs [out] add obstacle cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + /** + * @brief Checks if cost represents a collision + * @param cost Point cost at pose center + * @param x X of pose + * @param y Y of pose + * @param theta theta of pose + * @return bool if in collision + */ + bool inCollision(float cost, float x, float y, float theta); + + /** + * @brief cost at a robot pose + * @param x X of pose + * @param y Y of pose + * @return Collision information at pose + */ + float costAtPose(float x, float y); + + /** + * @brief Find the min cost of the inflation decay function for which the robot MAY be + * in collision in any orientation + * @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation) + * @return double circumscribed cost, any higher than this and need to do full footprint collision checking + * since some element of the robot could be in collision + */ + float findCircumscribedCost(std::shared_ptr costmap); + +protected: + nav2_costmap_2d::FootprintCollisionChecker + collision_checker_{nullptr}; + float possibly_inscribed_cost_; + + bool consider_footprint_{true}; + float circumscribed_radius_{0}; + float circumscribed_cost_{0}; + float collision_cost_{0}; + float critical_cost_{0}; + float weight_{0}; + + float near_goal_distance_; + std::string inflation_layer_name_; + + unsigned int power_{0}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index fceccc0ac14..f5a3211036c 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.1.13 + 1.1.14 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp new file mode 100644 index 00000000000..b91ab075b6a --- /dev/null +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -0,0 +1,212 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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 "nav2_mppi_controller/critics/cost_critic.hpp" + +namespace mppi::critics +{ + +void CostCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(consider_footprint_, "consider_footprint", false); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 3.81); + getParam(critical_cost_, "critical_cost", 300.0); + getParam(collision_cost_, "collision_cost", 1000000.0); + getParam(near_goal_distance_, "near_goal_distance", 0.5); + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); + + // Normalized by cost value to put in same regime as other weights + weight_ /= 254.0f; + + collision_checker_.setCostmap(costmap_); + possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + + if (possibly_inscribed_cost_ < 1.0f) { + RCLCPP_ERROR( + logger_, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + + RCLCPP_INFO( + logger_, + "InflationCostCritic instantiated with %d power and %f / %f weights. " + "Critic will collision check based on %s cost.", + power_, critical_cost_, weight_, consider_footprint_ ? + "footprint" : "circular"); +} + +float CostCritic::findCircumscribedCost( + std::shared_ptr costmap) +{ + bool inflation_layer_found = false; + double result = -1.0; + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + if (static_cast(circum_radius) == circumscribed_radius_) { + // early return if footprint size is unchanged + return circumscribed_cost_; + } + + // check if the costmap has an inflation layer + for (auto layer = costmap->getLayeredCostmap()->getPlugins()->begin(); + layer != costmap->getLayeredCostmap()->getPlugins()->end(); + ++layer) + { + auto inflation_layer = std::dynamic_pointer_cast(*layer); + if (!inflation_layer || + (!inflation_layer_name_.empty() && + inflation_layer->getName() != inflation_layer_name_)) + { + continue; + } + + inflation_layer_found = true; + const double resolution = costmap->getCostmap()->getResolution(); + result = inflation_layer->computeCost(circum_radius / resolution); + } + + if (!inflation_layer_found) { + RCLCPP_WARN( + logger_, + "No inflation layer found in costmap configuration. " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times and not avoid anything but absolute collisions!"); + } + + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(result); + + return circumscribed_cost_; +} + +void CostCritic::score(CriticData & data) +{ + using xt::evaluation_strategy::immediate; + if (!enabled_) { + return; + } + + if (consider_footprint_) { + // footprint may have changed since initialization if user has dynamic footprints + possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + } + + // If near the goal, don't apply the preferential term since the goal is near obstacles + bool near_goal = false; + if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) { + near_goal = true; + } + + auto && repulsive_cost = xt::xtensor::from_shape({data.costs.shape(0)}); + repulsive_cost.fill(0.0); + + const size_t traj_len = data.trajectories.x.shape(1); + bool all_trajectories_collide = true; + for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) { + bool trajectory_collide = false; + const auto & traj = data.trajectories; + float pose_cost; + + for (size_t j = 0; j < traj_len; j++) { + // The costAtPose doesn't use orientation + // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it + // So the center point has more information than the footprint + pose_cost = costAtPose(traj.x(i, j), traj.y(i, j)); + if (pose_cost < 1.0f) {continue;} // In free space + + if (inCollision(pose_cost, traj.x(i, j), traj.y(i, j), traj.yaws(i, j))) { + trajectory_collide = true; + break; + } + + // Let near-collision trajectory points be punished severely + // Note that we collision check based on the footprint actual, + // but score based on the center-point cost regardless + using namespace nav2_costmap_2d; // NOLINT + if (pose_cost >= INSCRIBED_INFLATED_OBSTACLE) { + repulsive_cost[i] += critical_cost_; + } else if (!near_goal) { // Generally prefer trajectories further from obstacles + repulsive_cost[i] += pose_cost; + } + } + + if (!trajectory_collide) { + all_trajectories_collide = false; + } else { + repulsive_cost[i] = collision_cost_; + } + } + + data.costs += xt::pow((weight_ * repulsive_cost / traj_len), power_); + data.fail_flag = all_trajectories_collide; +} + +/** + * @brief Checks if cost represents a collision + * @param cost Costmap cost + * @return bool if in collision + */ +bool CostCritic::inCollision(float cost, float x, float y, float theta) +{ + bool is_tracking_unknown = + costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + + // If consider_footprint_ check footprint scort for collision + if (consider_footprint_ && + (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f)) + { + cost = static_cast(collision_checker_.footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint())); + } + + switch (static_cast(cost)) { + using namespace nav2_costmap_2d; // NOLINT + case (LETHAL_OBSTACLE): + return true; + case (INSCRIBED_INFLATED_OBSTACLE): + return consider_footprint_ ? false : true; + case (NO_INFORMATION): + return is_tracking_unknown ? false : true; + } + + return false; +} + +float CostCritic::costAtPose(float x, float y) +{ + using namespace nav2_costmap_2d; // NOLINT + unsigned int x_i, y_i; + if (!collision_checker_.worldToMap(x, y, x_i, y_i)) { + return nav2_costmap_2d::NO_INFORMATION; + } + + return collision_checker_.pointCost(x_i, y_i); +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::CostCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index fd05ab4bee6..8b26aad774f 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -23,6 +23,7 @@ #include "nav2_mppi_controller/critics/goal_angle_critic.hpp" #include "nav2_mppi_controller/critics/goal_critic.hpp" #include "nav2_mppi_controller/critics/obstacles_critic.hpp" +#include "nav2_mppi_controller/critics/cost_critic.hpp" #include "nav2_mppi_controller/critics/path_align_critic.hpp" #include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp" #include "nav2_mppi_controller/critics/path_angle_critic.hpp" diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index d694496395d..9a77f6d8b06 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -103,7 +103,7 @@ INSTANTIATE_TEST_SUITE_P( std::make_tuple( "DiffDrive", std::vector( - {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {{"GoalCritic"}, {"GoalAngleCritic"}, {"CostCritic"}, {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}), true), std::make_tuple( diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 2be81db0a74..b2cbccec481 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -30,6 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/ManageLifecycleNodes.srv" "srv/LoadMap.srv" "srv/SaveMap.srv" + "srv/SetInitialPose.srv" "action/AssistedTeleop.action" "action/BackUp.action" "action/ComputePathToPose.action" diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 593337b0858..ad356d640d1 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.1.13 + 1.1.14 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_msgs/srv/SetInitialPose.srv b/nav2_msgs/srv/SetInitialPose.srv new file mode 100644 index 00000000000..ec1e50b65f1 --- /dev/null +++ b/nav2_msgs/srv/SetInitialPose.srv @@ -0,0 +1,3 @@ +geometry_msgs/PoseWithCovarianceStamped pose +--- + diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 8139ee98381..9f235683d01 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.1.13 + 1.1.14 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 36b13c0e5fa..0934f6e6e7a 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.1.13 + 1.1.14 TODO Steve Macenski Apache-2.0 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 01b2384845f..0e393f8304c 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -408,7 +408,10 @@ PlannerServer::computePlanThroughPoses() if (i == 0) { curr_start = start; } else { - curr_start = goal->goals[i - 1]; + // pick the end of the last planning task as the start for the next one + // to allow for path tolerance deviations + curr_start = concat_path.poses.back(); + curr_start.header = concat_path.header; } curr_goal = goal->goals[i]; diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index fde5ca99392..df20eda09be 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.1.13 + 1.1.14 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 19834eb5f88..6f3c8214312 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.1.13 + 1.1.14 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 192a0a4d65e..25b640ca9a3 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.1.13 + 1.1.14 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index 4f4b03332b9..9dae3590e19 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.1.13 + 1.1.14 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index c96df8da178..f9261cca91a 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -131,7 +131,7 @@ planner_server: max_iterations: 1000 w_smooth: 0.3 w_data: 0.2 - tolerance: 1e-10 + tolerance: 1.0e-10 do_refinement: true # Whether to recursively run the smoother 3 times on the results from prior runs to refine the results further ``` diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index c03b7de7fc3..617a3d570b5 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.1.13 + 1.1.14 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 20d288809da..549570fb262 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -62,6 +62,7 @@ void GridCollisionChecker::setFootprint( return; } + oriented_footprints_.clear(); oriented_footprints_.reserve(angles_.size()); double sin_th, cos_th; geometry_msgs::msg::Point new_pt; diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 08ae3b5e8d7..482e0d27a73 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -283,6 +283,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( } // Set collision checker and costmap information + _collision_checker.setFootprint( + _costmap_ros->getRobotFootprint(), + _costmap_ros->getUseRadius(), + findCircumscribedCost(_costmap_ros)); _a_star->setCollisionChecker(&_collision_checker); // Set starting point, in A* bin search coordinates diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 248ef1f218a..31090e10dd0 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -243,6 +243,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( std::unique_lock lock(*(_costmap->getMutex())); // Set collision checker and costmap information + _collision_checker.setFootprint( + _costmap_ros->getRobotFootprint(), + _costmap_ros->getUseRadius(), + findCircumscribedCost(_costmap_ros)); _a_star->setCollisionChecker(&_collision_checker); // Set starting point, in A* bin search coordinates diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 3d25dd76988..38a352d0bea 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.1.13 + 1.1.14 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 8a14df53ebc..6dd18d384a9 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.1.13 + 1.1.14 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 7366ee3fe44..7fa8d96b54a 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.1.13 + 1.1.14 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 86b4fab06c7..19e690c777b 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.1.13 + 1.1.14 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 3cf71d4e712..9fa7e99540e 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.1.13 + 1.1.14 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index d9ad807fa4f..7a2d8dd89f1 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.1.13 + 1.1.14 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 507251b514b..2669d720b88 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.1.13 + 1.1.14 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 8e47a24f0ea..d546039a2c8 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.1.13 + 1.1.14 ROS2 Navigation Stack