diff --git a/.circleci/config.yml b/.circleci/config.yml index 8f60f876b41..ede7b367e94 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v11\ + - "<< parameters.key >>-v12\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v11\ + - "<< parameters.key >>-v12\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v11\ + key: "<< parameters.key >>-v12\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ @@ -447,6 +447,7 @@ _environments: RCUTILS_LOGGING_BUFFERED_STREAM: "0" RCUTILS_LOGGING_USE_STDOUT: "0" DEBIAN_FRONTEND: "noninteractive" + PYTHONUNBUFFERED: "1" executors: release_exec: diff --git a/Dockerfile b/Dockerfile index 155f5aefbbf..ba26fc246f1 100644 --- a/Dockerfile +++ b/Dockerfile @@ -43,6 +43,7 @@ RUN echo '\ APT::Install-Recommends "0";\n\ APT::Install-Suggests "0";\n\ ' > /etc/apt/apt.conf.d/01norecommend +ENV PYTHONUNBUFFERED 1 # install CI dependencies ARG RTI_NC_LICENSE_ACCEPTED=yes diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index dea6e7a07fd..a2182a941da 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.1.2 + 1.1.3

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 a28b4927a3e..2e9730ae56b 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1158,18 +1158,53 @@ AmclNode::dynamicParametersCallback( if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == "alpha1") { alpha1_ = parameter.as_double(); + //alpha restricted to be non-negative + if (alpha1_ < 0.0) { + RCLCPP_WARN( + get_logger(), "You've set alpha1 to be negative," + " this isn't allowed, so the alpha1 will be set to be zero."); + alpha1_ = 0.0; + } reinit_odom = true; } else if (param_name == "alpha2") { alpha2_ = parameter.as_double(); + //alpha restricted to be non-negative + if (alpha2_ < 0.0) { + RCLCPP_WARN( + get_logger(), "You've set alpha2 to be negative," + " this isn't allowed, so the alpha2 will be set to be zero."); + alpha2_ = 0.0; + } reinit_odom = true; } else if (param_name == "alpha3") { alpha3_ = parameter.as_double(); + //alpha restricted to be non-negative + if (alpha3_ < 0.0) { + RCLCPP_WARN( + get_logger(), "You've set alpha3 to be negative," + " this isn't allowed, so the alpha3 will be set to be zero."); + alpha3_ = 0.0; + } reinit_odom = true; } else if (param_name == "alpha4") { alpha4_ = parameter.as_double(); + //alpha restricted to be non-negative + if (alpha4_ < 0.0) { + RCLCPP_WARN( + get_logger(), "You've set alpha4 to be negative," + " this isn't allowed, so the alpha4 will be set to be zero."); + alpha4_ = 0.0; + } reinit_odom = true; } else if (param_name == "alpha5") { alpha5_ = parameter.as_double(); + //alpha restricted to be non-negative + if (alpha5_ < 0.0) { + RCLCPP_WARN( + get_logger(), "You've set alpha5 to be negative," + " this isn't allowed, so the alpha5 will be set to be zero."); + alpha5_ = 0.0; + } reinit_odom = true; } else if (param_name == "beam_skip_distance") { beam_skip_distance_ = parameter.as_double(); diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index b1588dff6e4..71ba3326180 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -69,6 +69,9 @@ list(APPEND plugin_libs nav2_spin_cancel_bt_node) add_library(nav2_back_up_cancel_bt_node SHARED plugins/action/back_up_cancel_node.cpp) list(APPEND plugin_libs nav2_back_up_cancel_bt_node) +add_library(nav2_assisted_teleop_cancel_bt_node SHARED plugins/action/assisted_teleop_cancel_node.cpp) +list(APPEND plugin_libs nav2_assisted_teleop_cancel_bt_node) + add_library(nav2_drive_on_heading_cancel_bt_node SHARED plugins/action/drive_on_heading_cancel_node.cpp) list(APPEND plugin_libs nav2_drive_on_heading_cancel_bt_node) @@ -90,6 +93,9 @@ list(APPEND plugin_libs nav2_spin_action_bt_node) add_library(nav2_wait_action_bt_node SHARED plugins/action/wait_action.cpp) list(APPEND plugin_libs nav2_wait_action_bt_node) +add_library(nav2_assisted_teleop_action_bt_node SHARED plugins/action/assisted_teleop_action.cpp) +list(APPEND plugin_libs nav2_assisted_teleop_action_bt_node) + add_library(nav2_clear_costmap_service_bt_node SHARED plugins/action/clear_costmap_service.cpp) list(APPEND plugin_libs nav2_clear_costmap_service_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp new file mode 100644 index 00000000000..22cde94c35f --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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__ASSISTED_TELEOP_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_ACTION_HPP_ + +#include + +#include "nav2_behavior_tree/bt_action_node.hpp" +#include "nav2_msgs/action/assisted_teleop.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::AssistedTeleop + */ +class AssistedTeleopAction : public BtActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::nav2_msgs::action::AssistedTeleop + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ + AssistedTeleopAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Function to perform some user-defined operation on tick + */ + void on_tick() override; + + BT::NodeStatus on_aborted() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + BT::InputPort("time_allowance", 10.0, "Allowed time for running assisted teleop"), + BT::InputPort("is_recovery", false, "If true the recovery count will be incremented") + }); + } + +private: + bool is_recovery_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_ACTION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp new file mode 100644 index 00000000000..55fe337fe8e --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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__ASSISTED_TELEOP_CANCEL_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_CANCEL_NODE_HPP_ + +#include +#include + +#include "nav2_msgs/action/assisted_teleop.hpp" + +#include "nav2_behavior_tree/bt_cancel_action_node.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp + */ +class AssistedTeleopCancel : public BtCancelActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::BackUpAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ + AssistedTeleopCancel( + const std::string & xml_tag_name, + const std::string & action_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 providedBasicPorts( + { + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ASSISTED_TELEOP_CANCEL_NODE_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp index f87435995c5..fc95daaf439 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp @@ -61,6 +61,11 @@ class ComputePathToPoseAction : public BtActionNodeServer timeout - + Service name to cancel the drive on heading behavior Server timeout @@ -43,6 +43,11 @@ Server timeout + + Service name to cancel the assisted teleop behavior + Server timeout + + Service name to cancel the wait behavior Server timeout @@ -53,6 +58,18 @@ Server timeout + + Service name + Server timeout + Distance from the robot above which obstacles are cleared + + + + Service name + Server timeout + Distance from the robot under which obstacles are cleared + + Destination to plan to Start pose of the path if overriding current robot pose @@ -166,6 +183,13 @@ Server timeout + + Allowed time for spinning + If true recovery count will be incremented + Service name + Server timeout + + Destination diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index ce98becbc12..9db3bba4a03 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.2 + 1.1.3 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp new file mode 100644 index 00000000000..025f4786fc5 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -0,0 +1,62 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp" + +namespace nav2_behavior_tree +{ + +AssistedTeleopAction::AssistedTeleopAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) +{ + double time_allowance; + getInput("time_allowance", time_allowance); + getInput("is_recovery", is_recovery_); + + // Populate the input message + goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); +} + +void AssistedTeleopAction::on_tick() +{ + if (is_recovery_) { + increment_recovery_count(); + } +} + +BT::NodeStatus AssistedTeleopAction::on_aborted() +{ + return is_recovery_ ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "assisted_teleop", config); + }; + + factory.registerBuilder("AssistedTeleop", builder); +} diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp new file mode 100644 index 00000000000..e226ba19753 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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/assisted_teleop_cancel_node.hpp" + +namespace nav2_behavior_tree +{ + +AssistedTeleopCancel::AssistedTeleopCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtCancelActionNode(xml_tag_name, action_name, conf) +{ +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "assisted_teleop", config); + }; + + factory.registerBuilder( + "CancelAssistedTeleop", builder); +} diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp index e6d315e43ad..07bd9240d29 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp @@ -57,6 +57,13 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void ComputePathToPoseAction::halt() +{ + nav_msgs::msg::Path empty_path; + setOutput("path", empty_path); + BtActionNode::halt(); +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp_v3/bt_factory.h" diff --git a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp index b6afc0d5a4f..fee838c9379 100644 --- a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp @@ -35,7 +35,7 @@ BT::NodeStatus GloballyUpdatedGoalCondition::tick() first_time = false; config().blackboard->get>("goals", goals_); config().blackboard->get("goal", goal_); - return BT::NodeStatus::FAILURE; + return BT::NodeStatus::SUCCESS; } std::vector current_goals; diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 3c6e595ba7d..0b7cae1a6cc 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -19,6 +19,10 @@ ament_add_gtest(test_action_wait_action test_wait_action.cpp) target_link_libraries(test_action_wait_action nav2_wait_action_bt_node) ament_target_dependencies(test_action_wait_action ${dependencies}) +ament_add_gtest(test_action_assisted_teleop_action test_assisted_teleop_action.cpp) +target_link_libraries(test_action_assisted_teleop_action nav2_assisted_teleop_action_bt_node) +ament_target_dependencies(test_action_assisted_teleop_action ${dependencies}) + ament_add_gtest(test_action_controller_cancel_action test_controller_cancel_node.cpp) target_link_libraries(test_action_controller_cancel_action nav2_controller_cancel_bt_node) ament_target_dependencies(test_action_controller_cancel_action ${dependencies}) @@ -36,6 +40,10 @@ ament_add_gtest(test_action_back_up_cancel_action test_back_up_cancel_node.cpp) target_link_libraries(test_action_back_up_cancel_action nav2_back_up_cancel_bt_node) ament_target_dependencies(test_action_back_up_cancel_action ${dependencies}) +ament_add_gtest(test_action_assisted_teleop_cancel_action test_assisted_teleop_cancel_node.cpp) +target_link_libraries(test_action_assisted_teleop_cancel_action nav2_assisted_teleop_cancel_bt_node) +ament_target_dependencies(test_action_assisted_teleop_cancel_action ${dependencies}) + ament_add_gtest(test_action_drive_on_heading_cancel_action test_drive_on_heading_cancel_node.cpp) target_link_libraries(test_action_drive_on_heading_cancel_action nav2_drive_on_heading_cancel_bt_node) ament_target_dependencies(test_action_drive_on_heading_cancel_action ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp new file mode 100644 index 00000000000..d5b033184bf --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp @@ -0,0 +1,215 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp" + +class AssistedTeleopActionServer : public TestActionServer +{ +public: + AssistedTeleopActionServer() + : TestActionServer("assisted_teleop") + {} + +protected: + void execute( + const typename std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) + override + { + nav2_msgs::action::AssistedTeleop::Result::SharedPtr result = + std::make_shared(); + bool return_success = getReturnSuccess(); + if (return_success) { + goal_handle->succeed(result); + } else { + goal_handle->abort(result); + } + } +}; + +class AssistedTeleopActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("backup_action_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_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("number_recoveries", 0); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "assisted_teleop", config); + }; + + factory_->registerBuilder("AssistedTeleop", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + factory_.reset(); + } + + void SetUp() override + { + config_->blackboard->set("number_recoveries", 0); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr action_server_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr AssistedTeleopActionTestFixture::node_ = nullptr; +std::shared_ptr +AssistedTeleopActionTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * AssistedTeleopActionTestFixture::config_ = nullptr; +std::shared_ptr AssistedTeleopActionTestFixture::factory_ = nullptr; +std::shared_ptr AssistedTeleopActionTestFixture::tree_ = nullptr; + +TEST_F(AssistedTeleopActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->getInput("time_allowance"), 10.0); + + xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->getInput("time_allowance"), 20.0); +} + +TEST_F(AssistedTeleopActionTestFixture, test_tick) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 0); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); + + auto goal = action_server_->getCurrentGoal(); + EXPECT_EQ(goal->time_allowance.sec, 10.0); +} + +TEST_F(AssistedTeleopActionTestFixture, test_failure) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + action_server_->setReturnSuccess(false); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 0); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); + + auto goal = action_server_->getCurrentGoal(); + EXPECT_EQ(goal->time_allowance.sec, 10.0); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + AssistedTeleopActionTestFixture::action_server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(AssistedTeleopActionTestFixture::action_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp new file mode 100644 index 00000000000..006370b697c --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp @@ -0,0 +1,173 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" + +class CancelAssistedTeleopServer : public TestActionServer +{ +public: + CancelAssistedTeleopServer() + : TestActionServer("assisted_teleop") + {} + +protected: + void execute( + const typename std::shared_ptr< + rclcpp_action::ServerGoalHandle> + goal_handle) + { + while (!goal_handle->is_canceling()) { + // Assisted Teleop here until goal cancels + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + } + } +}; + +class CancelAssistedTeleopActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("cancel_back_up_action_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_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + client_ = rclcpp_action::create_client( + node_, "assisted_teleop"); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "assisted_teleop", config); + }; + + factory_->registerBuilder( + "CancelAssistedTeleop", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + client_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr action_server_; + static std::shared_ptr> client_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr CancelAssistedTeleopActionTestFixture::node_ = nullptr; +std::shared_ptr +CancelAssistedTeleopActionTestFixture::action_server_ = nullptr; +std::shared_ptr> +CancelAssistedTeleopActionTestFixture::client_ = nullptr; + +BT::NodeConfiguration * CancelAssistedTeleopActionTestFixture::config_ = nullptr; +std::shared_ptr +CancelAssistedTeleopActionTestFixture::factory_ = nullptr; +std::shared_ptr CancelAssistedTeleopActionTestFixture::tree_ = nullptr; + +TEST_F(CancelAssistedTeleopActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto send_goal_options = rclcpp_action::Client< + nav2_msgs::action::AssistedTeleop>::SendGoalOptions(); + + // Creating a dummy goal_msg + auto goal_msg = nav2_msgs::action::AssistedTeleop::Goal(); + + // BackUping for server and sending a goal + client_->wait_for_action_server(); + client_->async_send_goal(goal_msg, send_goal_options); + + // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + + // Executing tick + tree_->rootNode()->executeTick(); + + // BT node should return success, once when the goal is cancelled + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // Adding another test case to check if the goal is infact cancelling + EXPECT_EQ(action_server_->isGoalCancelled(), true); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and back_up on new thread + CancelAssistedTeleopActionTestFixture::action_server_ = + std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(CancelAssistedTeleopActionTestFixture::action_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp b/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp index d41a6d424cc..2e6810f5102 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_globally_updated_goal.cpp @@ -49,7 +49,7 @@ TEST_F(GloballyUpdatedGoalConditionTestFixture, test_behavior) config_->blackboard->set("goal", goal); EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); goal.pose.position.x = 1.0; config_->blackboard->set("goal", goal); diff --git a/nav2_behaviors/CMakeLists.txt b/nav2_behaviors/CMakeLists.txt index 273c5fe664e..df200309d94 100644 --- a/nav2_behaviors/CMakeLists.txt +++ b/nav2_behaviors/CMakeLists.txt @@ -79,6 +79,14 @@ ament_target_dependencies(nav2_back_up_behavior ${dependencies} ) +add_library(nav2_assisted_teleop_behavior SHARED + plugins/assisted_teleop.cpp +) + +ament_target_dependencies(nav2_assisted_teleop_behavior + ${dependencies} +) + pluginlib_export_plugin_description_file(nav2_core behavior_plugin.xml) # Library @@ -106,6 +114,7 @@ rclcpp_components_register_nodes(${library_name} "behavior_server::BehaviorServe install(TARGETS ${library_name} nav2_spin_behavior nav2_wait_behavior + nav2_assisted_teleop_behavior nav2_drive_on_heading_behavior nav2_back_up_behavior ARCHIVE DESTINATION lib @@ -140,6 +149,7 @@ ament_export_include_directories(include) ament_export_libraries(${library_name} nav2_spin_behavior nav2_wait_behavior + nav2_assisted_teleop_behavior nav2_drive_on_heading_behavior nav2_back_up_behavior ) diff --git a/nav2_behaviors/behavior_plugin.xml b/nav2_behaviors/behavior_plugin.xml index 5989f566a99..cd20ae7aa93 100644 --- a/nav2_behaviors/behavior_plugin.xml +++ b/nav2_behaviors/behavior_plugin.xml @@ -22,4 +22,10 @@ + + + + + + diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 06528be8872..579a2fd0ce9 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -95,6 +95,11 @@ class TimedBehavior : public nav2_core::Behavior { } + // an opportunity for a derived class to do something on action completion + virtual void onActionCompletion() + { + } + // configure the server on lifecycle setup void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, @@ -167,6 +172,7 @@ class TimedBehavior : public nav2_core::Behavior std::string global_frame_; std::string robot_base_frame_; double transform_tolerance_; + rclcpp::Duration elasped_time_{0, 0}; // Clock rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; @@ -178,7 +184,7 @@ class TimedBehavior : public nav2_core::Behavior // onRun and cycle functions to execute a specific behavior void execute() { - RCLCPP_INFO(logger_, "Attempting %s", behavior_name_.c_str()); + RCLCPP_INFO(logger_, "Running %s", behavior_name_.c_str()); if (!enabled_) { RCLCPP_WARN( @@ -203,11 +209,13 @@ class TimedBehavior : public nav2_core::Behavior rclcpp::WallRate loop_rate(cycle_frequency_); while (rclcpp::ok()) { + elasped_time_ = steady_clock_.now() - start_time; if (action_server_->is_cancel_requested()) { RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str()); stopRobot(); - result->total_elapsed_time = steady_clock_.now() - start_time; + result->total_elapsed_time = elasped_time_; action_server_->terminate_all(result); + onActionCompletion(); return; } @@ -220,6 +228,7 @@ class TimedBehavior : public nav2_core::Behavior stopRobot(); result->total_elapsed_time = steady_clock_.now() - start_time; action_server_->terminate_current(result); + onActionCompletion(); return; } @@ -230,12 +239,14 @@ class TimedBehavior : public nav2_core::Behavior "%s completed successfully", behavior_name_.c_str()); result->total_elapsed_time = steady_clock_.now() - start_time; action_server_->succeeded_current(result); + onActionCompletion(); return; case Status::FAILED: RCLCPP_WARN(logger_, "%s failed", behavior_name_.c_str()); result->total_elapsed_time = steady_clock_.now() - start_time; action_server_->terminate_current(result); + onActionCompletion(); return; case Status::RUNNING: diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 002a0b17364..e3720b02dc2 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.2 + 1.1.3 TODO Carlos Orduno Steve Macenski diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp new file mode 100644 index 00000000000..defdfaf5a70 --- /dev/null +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -0,0 +1,186 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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 "assisted_teleop.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_behaviors +{ +AssistedTeleop::AssistedTeleop() +: TimedBehavior(), + feedback_(std::make_shared()) +{} + +void AssistedTeleop::onConfigure() +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // set up parameters + nav2_util::declare_parameter_if_not_declared( + node, + "projection_time", rclcpp::ParameterValue(1.0)); + + nav2_util::declare_parameter_if_not_declared( + node, + "simulation_time_step", rclcpp::ParameterValue(0.1)); + + nav2_util::declare_parameter_if_not_declared( + node, + "cmd_vel_teleop", rclcpp::ParameterValue(std::string("cmd_vel_teleop"))); + + node->get_parameter("projection_time", projection_time_); + node->get_parameter("simulation_time_step", simulation_time_step_); + + std::string cmd_vel_teleop; + node->get_parameter("cmd_vel_teleop", cmd_vel_teleop); + + vel_sub_ = node->create_subscription( + cmd_vel_teleop, rclcpp::SystemDefaultsQoS(), + std::bind( + &AssistedTeleop::teleopVelocityCallback, + this, std::placeholders::_1)); + + preempt_teleop_sub_ = node->create_subscription( + "preempt_teleop", rclcpp::SystemDefaultsQoS(), + std::bind( + &AssistedTeleop::preemptTeleopCallback, + this, std::placeholders::_1)); +} + +Status AssistedTeleop::onRun(const std::shared_ptr command) +{ + preempt_teleop_ = false; + command_time_allowance_ = command->time_allowance; + end_time_ = steady_clock_.now() + command_time_allowance_; + return Status::SUCCEEDED; +} + +void AssistedTeleop::onActionCompletion() +{ + teleop_twist_ = geometry_msgs::msg::Twist(); + preempt_teleop_ = false; +} + +Status AssistedTeleop::onCycleUpdate() +{ + feedback_->current_teleop_duration = elasped_time_; + action_server_->publish_feedback(feedback_); + + rclcpp::Duration time_remaining = end_time_ - steady_clock_.now(); + if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { + stopRobot(); + RCLCPP_WARN_STREAM( + logger_, + "Exceeded time allowance before reaching the " << behavior_name_.c_str() << + "goal - Exiting " << behavior_name_.c_str()); + return Status::FAILED; + } + + // user states that teleop was successful + if (preempt_teleop_) { + stopRobot(); + return Status::SUCCEEDED; + } + + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { + RCLCPP_ERROR_STREAM( + logger_, + "Current robot pose is not available for " << + behavior_name_.c_str()); + return Status::FAILED; + } + geometry_msgs::msg::Pose2D projected_pose; + projected_pose.x = current_pose.pose.position.x; + projected_pose.y = current_pose.pose.position.y; + projected_pose.theta = tf2::getYaw(current_pose.pose.orientation); + + geometry_msgs::msg::Twist scaled_twist = teleop_twist_; + for (double time = simulation_time_step_; time < projection_time_; + time += simulation_time_step_) + { + projected_pose = projectPose(projected_pose, teleop_twist_, simulation_time_step_); + + if (!collision_checker_->isCollisionFree(projected_pose)) { + if (time == simulation_time_step_) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, + steady_clock_, + 1000, + behavior_name_.c_str() << " collided on first time step, setting velocity to zero"); + scaled_twist.linear.x = 0.0f; + scaled_twist.linear.y = 0.0f; + scaled_twist.angular.z = 0.0f; + break; + } else { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, + steady_clock_, + 1000, + behavior_name_.c_str() << " collision approaching in " << time << " seconds"); + double scale_factor = time / projection_time_; + scaled_twist.linear.x *= scale_factor; + scaled_twist.linear.y *= scale_factor; + scaled_twist.angular.z *= scale_factor; + break; + } + } + } + vel_pub_->publish(std::move(scaled_twist)); + + return Status::RUNNING; +} + +geometry_msgs::msg::Pose2D AssistedTeleop::projectPose( + const geometry_msgs::msg::Pose2D & pose, + const geometry_msgs::msg::Twist & twist, + double projection_time) +{ + geometry_msgs::msg::Pose2D projected_pose = pose; + + projected_pose.x += projection_time * ( + twist.linear.x * cos(pose.theta) + + twist.linear.y * sin(pose.theta)); + + projected_pose.y += projection_time * ( + twist.linear.x * sin(pose.theta) - + twist.linear.y * cos(pose.theta)); + + projected_pose.theta += projection_time * twist.angular.z; + + return projected_pose; +} + +void AssistedTeleop::teleopVelocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + teleop_twist_ = *msg; +} + +void AssistedTeleop::preemptTeleopCallback(const std_msgs::msg::Empty::SharedPtr) +{ + preempt_teleop_ = true; +} + +} // namespace nav2_behaviors + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_behaviors::AssistedTeleop, nav2_core::Behavior) diff --git a/nav2_behaviors/plugins/assisted_teleop.hpp b/nav2_behaviors/plugins/assisted_teleop.hpp new file mode 100644 index 00000000000..12a85f327c4 --- /dev/null +++ b/nav2_behaviors/plugins/assisted_teleop.hpp @@ -0,0 +1,105 @@ +// Copyright (c) 2022 Joshua Wallace +// +// 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_BEHAVIORS__PLUGINS__ASSISTED_TELEOP_HPP_ +#define NAV2_BEHAVIORS__PLUGINS__ASSISTED_TELEOP_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/twist.hpp" +#include "std_msgs/msg/empty.hpp" +#include "nav2_behaviors/timed_behavior.hpp" +#include "nav2_msgs/action/assisted_teleop.hpp" + +namespace nav2_behaviors +{ +using AssistedTeleopAction = nav2_msgs::action::AssistedTeleop; + +/** + * @class nav2_behaviors::AssistedTeleop + * @brief An action server behavior for assisted teleop + */ +class AssistedTeleop : public TimedBehavior +{ +public: + AssistedTeleop(); + + /** + * @brief Initialization to run behavior + * @param command Goal to execute + * @return Status of behavior + */ + Status onRun(const std::shared_ptr command) override; + + /** + * @brief func to run at the completion of the action + */ + void onActionCompletion() override; + + /** + * @brief Loop function to run behavior + * @return Status of behavior + */ + Status onCycleUpdate() override; + +protected: + /** + * @brief Configuration of behavior action + */ + void onConfigure() override; + + /** + * @brief project a position + * @param pose initial pose to project + * @param twist velocity to project pose by + * @param projection_time time to project by + */ + geometry_msgs::msg::Pose2D projectPose( + const geometry_msgs::msg::Pose2D & pose, + const geometry_msgs::msg::Twist & twist, + double projection_time); + + /** + * @brief Callback function for velocity subscriber + * @param msg received Twist message + */ + void teleopVelocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + + /** + * @brief Callback function to preempt assisted teleop + * @param msg empty message + */ + void preemptTeleopCallback(const std_msgs::msg::Empty::SharedPtr msg); + + AssistedTeleopAction::Feedback::SharedPtr feedback_; + + // parameters + double projection_time_; + double simulation_time_step_; + + geometry_msgs::msg::Twist teleop_twist_; + bool preempt_teleop_{false}; + + // subscribers + rclcpp::Subscription::SharedPtr vel_sub_; + rclcpp::Subscription::SharedPtr preempt_teleop_sub_; + + rclcpp::Duration command_time_allowance_{0, 0}; + rclcpp::Time end_time_; +}; +} // namespace nav2_behaviors + +#endif // NAV2_BEHAVIORS__PLUGINS__ASSISTED_TELEOP_HPP_ diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 117170777ad..1a810c2a231 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.2 + 1.1.3 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 621d93a4356..41e149be7f2 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -58,6 +58,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -95,6 +96,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node bt_navigator_navigate_through_poses_rclcpp_node: diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index d6c62807686..0b22b7146bd 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -58,6 +58,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -95,6 +96,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node bt_navigator_navigate_through_poses_rclcpp_node: diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 76cbbbaef0c..1c70b374063 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -58,6 +58,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -96,6 +97,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node bt_navigator_navigate_through_poses_rclcpp_node: @@ -296,7 +298,7 @@ behavior_server: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: plugin: "nav2_behaviors/Spin" backup: @@ -305,6 +307,8 @@ behavior_server: plugin: "nav2_behaviors/DriveOnHeading" wait: plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 diff --git a/nav2_bringup/worlds/waffle.model b/nav2_bringup/worlds/waffle.model index cd5a89bf235..3c5923d8639 100644 --- a/nav2_bringup/worlds/waffle.model +++ b/nav2_bringup/worlds/waffle.model @@ -98,7 +98,7 @@ - -0.052 0 0.111 0 0 0 + -0.064 0 0.121 0 0 0 0.001 0.000 @@ -133,7 +133,7 @@ true true - -0.064 0 0.121 0 0 0 + -0.064 0 0.15 0 0 0 5 @@ -146,7 +146,7 @@ 0.120000 - 3.5 + 20.0 0.015000 @@ -454,15 +454,6 @@ - - camera_link - camera_rgb_frame - 0.005 0.018 0.013 0 0 0 - - 0 0 1 - - - diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index e60a2e77513..d21efa3b4bf 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -7,11 +7,11 @@ - - - - - + + + + + @@ -22,21 +22,21 @@ - + - + - - - + + + diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 8d7192149d4..a636f3590be 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.2 + 1.1.3 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 7cf1e8e63ec..1168006704e 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -43,6 +43,7 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "nav2_follow_path_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", + "nav2_assisted_teleop_action_bt_node", "nav2_back_up_action_bt_node", "nav2_drive_on_heading_bt_node", "nav2_clear_costmap_service_bt_node", @@ -80,6 +81,8 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "nav2_path_longer_on_approach_bt_node", "nav2_wait_cancel_bt_node", "nav2_spin_cancel_bt_node", + "nav2_assisted_teleop_cancel_bt_node", + "nav2_back_up_cancel_bt_node", "nav2_back_up_cancel_bt_node", "nav2_drive_on_heading_cancel_bt_node" }; diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index e1b1c675670..8544429466a 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -47,6 +47,7 @@ add_library(${library_name} SHARED src/source.cpp src/scan.cpp src/pointcloud.cpp + src/range.cpp src/kinematics.cpp ) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index bb6fac5b753..54841675e90 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -35,7 +35,7 @@ The data may be obtained from different data sources: * Laser scanners (`sensor_msgs::msg::LaserScan` messages) * PointClouds (`sensor_msgs::msg::PointCloud2` messages) -* IR/Sonars (`sensor_msgs::msg::Range` messages, not implemented yet) +* IR/Sonars (`sensor_msgs::msg::Range` messages) ## Design diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index bd902c1ea43..807d4b3dc7f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 2de3c222462..87f498e401d 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -34,6 +34,7 @@ #include "nav2_collision_monitor/source.hpp" #include "nav2_collision_monitor/scan.hpp" #include "nav2_collision_monitor/pointcloud.hpp" +#include "nav2_collision_monitor/range.hpp" namespace nav2_collision_monitor { diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp index 8dc418bd0e9..00038eca9be 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index db3d5e11353..c316c065f72 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -35,7 +35,7 @@ class PointCloud : public Source /** * @brief PointCloud constructor * @param node Collision Monitor node pointer - * @param polygon_name Name of data source + * @param source_name Name of data source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 91a95a1251b..97423dc411d 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp new file mode 100644 index 00000000000..0fbe47501a8 --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp @@ -0,0 +1,101 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// +// 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_COLLISION_MONITOR__RANGE_HPP_ +#define NAV2_COLLISION_MONITOR__RANGE_HPP_ + +#include +#include +#include + +#include "sensor_msgs/msg/range.hpp" + +#include "nav2_collision_monitor/source.hpp" + +namespace nav2_collision_monitor +{ + +/** + * @brief Implementation for IR/ultrasound range sensor source + */ +class Range : public Source +{ +public: + /** + * @brief Range constructor + * @param node Collision Monitor node pointer + * @param source_name Name of data source + * @param tf_buffer Shared pointer to a TF buffer + * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. + * @param global_frame_id Global frame ID for correct transform calculation + * @param transform_tolerance Transform tolerance + * @param source_timeout Maximum time interval in which data is considered valid + */ + Range( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout); + /** + * @brief Range destructor + */ + ~Range(); + + /** + * @brief Data source configuration routine. Obtains ROS-parameters + * and creates range sensor subscriber. + */ + void configure(); + + /** + * @brief Adds latest data from range sensor to the data array. + * @param curr_time Current node time for data interpolation + * @param data Array where the data from source to be added. + * Added data is transformed to base_frame_id_ coordinate system at curr_time. + */ + void getData( + const rclcpp::Time & curr_time, + std::vector & data) const; + +protected: + /** + * @brief Getting sensor-specific ROS-parameters + * @param source_topic Output name of source subscription topic + */ + void getParameters(std::string & source_topic); + + /** + * @brief Range sensor data callback + * @param msg Shared pointer to Range sensor message + */ + void dataCallback(sensor_msgs::msg::Range::ConstSharedPtr msg); + + // ----- Variables ----- + + /// @brief Range sensor data subscriber + rclcpp::Subscription::SharedPtr data_sub_; + + /// @brief Angle increment (in rad) between two obstacle points at the range arc + double obstacles_angle_; + + /// @brief Latest data obtained from range sensor + sensor_msgs::msg::Range::ConstSharedPtr data_; +}; // class Range + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__RANGE_HPP_ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp index f96ae211f82..29747e8131f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -35,7 +35,7 @@ class Scan : public Source /** * @brief Scan constructor * @param node Collision Monitor node pointer - * @param polygon_name Name of data source + * @param source_name Name of data source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 32227e1df39..a24859bb4a5 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -40,7 +40,7 @@ class Source /** * @brief Source constructor * @param node Collision Monitor node pointer - * @param polygon_name Name of data source + * @param source_name Name of data source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp index 2f239d694bd..aa0b9d729ba 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -27,8 +27,9 @@ struct Velocity inline bool operator<(const Velocity & second) const { - const double first_vel = x * x + y * y; - const double second_vel = second.x * second.x + second.y * second.y; + const double first_vel = x * x + y * y + tw * tw; + const double second_vel = second.x * second.x + second.y * second.y + second.tw * second.tw; + // This comparison includes rotations in place, where linear velocities are equal to zero return first_vel < second_vel; } diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py index 7e96aeec2b6..d03d723498e 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright (c) 2022 Samsung Research Russia +# Copyright (c) 2022 Samsung R&D Institute Russia # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index cb7a3008683..53174b7aab0 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.2 + 1.1.3 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_collision_monitor/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp index 6a05477f490..7dc97089677 100644 --- a/nav2_collision_monitor/src/circle.cpp +++ b/nav2_collision_monitor/src/circle.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 5088e2c5a25..313d71fb0ac 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -302,6 +302,14 @@ bool CollisionMonitor::configureSources( p->configure(); sources_.push_back(p); + } else if (source_type == "range") { + std::shared_ptr r = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout); + + r->configure(); + + sources_.push_back(r); } else { // Error if something else RCLCPP_ERROR( get_logger(), diff --git a/nav2_collision_monitor/src/kinematics.cpp b/nav2_collision_monitor/src/kinematics.cpp index 1b6ee226b75..e51b7c48791 100644 --- a/nav2_collision_monitor/src/kinematics.cpp +++ b/nav2_collision_monitor/src/kinematics.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/src/main.cpp b/nav2_collision_monitor/src/main.cpp index 01ae87e3537..6c23d630245 100644 --- a/nav2_collision_monitor/src/main.cpp +++ b/nav2_collision_monitor/src/main.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index eb25c911140..df4e86b63ea 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 0e1c47b59c0..d17c34b46af 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp new file mode 100644 index 00000000000..3ef51e2b69e --- /dev/null +++ b/nav2_collision_monitor/src/range.cpp @@ -0,0 +1,145 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// +// 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 "nav2_collision_monitor/range.hpp" + +#include +#include +#include + +#include "nav2_util/node_utils.hpp" + +namespace nav2_collision_monitor +{ + +Range::Range( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout) +: Source( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, source_timeout), + data_(nullptr) +{ + RCLCPP_INFO(logger_, "[%s]: Creating Range", source_name_.c_str()); +} + +Range::~Range() +{ + RCLCPP_INFO(logger_, "[%s]: Destroying Range", source_name_.c_str()); + data_sub_.reset(); +} + +void Range::configure() +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + std::string source_topic; + + getParameters(source_topic); + + rclcpp::QoS range_qos = rclcpp::SensorDataQoS(); // set to default + data_sub_ = node->create_subscription( + source_topic, range_qos, + std::bind(&Range::dataCallback, this, std::placeholders::_1)); +} + +void Range::getData( + const rclcpp::Time & curr_time, + std::vector & data) const +{ + // Ignore data from the source if it is not being published yet or + // not being published for a long time + if (data_ == nullptr) { + return; + } + if (!sourceValid(data_->header.stamp, curr_time)) { + return; + } + + // Ignore data, if its range is out of scope of range sensor abilities + if (data_->range < data_->min_range || data_->range > data_->max_range) { + RCLCPP_WARN( + logger_, + "[%s]: Data range %fm is out of {%f..%f} sensor span. Ignoring...", + source_name_.c_str(), data_->range, data_->min_range, data_->max_range); + return; + } + + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + tf2::Transform tf_transform; + if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) { + return; + } + + // Calculate poses and refill data array + float angle; + for ( + angle = -data_->field_of_view / 2; + angle < data_->field_of_view / 2; + angle += obstacles_angle_) + { + // Transform point coordinates from source frame -> to base frame + tf2::Vector3 p_v3_s( + data_->range * std::cos(angle), + data_->range * std::sin(angle), + 0.0); + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + + // Refill data array + data.push_back({p_v3_b.x(), p_v3_b.y()}); + } + + // Make sure that last (field_of_view / 2) point will be in the data array + angle = data_->field_of_view / 2; + + // Transform point coordinates from source frame -> to base frame + tf2::Vector3 p_v3_s( + data_->range * std::cos(angle), + data_->range * std::sin(angle), + 0.0); + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + + // Refill data array + data.push_back({p_v3_b.x(), p_v3_b.y()}); +} + +void Range::getParameters(std::string & source_topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + getCommonParameters(source_topic); + + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 180)); + obstacles_angle_ = node->get_parameter(source_name_ + ".obstacles_angle").as_double(); +} + +void Range::dataCallback(sensor_msgs::msg::Range::ConstSharedPtr msg) +{ + data_ = msg; +} + +} // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index a73d9e53cf0..d1f52b31f17 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index e9453016406..bd1028518ce 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 094a3f1e91a..472530f7865 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -26,8 +27,10 @@ #include "nav2_util/lifecycle_node.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/msg/range.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/polygon_stamped.hpp" #include "tf2_ros/transform_broadcaster.h" @@ -36,15 +39,17 @@ using namespace std::chrono_literals; -static constexpr double EPSILON = std::numeric_limits::epsilon(); +static constexpr double EPSILON = 1e-5; static const char BASE_FRAME_ID[]{"base_link"}; static const char SOURCE_FRAME_ID[]{"base_source"}; static const char ODOM_FRAME_ID[]{"odom"}; static const char CMD_VEL_IN_TOPIC[]{"cmd_vel_in"}; static const char CMD_VEL_OUT_TOPIC[]{"cmd_vel_out"}; +static const char FOOTPRINT_TOPIC[]{"footprint"}; static const char SCAN_NAME[]{"Scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; +static const char RANGE_NAME[]{"Range"}; static const int MAX_POINTS{1}; static const double SLOWDOWN_RATIO{0.7}; static const double TIME_BEFORE_COLLISION{1.0}; @@ -64,7 +69,8 @@ enum SourceType { SOURCE_UNKNOWN = 0, SCAN = 1, - POINTCLOUD = 2 + POINTCLOUD = 2, + RANGE = 3 }; class CollisionMonitorWrapper : public nav2_collision_monitor::CollisionMonitor @@ -93,16 +99,19 @@ class CollisionMonitorWrapper : public nav2_collision_monitor::CollisionMonitor ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::FAILURE); } - bool isDataReceived(const rclcpp::Time & stamp) + bool correctDataReceived(const double expected_dist, const rclcpp::Time & stamp) { for (std::shared_ptr source : sources_) { std::vector collision_points; source->getData(stamp, collision_points); - if (collision_points.size() == 0) { - return false; + if (collision_points.size() != 0) { + const double dist = std::hypot(collision_points[0].x, collision_points[0].y); + if (std::fabs(dist - expected_dist) <= EPSILON) { + return true; + } } } - return true; + return false; } }; // CollisionMonitorWrapper @@ -125,11 +134,18 @@ class Tester : public ::testing::Test // Setting TF chains void sendTransforms(const rclcpp::Time & stamp); + // Publish robot footprint + void publishFootprint(const double radius, const rclcpp::Time & stamp); + // Main topic/data working routines void publishScan(const double dist, const rclcpp::Time & stamp); void publishPointCloud(const double dist, const rclcpp::Time & stamp); + void publishRange(const double dist, const rclcpp::Time & stamp); void publishCmdVel(const double x, const double y, const double tw); - bool waitData(const std::chrono::nanoseconds & timeout, const rclcpp::Time & stamp); + bool waitData( + const double expected_dist, + const std::chrono::nanoseconds & timeout, + const rclcpp::Time & stamp); bool waitCmdVel(const std::chrono::nanoseconds & timeout); protected: @@ -138,9 +154,13 @@ class Tester : public ::testing::Test // CollisionMonitor node std::shared_ptr cm_; + // Footprint publisher + rclcpp::Publisher::SharedPtr footprint_pub_; + // Data source publishers rclcpp::Publisher::SharedPtr scan_pub_; rclcpp::Publisher::SharedPtr pointcloud_pub_; + rclcpp::Publisher::SharedPtr range_pub_; // Working with cmd_vel_in/cmd_vel_out rclcpp::Publisher::SharedPtr cmd_vel_in_pub_; @@ -153,10 +173,15 @@ Tester::Tester() { cm_ = std::make_shared(); + footprint_pub_ = cm_->create_publisher( + FOOTPRINT_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + scan_pub_ = cm_->create_publisher( SCAN_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); pointcloud_pub_ = cm_->create_publisher( POINTCLOUD_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + range_pub_ = cm_->create_publisher( + RANGE_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); cmd_vel_in_pub_ = cm_->create_publisher( CMD_VEL_IN_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); @@ -167,8 +192,11 @@ Tester::Tester() Tester::~Tester() { + footprint_pub_.reset(); + scan_pub_.reset(); pointcloud_pub_.reset(); + range_pub_.reset(); cmd_vel_in_pub_.reset(); cmd_vel_out_sub_.reset(); @@ -221,12 +249,19 @@ void Tester::addPolygon( cm_->set_parameter( rclcpp::Parameter(polygon_name + ".type", "polygon")); - const std::vector points { - size, size, size, -size, -size, -size, -size, size}; - cm_->declare_parameter( - polygon_name + ".points", rclcpp::ParameterValue(points)); - cm_->set_parameter( - rclcpp::Parameter(polygon_name + ".points", points)); + if (at != "approach") { + const std::vector points { + size, size, size, -size, -size, -size, -size, size}; + cm_->declare_parameter( + polygon_name + ".points", rclcpp::ParameterValue(points)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", points)); + } else { // at == "approach" + cm_->declare_parameter( + polygon_name + ".footprint_topic", rclcpp::ParameterValue(FOOTPRINT_TOPIC)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".footprint_topic", FOOTPRINT_TOPIC)); + } } else if (type == CIRCLE) { cm_->declare_parameter( polygon_name + ".type", rclcpp::ParameterValue("circle")); @@ -302,6 +337,16 @@ void Tester::addSource( source_name + ".max_height", rclcpp::ParameterValue(1.0)); cm_->set_parameter( rclcpp::Parameter(source_name + ".max_height", 1.0)); + } else if (type == RANGE) { + cm_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("range")); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".type", "range")); + + cm_->declare_parameter( + source_name + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 200)); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".obstacles_angle", M_PI / 200)); } else { // type == SOURCE_UNKNOWN cm_->declare_parameter( source_name + ".type", rclcpp::ParameterValue("unknown")); @@ -354,6 +399,31 @@ void Tester::sendTransforms(const rclcpp::Time & stamp) } } +void Tester::publishFootprint(const double radius, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = BASE_FRAME_ID; + msg->header.stamp = stamp; + + geometry_msgs::msg::Point32 p; + p.x = radius; + p.y = radius; + msg->polygon.points.push_back(p); + p.x = radius; + p.y = -radius; + msg->polygon.points.push_back(p); + p.x = -radius; + p.y = -radius; + msg->polygon.points.push_back(p); + p.x = -radius; + p.y = radius; + msg->polygon.points.push_back(p); + + footprint_pub_->publish(std::move(msg)); +} + void Tester::publishScan(const double dist, const rclcpp::Time & stamp) { std::unique_ptr msg = @@ -408,6 +478,23 @@ void Tester::publishPointCloud(const double dist, const rclcpp::Time & stamp) pointcloud_pub_->publish(std::move(msg)); } +void Tester::publishRange(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + msg->radiation_type = 0; + msg->field_of_view = M_PI / 10; + msg->min_range = 0.1; + msg->max_range = dist + 0.1; + msg->range = dist; + + range_pub_->publish(std::move(msg)); +} + void Tester::publishCmdVel(const double x, const double y, const double tw) { // Reset cmd_vel_out_ before calling CollisionMonitor::process() @@ -423,11 +510,14 @@ void Tester::publishCmdVel(const double x, const double y, const double tw) cmd_vel_in_pub_->publish(std::move(msg)); } -bool Tester::waitData(const std::chrono::nanoseconds & timeout, const rclcpp::Time & stamp) +bool Tester::waitData( + const double expected_dist, + const std::chrono::nanoseconds & timeout, + const rclcpp::Time & stamp) { rclcpp::Time start_time = cm_->now(); while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { - if (cm_->isDataReceived(stamp)) { + if (cm_->correctDataReceived(expected_dist, stamp)) { return true; } rclcpp::spin_some(cm_->get_node_base_interface()); @@ -474,7 +564,7 @@ TEST_F(Tester, testProcessStopSlowdown) // 1. Obstacle is far away from robot publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); @@ -483,7 +573,7 @@ TEST_F(Tester, testProcessStopSlowdown) // 2. Obstacle is in slowdown robot zone publishScan(1.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5 * SLOWDOWN_RATIO, EPSILON); @@ -492,16 +582,16 @@ TEST_F(Tester, testProcessStopSlowdown) // 3. Obstacle is inside stop zone publishScan(0.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); - // 4. Restorint back normal operation + // 4. Restoring back normal operation publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); @@ -531,7 +621,7 @@ TEST_F(Tester, testProcessApproach) // 1. Obstacle is far away from robot publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); @@ -540,7 +630,7 @@ TEST_F(Tester, testProcessApproach) // 2. Approaching obstacle (0.2 m ahead from robot footprint) publishScan(1.2, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(1.2, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); // change_ratio = (0.2 m / speed m/s) / TIME_BEFORE_COLLISION s @@ -554,16 +644,16 @@ TEST_F(Tester, testProcessApproach) // 3. Obstacle is inside robot footprint publishScan(0.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); - // 4. Restorint back normal operation + // 4. Restoring back normal operation publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); @@ -574,6 +664,71 @@ TEST_F(Tester, testProcessApproach) cm_->stop(); } +TEST_F(Tester, testProcessApproachRotation) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making one circle footprint for approach. + setCommonParameters(); + addPolygon("Approach", POLYGON, 1.0, "approach"); + addSource(RANGE_NAME, RANGE); + setVectors({"Approach"}, {RANGE_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Publish robot footprint + publishFootprint(1.0, curr_time); + + // Share TF + sendTransforms(curr_time); + + // 1. Obstacle is far away from robot + publishRange(2.0, curr_time); + ASSERT_TRUE(waitData(2.0, 500ms, curr_time)); + publishCmdVel(0.0, 0.0, M_PI / 4); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, M_PI / 4, EPSILON); + + // 2. Approaching rotation to obstacle ( M_PI / 4 - M_PI / 20 ahead from robot) + publishRange(1.4, curr_time); + ASSERT_TRUE(waitData(1.4, 500ms, curr_time)); + publishCmdVel(0.0, 0.0, M_PI / 4); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR( + cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR( + cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR( + cmd_vel_out_->angular.z, + M_PI / 5, + (M_PI / 4) * (SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION)); + + // 3. Obstacle is inside robot footprint + publishRange(0.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + publishCmdVel(0.0, 0.0, M_PI / 4); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + + // 4. Restoring back normal operation + publishRange(2.5, curr_time); + ASSERT_TRUE(waitData(2.5, 500ms, curr_time)); + publishCmdVel(0.0, 0.0, M_PI / 4); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, M_PI / 4, EPSILON); + + // Stop Collision Monitor node + cm_->stop(); +} + TEST_F(Tester, testCrossOver) { rclcpp::Time curr_time = cm_->now(); @@ -585,7 +740,8 @@ TEST_F(Tester, testCrossOver) addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); addPolygon("Approach", CIRCLE, 1.0, "approach"); addSource(POINTCLOUD_NAME, POINTCLOUD); - setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME}); + addSource(RANGE_NAME, RANGE); + setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME, RANGE_NAME}); // Start Collision Monitor node cm_->start(); @@ -596,7 +752,7 @@ TEST_F(Tester, testCrossOver) // 1. Obstacle is not in the slowdown zone, but less than TIME_BEFORE_COLLISION (ahead in 1.5 m). // Robot should approach the obstacle. publishPointCloud(2.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(std::hypot(2.5, 0.01), 500ms, curr_time)); publishCmdVel(3.0, 0.0, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); // change_ratio = (1.5 m / 3.0 m/s) / TIME_BEFORE_COLLISION s @@ -607,8 +763,8 @@ TEST_F(Tester, testCrossOver) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); // 2. Obstacle is inside slowdown zone, but speed is too slow for approach - publishPointCloud(1.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + publishRange(1.5, curr_time); + ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); publishCmdVel(0.1, 0.0, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.1 * SLOWDOWN_RATIO, EPSILON); @@ -650,7 +806,7 @@ TEST_F(Tester, testCeasePublishZeroVel) // 1. Obstacle is inside approach footprint: robot should stop publishScan(1.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); @@ -666,7 +822,7 @@ TEST_F(Tester, testCeasePublishZeroVel) // 3. Release robot to normal operation publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); @@ -675,7 +831,7 @@ TEST_F(Tester, testCeasePublishZeroVel) // 4. Obstacle is inside stop zone publishScan(0.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); diff --git a/nav2_collision_monitor/test/kinematics_test.cpp b/nav2_collision_monitor/test/kinematics_test.cpp index 2c1ec599a73..65933daa389 100644 --- a/nav2_collision_monitor/test/kinematics_test.cpp +++ b/nav2_collision_monitor/test/kinematics_test.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index ec7a19f124e..87576ddda93 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index c199c02286c..7101b611751 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -26,6 +27,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/msg/range.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" #include "tf2_ros/buffer.h" @@ -35,6 +37,7 @@ #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/scan.hpp" #include "nav2_collision_monitor/pointcloud.hpp" +#include "nav2_collision_monitor/range.hpp" using namespace std::chrono_literals; @@ -47,6 +50,8 @@ static const char SCAN_NAME[]{"LaserScan"}; static const char SCAN_TOPIC[]{"scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; static const char POINTCLOUD_TOPIC[]{"pointcloud"}; +static const char RANGE_NAME[]{"Range"}; +static const char RANGE_TOPIC[]{"range"}; static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; static const rclcpp::Duration DATA_TIMEOUT{rclcpp::Duration::from_seconds(5.0)}; @@ -61,9 +66,11 @@ class TestNode : public nav2_util::LifecycleNode ~TestNode() { scan_pub_.reset(); + pointcloud_pub_.reset(); + range_pub_.reset(); } - void publishScan(const rclcpp::Time & stamp) + void publishScan(const rclcpp::Time & stamp, const double range) { scan_pub_ = this->create_publisher( SCAN_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); @@ -81,7 +88,7 @@ class TestNode : public nav2_util::LifecycleNode msg->scan_time = 0.0; msg->range_min = 0.1; msg->range_max = 1.1; - std::vector ranges(4, 1.0); + std::vector ranges(4, range); msg->ranges = ranges; scan_pub_->publish(std::move(msg)); @@ -129,9 +136,30 @@ class TestNode : public nav2_util::LifecycleNode pointcloud_pub_->publish(std::move(msg)); } + void publishRange(const rclcpp::Time & stamp, const double range) + { + range_pub_ = this->create_publisher( + RANGE_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + msg->radiation_type = 0; + msg->field_of_view = M_PI / 10; + msg->min_range = 0.1; + msg->max_range = 1.1; + msg->range = range; + + range_pub_->publish(std::move(msg)); + } + private: rclcpp::Publisher::SharedPtr scan_pub_; rclcpp::Publisher::SharedPtr pointcloud_pub_; + rclcpp::Publisher::SharedPtr range_pub_; }; // TestNode class ScanWrapper : public nav2_collision_monitor::Scan @@ -178,6 +206,28 @@ class PointCloudWrapper : public nav2_collision_monitor::PointCloud } }; // PointCloudWrapper +class RangeWrapper : public nav2_collision_monitor::Range +{ +public: + RangeWrapper( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & data_timeout) + : nav2_collision_monitor::Range( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, data_timeout) + {} + + bool dataReceived() const + { + return data_ != nullptr; + } +}; // RangeWrapper + class Tester : public ::testing::Test { public: @@ -191,12 +241,15 @@ class Tester : public ::testing::Test // Data sources working routines bool waitScan(const std::chrono::nanoseconds & timeout); bool waitPointCloud(const std::chrono::nanoseconds & timeout); + bool waitRange(const std::chrono::nanoseconds & timeout); void checkScan(const std::vector & data); void checkPointCloud(const std::vector & data); + void checkRange(const std::vector & data); std::shared_ptr test_node_; std::shared_ptr scan_; std::shared_ptr pointcloud_; + std::shared_ptr range_; private: std::shared_ptr tf_buffer_; @@ -242,12 +295,28 @@ Tester::Tester() BASE_FRAME_ID, GLOBAL_FRAME_ID, TRANSFORM_TOLERANCE, DATA_TIMEOUT); pointcloud_->configure(); + + // Create Range object + test_node_->declare_parameter( + std::string(RANGE_NAME) + ".topic", rclcpp::ParameterValue(RANGE_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(RANGE_NAME) + ".topic", RANGE_TOPIC)); + + test_node_->declare_parameter( + std::string(RANGE_NAME) + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 199)); + + range_ = std::make_shared( + test_node_, RANGE_NAME, tf_buffer_, + BASE_FRAME_ID, GLOBAL_FRAME_ID, + TRANSFORM_TOLERANCE, DATA_TIMEOUT); + range_->configure(); } Tester::~Tester() { scan_.reset(); pointcloud_.reset(); + range_.reset(); test_node_.reset(); @@ -313,6 +382,19 @@ bool Tester::waitPointCloud(const std::chrono::nanoseconds & timeout) return false; } +bool Tester::waitRange(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (range_->dataReceived()) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + void Tester::checkScan(const std::vector & data) { ASSERT_EQ(data.size(), 4u); @@ -349,6 +431,24 @@ void Tester::checkPointCloud(const std::vector & // Point 2 should be out of scope by height } +void Tester::checkRange(const std::vector & data) +{ + ASSERT_EQ(data.size(), 21u); + + const double angle_increment = M_PI / 199; + double angle = -M_PI / (10 * 2); + int i; + for (i = 0; i < 199 / 10 + 1; i++) { + ASSERT_NEAR(data[i].x, 1.0 * std::cos(angle) + 0.1, EPSILON); + ASSERT_NEAR(data[i].y, 1.0 * std::sin(angle) + 0.1, EPSILON); + angle += angle_increment; + } + // Check for the latest FoW/2 point + angle = M_PI / (10 * 2); + ASSERT_NEAR(data[i].x, 1.0 * std::cos(angle) + 0.1, EPSILON); + ASSERT_NEAR(data[i].y, 1.0 * std::sin(angle) + 0.1, EPSILON); +} + TEST_F(Tester, testGetData) { rclcpp::Time curr_time = test_node_->now(); @@ -356,12 +456,14 @@ TEST_F(Tester, testGetData) sendTransforms(curr_time); // Publish data for sources - test_node_->publishScan(curr_time); + test_node_->publishScan(curr_time, 1.0); test_node_->publishPointCloud(curr_time); + test_node_->publishRange(curr_time, 1.0); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); + ASSERT_TRUE(waitRange(500ms)); // Check Scan data std::vector data; @@ -372,6 +474,11 @@ TEST_F(Tester, testGetData) data.clear(); pointcloud_->getData(curr_time, data); checkPointCloud(data); + + // Check Range data + data.clear(); + range_->getData(curr_time, data); + checkRange(data); } TEST_F(Tester, testGetOutdatedData) @@ -381,12 +488,14 @@ TEST_F(Tester, testGetOutdatedData) sendTransforms(curr_time); // Publish outdated data for sources - test_node_->publishScan(curr_time - DATA_TIMEOUT - 1s); + test_node_->publishScan(curr_time - DATA_TIMEOUT - 1s, 1.0); test_node_->publishPointCloud(curr_time - DATA_TIMEOUT - 1s); + test_node_->publishRange(curr_time - DATA_TIMEOUT - 1s, 1.0); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); + ASSERT_TRUE(waitRange(500ms)); // Scan data should be empty std::vector data; @@ -396,6 +505,10 @@ TEST_F(Tester, testGetOutdatedData) // Pointcloud data should be empty pointcloud_->getData(curr_time, data); ASSERT_EQ(data.size(), 0u); + + // Range data should be empty + range_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); } TEST_F(Tester, testIncorrectFrameData) @@ -406,12 +519,14 @@ TEST_F(Tester, testIncorrectFrameData) sendTransforms(curr_time - 1s); // Publish data for sources - test_node_->publishScan(curr_time); + test_node_->publishScan(curr_time, 1.0); test_node_->publishPointCloud(curr_time); + test_node_->publishRange(curr_time, 1.0); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); + ASSERT_TRUE(waitRange(500ms)); // Scan data should be empty std::vector data; @@ -421,6 +536,35 @@ TEST_F(Tester, testIncorrectFrameData) // Pointcloud data should be empty pointcloud_->getData(curr_time, data); ASSERT_EQ(data.size(), 0u); + + // Range data should be empty + range_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); +} + +TEST_F(Tester, testIncorrectData) +{ + rclcpp::Time curr_time = test_node_->now(); + + sendTransforms(curr_time); + + // Publish data for sources + test_node_->publishScan(curr_time, 2.0); + test_node_->publishPointCloud(curr_time); + test_node_->publishRange(curr_time, 2.0); + + // Wait until all sources will receive the data + ASSERT_TRUE(waitScan(500ms)); + ASSERT_TRUE(waitRange(500ms)); + + // Scan data should be empty + std::vector data; + scan_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); + + // Range data should be empty + range_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); } int main(int argc, char ** argv) diff --git a/nav2_common/package.xml b/nav2_common/package.xml index a4812375de1..2af8bace078 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.2 + 1.1.3 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/README.md b/nav2_constrained_smoother/README.md index 8359e5d3d72..f42f08f9d27 100644 --- a/nav2_constrained_smoother/README.md +++ b/nav2_constrained_smoother/README.md @@ -29,7 +29,7 @@ smoother_server: # Parameters used to improve obstacle avoidance near cusps (forward/reverse movement changes) # See the [docs page](https://navigation.ros.org/configuration/packages/configuring-constrained-smoother) for further clarification w_cost_cusp_multiplier: 3.0 # option to have higher weight during forward/reverse direction change which is often accompanied with dangerous rotations - cusp_zone_length: 2.5 # length of the section around cusp in which nodes use w_cost_cusp_multiplier (w_cost rises gradually inside the zone towards the cusp point, whose costmap weight eqals w_cost*w_cost_cusp_multiplier) + cusp_zone_length: 2.5 # length of the section around cusp in which nodes use w_cost_cusp_multiplier (w_cost rises gradually inside the zone towards the cusp point, whose costmap weight equals w_cost*w_cost_cusp_multiplier) # Points in robot frame to grab costmap values from. Format: [x1, y1, weight1, x2, y2, weight2, ...] # IMPORTANT: Requires much higher number of iterations to actually improve the path. Uncomment only if you really need it (highly elongated/asymmetric robots) diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp index 8272b2f2aa4..c0c3919cccf 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp @@ -159,7 +159,7 @@ class SmootherCostFunction Eigen::Matrix center = arcCenter( pt_prev, pt, pt_next, next_to_last_length_ratio_ < 0); - if (ceres::IsInfinite(center[0])) { + if (ceres::isinf(center[0])) { return; } T turning_rad = (pt - center).norm(); diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp index 9be4e88f0ba..682ed1c16b9 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp @@ -86,7 +86,7 @@ inline Eigen::Matrix tangentDir( bool is_cusp) { Eigen::Matrix center = arcCenter(pt_prev, pt, pt_next, is_cusp); - if (ceres::IsInfinite(center[0])) { // straight line + if (ceres::isinf(center[0])) { // straight line Eigen::Matrix d1 = pt - pt_prev; Eigen::Matrix d2 = pt_next - pt; diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index 12aa84f07bb..9b41192a0d7 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.2 + 1.1.3 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index a9f5922005e..50faaaf4cbc 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.2 + 1.1.3 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index ce9f99acb42..3e796d0ef19 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.2 + 1.1.3 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 84f86617ab5..96b4f493393 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(rmw REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) @@ -66,6 +67,7 @@ set(dependencies rclcpp_lifecycle sensor_msgs std_msgs + std_srvs tf2 tf2_geometry_msgs tf2_ros @@ -96,6 +98,7 @@ target_link_libraries(layers add_library(filters SHARED plugins/costmap_filters/keepout_filter.cpp plugins/costmap_filters/speed_filter.cpp + plugins/costmap_filters/binary_filter.cpp ) ament_target_dependencies(filters ${dependencies} diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index d7cb9854933..e5867d51089 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -24,6 +24,9 @@ Restricts maximum speed of robot in speed-limit zones marked on map. + + Binary flip filter. + diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp new file mode 100644 index 00000000000..e36507ef8fb --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp @@ -0,0 +1,126 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2022 Samsung R&D Institute Russia + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Alexey Merzlyakov + *********************************************************************/ + +#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__BINARY_FILTER_HPP_ +#define NAV2_COSTMAP_2D__COSTMAP_FILTERS__BINARY_FILTER_HPP_ + +#include +#include + +#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" + +#include "std_msgs/msg/bool.hpp" +#include "nav2_msgs/msg/costmap_filter_info.hpp" + +namespace nav2_costmap_2d +{ +/** + * @class BinaryFilter + * @brief Reads in a speed restriction mask and enables a robot to + * dynamically adjust speed based on pose in map to slow in dangerous + * areas. Done via absolute speed setting or percentage of maximum speed + */ +class BinaryFilter : public CostmapFilter +{ +public: + /** + * @brief A constructor + */ + BinaryFilter(); + + /** + * @brief Initialize the filter and subscribe to the info topic + */ + void initializeFilter( + const std::string & filter_info_topic); + + /** + * @brief Process the keepout layer at the current pose / bounds / grid + */ + void process( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j, + const geometry_msgs::msg::Pose2D & pose); + + /** + * @brief Reset the costmap filter / topic / info + */ + void resetFilter(); + + /** + * @brief If this filter is active + */ + bool isActive(); + +private: + /** + * @brief Callback for the filter information + */ + void filterInfoCallback(const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg); + /** + * @brief Callback for the filter mask + */ + void maskCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + /** + * @brief Changes binary state of filter. Sends a message with new state. + * @param state New binary state + */ + void changeState(const bool state); + + // Working with filter info and mask + rclcpp::Subscription::SharedPtr filter_info_sub_; + rclcpp::Subscription::SharedPtr mask_sub_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr binary_state_pub_; + + nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_; + + std::string mask_frame_; // Frame where mask located in + std::string global_frame_; // Frame of currnet layer (master_grid) + + double base_, multiplier_; + // Filter values higher than this threshold, + // will set binary state to non-default + double flip_threshold_; + + bool default_state_; // Default Binary Filter state + bool binary_state_; // Current Binary Filter state +}; + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__BINARY_FILTER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index c71d3bfea13..35f53c6b96c 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -42,7 +42,9 @@ #include #include "geometry_msgs/msg/pose2_d.hpp" +#include "std_srvs/srv/set_bool.hpp" #include "nav2_costmap_2d/layer.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" namespace nav2_costmap_2d { @@ -153,6 +155,59 @@ class CostmapFilter : public Layer virtual void resetFilter() = 0; protected: + /** + * @brief Costmap filter enabling/disabling callback + * @param request_header Service request header + * @param request Service request + * @param response Service response + */ + void enableCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + /** + * @brief: Transforms robot pose from current layer frame to mask frame + * @param: global_frame Costmap frame to transform from + * @param: global_pose Robot pose in costmap frame + * @param: mask_frame Filter mask frame to transform to + * @param: mask_pose Output robot pose in mask frame + * @return: True if the transformation was successful, false otherwise + */ + bool transformPose( + const std::string global_frame, + const geometry_msgs::msg::Pose2D & global_pose, + const std::string mask_frame, + geometry_msgs::msg::Pose2D & mask_pose) const; + + /** + * @brief: Convert from world coordinates to mask coordinates. + Similar to Costmap2D::worldToMap() method but works directly with OccupancyGrid-s. + * @param filter_mask Filter mask on which to convert + * @param wx The x world coordinate + * @param wy The y world coordinate + * @param mx Will be set to the associated mask x coordinate + * @param my Will be set to the associated mask y coordinate + * @return True if the conversion was successful (legal bounds) false otherwise + */ + bool worldToMask( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, + double wx, double wy, unsigned int & mx, unsigned int & my) const; + + /** + * @brief Get the data of a cell in the filter mask + * @param filter_mask Filter mask to get the data from + * @param mx The x coordinate of the cell + * @param my The y coordinate of the cell + * @return The data of the selected cell + */ + inline int8_t getMaskData( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, + const unsigned int mx, const unsigned int my) const + { + return filter_mask->data[my * filter_mask->info.width + mx]; + } + /** * @brief: Name of costmap filter info topic */ @@ -168,6 +223,11 @@ class CostmapFilter : public Layer */ tf2::Duration transform_tolerance_; + /** + * @brief: A service to enable/disable costmap filter + */ + rclcpp::Service::SharedPtr enable_service_; + private: /** * @brief: Latest robot position diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp index 03a157a3e44..89bdacf1bae 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp @@ -47,6 +47,7 @@ namespace nav2_costmap_2d static constexpr uint8_t KEEPOUT_FILTER = 0; static constexpr uint8_t SPEED_FILTER_PERCENT = 1; static constexpr uint8_t SPEED_FILTER_ABSOLUTE = 2; +static constexpr uint8_t BINARY_FILTER = 3; /** Default values for base and multiplier */ static constexpr double BASE_DEFAULT = 0.0; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp index e5434571d47..54159cf6242 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp @@ -44,7 +44,6 @@ #include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" #include "rclcpp/rclcpp.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_msgs/msg/costmap_filter_info.hpp" namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp index c4655607aa8..3ad4b9ba272 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp @@ -43,7 +43,6 @@ #include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_msgs/msg/costmap_filter_info.hpp" #include "nav2_msgs/msg/speed_limit.hpp" @@ -87,37 +86,6 @@ class SpeedFilter : public CostmapFilter */ bool isActive(); -protected: - /** - * @brief: Transforms robot pose from current layer frame to mask frame - * @param: pose Robot pose in costmap frame - * @param: mask_pose Robot pose in mask frame - * @return: True if the transformation was successful, false otherwise - */ - bool transformPose( - const geometry_msgs::msg::Pose2D & pose, - geometry_msgs::msg::Pose2D & mask_pose) const; - - /** - * @brief: Convert from world coordinates to mask coordinates. - Similar to Costmap2D::worldToMap() method but works directly with OccupancyGrid-s. - * @param wx The x world coordinate - * @param wy The y world coordinate - * @param mx Will be set to the associated mask x coordinate - * @param my Will be set to the associated mask y coordinate - * @return True if the conversion was successful (legal bounds) false otherwise - */ - bool worldToMask(double wx, double wy, unsigned int & mx, unsigned int & my) const; - - /** - * @brief Get the data of a cell in the filter mask - * @param mx The x coordinate of the cell - * @param my The y coordinate of the cell - * @return The data of the selected cell - */ - inline int8_t getMaskData( - const unsigned int mx, const unsigned int my) const; - private: /** * @brief Callback for the filter information diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index def79e349f4..aec68fac1f5 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -183,8 +183,8 @@ class StaticLayer : public CostmapLayer unsigned char unknown_cost_value_; bool trinary_costmap_; bool map_received_{false}; + bool map_received_in_update_bounds_{false}; tf2::Duration transform_tolerance_; - std::atomic update_in_progress_; nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_; // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 8e889dd61e8..73dcd3d87d2 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.1.2 + 1.1.3 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 @@ -32,6 +32,7 @@ rclcpp_lifecycle sensor_msgs std_msgs + std_srvs tf2 tf2_geometry_msgs tf2_ros diff --git a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp new file mode 100644 index 00000000000..b57a27c4149 --- /dev/null +++ b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp @@ -0,0 +1,268 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2022 Samsung R&D Institute Russia + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Alexey Merzlyakov + *********************************************************************/ + +#include "nav2_costmap_2d/costmap_filters/binary_filter.hpp" + +#include +#include +#include +#include + +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "nav2_util/occ_grid_values.hpp" + +namespace nav2_costmap_2d +{ + +BinaryFilter::BinaryFilter() +: filter_info_sub_(nullptr), mask_sub_(nullptr), + binary_state_pub_(nullptr), filter_mask_(nullptr), mask_frame_(""), global_frame_(""), + default_state_(false), binary_state_(default_state_) +{ +} + +void BinaryFilter::initializeFilter( + const std::string & filter_info_topic) +{ + std::lock_guard guard(*getMutex()); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Declare parameters specific to BinaryFilter only + std::string binary_state_topic; + declareParameter("default_state", rclcpp::ParameterValue(false)); + node->get_parameter(name_ + "." + "default_state", default_state_); + declareParameter("binary_state_topic", rclcpp::ParameterValue("binary_state")); + node->get_parameter(name_ + "." + "binary_state_topic", binary_state_topic); + declareParameter("flip_threshold", rclcpp::ParameterValue(50.0)); + node->get_parameter(name_ + "." + "flip_threshold", flip_threshold_); + + filter_info_topic_ = filter_info_topic; + // Setting new costmap filter info subscriber + RCLCPP_INFO( + logger_, + "BinaryFilter: Subscribing to \"%s\" topic for filter info...", + filter_info_topic_.c_str()); + filter_info_sub_ = node->create_subscription( + filter_info_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&BinaryFilter::filterInfoCallback, this, std::placeholders::_1)); + + // Get global frame required for binary state publisher + global_frame_ = layered_costmap_->getGlobalFrameID(); + + // Create new binary state publisher + binary_state_pub_ = node->create_publisher( + binary_state_topic, rclcpp::QoS(10)); + binary_state_pub_->on_activate(); + + // Reset parameters + base_ = BASE_DEFAULT; + multiplier_ = MULTIPLIER_DEFAULT; + + // Initialize state as "false" by-default + changeState(default_state_); +} + +void BinaryFilter::filterInfoCallback( + const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg) +{ + std::lock_guard guard(*getMutex()); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!mask_sub_) { + RCLCPP_INFO( + logger_, + "BinaryFilter: Received filter info from %s topic.", filter_info_topic_.c_str()); + } else { + RCLCPP_WARN( + logger_, + "BinaryFilter: New costmap filter info arrived from %s topic. Updating old filter info.", + filter_info_topic_.c_str()); + // Resetting previous subscriber each time when new costmap filter information arrives + mask_sub_.reset(); + } + + if (msg->type != BINARY_FILTER) { + RCLCPP_ERROR(logger_, "BinaryFilter: Mode %i is not supported", msg->type); + return; + } + + // Set base_ and multiplier_ + base_ = msg->base; + multiplier_ = msg->multiplier; + // Set topic name to receive filter mask from + mask_topic_ = msg->filter_mask_topic; + + // Setting new filter mask subscriber + RCLCPP_INFO( + logger_, + "BinaryFilter: Subscribing to \"%s\" topic for filter mask...", + mask_topic_.c_str()); + mask_sub_ = node->create_subscription( + mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&BinaryFilter::maskCallback, this, std::placeholders::_1)); +} + +void BinaryFilter::maskCallback( + const nav_msgs::msg::OccupancyGrid::SharedPtr msg) +{ + std::lock_guard guard(*getMutex()); + + if (!filter_mask_) { + RCLCPP_INFO( + logger_, + "BinaryFilter: Received filter mask from %s topic.", mask_topic_.c_str()); + } else { + RCLCPP_WARN( + logger_, + "BinaryFilter: New filter mask arrived from %s topic. Updating old filter mask.", + mask_topic_.c_str()); + filter_mask_.reset(); + } + + filter_mask_ = msg; + mask_frame_ = msg->header.frame_id; +} + +void BinaryFilter::process( + nav2_costmap_2d::Costmap2D & /*master_grid*/, + int /*min_i*/, int /*min_j*/, int /*max_i*/, int /*max_j*/, + const geometry_msgs::msg::Pose2D & pose) +{ + std::lock_guard guard(*getMutex()); + + if (!filter_mask_) { + // Show warning message every 2 seconds to not litter an output + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 2000, + "BinaryFilter: Filter mask was not received"); + return; + } + + geometry_msgs::msg::Pose2D mask_pose; // robot coordinates in mask frame + + // Transforming robot pose from current layer frame to mask frame + if (!transformPose(global_frame_, pose, mask_frame_, mask_pose)) { + return; + } + + // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j) + unsigned int mask_robot_i, mask_robot_j; + if (!worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) { + // Robot went out of mask range. Set "false" state by-default + RCLCPP_WARN( + logger_, + "BinaryFilter: Robot is outside of filter mask. Resetting binary state to default."); + changeState(default_state_); + return; + } + + // Getting filter_mask data from cell where the robot placed + int8_t mask_data = getMaskData(filter_mask_, mask_robot_i, mask_robot_j); + if (mask_data == nav2_util::OCC_GRID_UNKNOWN) { + // Corresponding filter mask cell is unknown. + // Warn and do nothing. + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 2000, + "BinaryFilter: Filter mask [%i, %i] data is unknown. Do nothing.", + mask_robot_i, mask_robot_j); + return; + } + // Check and flip binary state, if necessary + if (base_ + mask_data * multiplier_ > flip_threshold_) { + if (binary_state_ == default_state_) { + changeState(!default_state_); + } + } else { + if (binary_state_ != default_state_) { + changeState(default_state_); + } + } +} + +void BinaryFilter::resetFilter() +{ + std::lock_guard guard(*getMutex()); + + RCLCPP_INFO(logger_, "BinaryFilter: Resetting the filter to default state"); + changeState(default_state_); + + filter_info_sub_.reset(); + mask_sub_.reset(); + if (binary_state_pub_) { + binary_state_pub_->on_deactivate(); + binary_state_pub_.reset(); + } +} + +bool BinaryFilter::isActive() +{ + std::lock_guard guard(*getMutex()); + + if (filter_mask_) { + return true; + } + return false; +} + +void BinaryFilter::changeState(const bool state) +{ + binary_state_ = state; + if (state) { + RCLCPP_INFO(logger_, "BinaryFilter: Switched on"); + } else { + RCLCPP_INFO(logger_, "BinaryFilter: Switched off"); + } + + // Forming and publishing new BinaryState message + std::unique_ptr msg = + std::make_unique(); + msg->data = state; + binary_state_pub_->publish(std::move(msg)); +} + +} // namespace nav2_costmap_2d + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::BinaryFilter, nav2_costmap_2d::Layer) diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index cdec0a4b2ef..40045494909 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -39,6 +39,9 @@ #include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" + namespace nav2_costmap_2d { @@ -72,6 +75,13 @@ void CostmapFilter::onInitialize() double transform_tolerance; node->get_parameter(name_ + "." + "transform_tolerance", transform_tolerance); transform_tolerance_ = tf2::durationFromSec(transform_tolerance); + + // Costmap Filter enabling service + enable_service_ = node->create_service( + name_ + "/toggle_filter", + std::bind( + &CostmapFilter::enableCallback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } catch (const std::exception & ex) { RCLCPP_ERROR(logger_, "Parameter problem: %s", ex.what()); throw ex; @@ -120,4 +130,80 @@ void CostmapFilter::updateCosts( current_ = true; } +void CostmapFilter::enableCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + enabled_ = request->data; + response->success = true; + if (enabled_) { + response->message = "Enabled"; + } else { + response->message = "Disabled"; + } +} + +bool CostmapFilter::transformPose( + const std::string global_frame, + const geometry_msgs::msg::Pose2D & global_pose, + const std::string mask_frame, + geometry_msgs::msg::Pose2D & mask_pose) const +{ + if (mask_frame != global_frame) { + // Filter mask and current layer are in different frames: + // Transform (global_pose.x, global_pose.y) point from current layer frame (global_frame) + // to mask_pose point in mask_frame + geometry_msgs::msg::TransformStamped transform; + geometry_msgs::msg::PointStamped in, out; + in.header.stamp = clock_->now(); + in.header.frame_id = global_frame; + in.point.x = global_pose.x; + in.point.y = global_pose.y; + in.point.z = 0; + + try { + tf_->transform(in, out, mask_frame, transform_tolerance_); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + logger_, + "CostmapFilter: failed to get costmap frame (%s) " + "transformation to mask frame (%s) with error: %s", + global_frame.c_str(), mask_frame.c_str(), ex.what()); + return false; + } + mask_pose.x = out.point.x; + mask_pose.y = out.point.y; + } else { + // Filter mask and current layer are in the same frame: + // Just use global_pose coordinates + mask_pose = global_pose; + } + + return true; +} + +bool CostmapFilter::worldToMask( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, + double wx, double wy, unsigned int & mx, unsigned int & my) const +{ + double origin_x = filter_mask->info.origin.position.x; + double origin_y = filter_mask->info.origin.position.y; + double resolution = filter_mask->info.resolution; + unsigned int size_x = filter_mask->info.width; + unsigned int size_y = filter_mask->info.height; + + if (wx < origin_x || wy < origin_y) { + return false; + } + + mx = std::round((wx - origin_x) / resolution); + my = std::round((wy - origin_y) / resolution); + if (mx >= size_x || my >= size_y) { + return false; + } + + return true; +} + } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp index cc368b46833..5f984a10fca 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp @@ -41,8 +41,6 @@ #include #include #include -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "geometry_msgs/msg/point_stamped.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" @@ -174,70 +172,6 @@ void SpeedFilter::maskCallback( mask_frame_ = msg->header.frame_id; } -bool SpeedFilter::transformPose( - const geometry_msgs::msg::Pose2D & pose, - geometry_msgs::msg::Pose2D & mask_pose) const -{ - if (mask_frame_ != global_frame_) { - // Filter mask and current layer are in different frames: - // Transform (pose.x, pose.y) point from current layer frame (global_frame_) - // to mask_pose point in mask_frame_ - geometry_msgs::msg::TransformStamped transform; - geometry_msgs::msg::PointStamped in, out; - in.header.stamp = clock_->now(); - in.header.frame_id = global_frame_; - in.point.x = pose.x; - in.point.y = pose.y; - in.point.z = 0; - - try { - tf_->transform(in, out, mask_frame_, transform_tolerance_); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - logger_, - "SpeedFilter: failed to get costmap frame (%s) " - "transformation to mask frame (%s) with error: %s", - global_frame_.c_str(), mask_frame_.c_str(), ex.what()); - return false; - } - mask_pose.x = out.point.x; - mask_pose.y = out.point.y; - } else { - // Filter mask and current layer are in the same frame: - // Just use pose coordinates - mask_pose = pose; - } - - return true; -} - -bool SpeedFilter::worldToMask(double wx, double wy, unsigned int & mx, unsigned int & my) const -{ - double origin_x = filter_mask_->info.origin.position.x; - double origin_y = filter_mask_->info.origin.position.y; - double resolution = filter_mask_->info.resolution; - unsigned int size_x = filter_mask_->info.width; - unsigned int size_y = filter_mask_->info.height; - - if (wx < origin_x || wy < origin_y) { - return false; - } - - mx = std::round((wx - origin_x) / resolution); - my = std::round((wy - origin_y) / resolution); - if (mx >= size_x || my >= size_y) { - return false; - } - - return true; -} - -inline int8_t SpeedFilter::getMaskData( - const unsigned int mx, const unsigned int my) const -{ - return filter_mask_->data[my * filter_mask_->info.width + mx]; -} - void SpeedFilter::process( nav2_costmap_2d::Costmap2D & /*master_grid*/, int /*min_i*/, int /*min_j*/, int /*max_i*/, int /*max_j*/, @@ -256,19 +190,19 @@ void SpeedFilter::process( geometry_msgs::msg::Pose2D mask_pose; // robot coordinates in mask frame // Transforming robot pose from current layer frame to mask frame - if (!transformPose(pose, mask_pose)) { + if (!transformPose(global_frame_, pose, mask_frame_, mask_pose)) { return; } // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j) unsigned int mask_robot_i, mask_robot_j; - if (!worldToMask(mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) { + if (!worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) { return; } // Getting filter_mask data from cell where the robot placed and // calculating speed limit value - int8_t speed_mask_data = getMaskData(mask_robot_i, mask_robot_j); + int8_t speed_mask_data = getMaskData(filter_mask_, mask_robot_i, mask_robot_j); if (speed_mask_data == SPEED_MASK_NO_LIMIT) { // Corresponding filter mask cell is free. // Setting no speed limit there. diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 073109c2352..18cd51164f2 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -161,7 +161,7 @@ StaticLayer::getParameters() // Enforce bounds lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); map_received_ = false; - update_in_progress_.store(false); + map_received_in_update_bounds_ = false; transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); @@ -277,17 +277,13 @@ StaticLayer::interpretValue(unsigned char value) void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map) { - std::lock_guard guard(*getMutex()); if (!map_received_) { - map_received_ = true; - processMap(*new_map); - } - if (update_in_progress_.load()) { - map_buffer_ = new_map; - } else { processMap(*new_map); - map_buffer_ = nullptr; + map_received_ = true; + return; } + std::lock_guard guard(*getMutex()); + map_buffer_ = new_map; } void @@ -338,11 +334,12 @@ StaticLayer::updateBounds( double * max_y) { if (!map_received_) { + map_received_in_update_bounds_ = false; return; } + map_received_in_update_bounds_ = true; std::lock_guard guard(*getMutex()); - update_in_progress_.store(true); // If there is a new available map, load it. if (map_buffer_) { @@ -378,17 +375,15 @@ StaticLayer::updateCosts( { std::lock_guard guard(*getMutex()); if (!enabled_) { - update_in_progress_.store(false); return; } - if (!map_received_) { + if (!map_received_in_update_bounds_) { static int count = 0; // throttle warning down to only 1/10 message rate if (++count == 10) { RCLCPP_WARN(logger_, "Can't update static costmap layer, no map received"); count = 0; } - update_in_progress_.store(false); return; } @@ -411,7 +406,6 @@ StaticLayer::updateCosts( transform_tolerance_); } catch (tf2::TransformException & ex) { RCLCPP_ERROR(logger_, "StaticLayer: %s", ex.what()); - update_in_progress_.store(false); return; } // Copy map data given proper transformations @@ -436,7 +430,6 @@ StaticLayer::updateCosts( } } } - update_in_progress_.store(false); current_ = true; } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 12d6b2ee687..58f346ba4fd 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -637,11 +637,27 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter } } else if (type == ParameterType::PARAMETER_INTEGER) { if (name == "width") { - resize_map = true; - map_width_meters_ = parameter.as_int(); + if (parameter.as_int() > 0) { + resize_map = true; + map_width_meters_ = parameter.as_int(); + } else { + RCLCPP_ERROR( + get_logger(), "You try to set width of map to be negative or zero," + " this isn't allowed, please give a positive value."); + result.successful = false; + return result; + } } else if (name == "height") { - resize_map = true; - map_height_meters_ = parameter.as_int(); + if (parameter.as_int() > 0) { + resize_map = true; + map_height_meters_ = parameter.as_int(); + } else { + RCLCPP_ERROR( + get_logger(), "You try to set height of map to be negative or zero," + " this isn't allowed, please give a positive value."); + result.successful = false; + return result; + } } } else if (type == ParameterType::PARAMETER_STRING) { if (name == "footprint") { diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 7a1ec6a9098..cb54ecd856c 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -30,7 +30,18 @@ target_link_libraries(speed_filter_test filters ) +ament_add_gtest(binary_filter_test binary_filter_test.cpp) +target_link_libraries(binary_filter_test + nav2_costmap_2d_core + filters +) + ament_add_gtest(copy_window_test copy_window_test.cpp) target_link_libraries(copy_window_test 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 +) diff --git a/nav2_costmap_2d/test/unit/binary_filter_test.cpp b/nav2_costmap_2d/test/unit/binary_filter_test.cpp new file mode 100644 index 00000000000..31bdfc025bc --- /dev/null +++ b/nav2_costmap_2d/test/unit/binary_filter_test.cpp @@ -0,0 +1,877 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// +// 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. Reserved. + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.h" +#include "nav2_util/occ_grid_values.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "std_msgs/msg/bool.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_msgs/msg/costmap_filter_info.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" +#include "nav2_costmap_2d/costmap_filters/binary_filter.hpp" + +using namespace std::chrono_literals; + +static const char FILTER_NAME[]{"binary_filter"}; +static const char INFO_TOPIC[]{"costmap_filter_info"}; +static const char MASK_TOPIC[]{"mask"}; +static const char BINARY_STATE_TOPIC[]{"binary_state"}; + +static const double NO_TRANSLATION = 0.0; +static const double TRANSLATION_X = 1.0; +static const double TRANSLATION_Y = 1.0; + +static const uint8_t INCORRECT_TYPE = 200; + +class InfoPublisher : public rclcpp::Node +{ +public: + InfoPublisher(uint8_t type, const char * mask_topic, double base, double multiplier) + : Node("costmap_filter_info_pub") + { + publisher_ = this->create_publisher( + INFO_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + msg->type = type; + msg->filter_mask_topic = mask_topic; + msg->base = static_cast(base); + msg->multiplier = static_cast(multiplier); + + publisher_->publish(std::move(msg)); + } + + ~InfoPublisher() + { + publisher_.reset(); + } + +private: + rclcpp::Publisher::SharedPtr publisher_; +}; // InfoPublisher + +class MaskPublisher : public rclcpp::Node +{ +public: + explicit MaskPublisher(const nav_msgs::msg::OccupancyGrid & mask) + : Node("mask_pub") + { + publisher_ = this->create_publisher( + MASK_TOPIC, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + publisher_->publish(mask); + } + + ~MaskPublisher() + { + publisher_.reset(); + } + +private: + rclcpp::Publisher::SharedPtr publisher_; +}; // MaskPublisher + +class BinaryStateSubscriber : public rclcpp::Node +{ +public: + explicit BinaryStateSubscriber(const std::string & binary_state_topic, bool default_state) + : Node("binary_state_sub"), binary_state_updated_(false) + { + subscriber_ = this->create_subscription( + binary_state_topic, rclcpp::QoS(10), + std::bind(&BinaryStateSubscriber::binaryStateCallback, this, std::placeholders::_1)); + + // Initialize with default state + msg_ = std::make_shared(); + msg_->data = default_state; + } + + void binaryStateCallback( + const std_msgs::msg::Bool::SharedPtr msg) + { + msg_ = msg; + binary_state_updated_ = true; + } + + std_msgs::msg::Bool::SharedPtr getBinaryState() + { + return msg_; + } + + inline bool binaryStateUpdated() + { + return binary_state_updated_; + } + + inline void resetBinaryStateIndicator() + { + binary_state_updated_ = false; + } + +private: + rclcpp::Subscription::SharedPtr subscriber_; + std_msgs::msg::Bool::SharedPtr msg_; + bool binary_state_updated_; +}; // BinaryStateSubscriber + +class TestMask : public nav_msgs::msg::OccupancyGrid +{ +public: + TestMask( + unsigned int width, unsigned int height, double resolution, + const std::string & mask_frame) + : width_(width), height_(height) + { + // Fill filter mask info + header.frame_id = mask_frame; + info.resolution = resolution; + info.width = width_; + info.height = height_; + info.origin.position.x = 0.0; + info.origin.position.y = 0.0; + info.origin.position.z = 0.0; + info.origin.orientation.x = 0.0; + info.origin.orientation.y = 0.0; + info.origin.orientation.z = 0.0; + info.origin.orientation.w = 1.0; + + // Fill test mask as follows: + // + // mask (10,11) + // *----------------* + // |91|92|...|99|100| + // |... | + // |... | + // |11|12|13|...| 20| + // | 1| 2| 3|...| 10| + // |-1| 0| 0|...| 0| + // *----------------* + // (0,0) + data.resize(width_ * height_, nav2_util::OCC_GRID_UNKNOWN); + + unsigned int mx, my; + data[0] = nav2_util::OCC_GRID_UNKNOWN; + for (mx = 1; mx < width_; mx++) { + data[mx] = nav2_util::OCC_GRID_FREE; + } + unsigned int it; + for (my = 1; my < height_; my++) { + for (mx = 0; mx < width_; mx++) { + it = mx + my * width_; + data[it] = makeData(mx, my); + } + } + } + + inline int8_t makeData(unsigned int mx, unsigned int my) + { + return mx + (my - 1) * width_ + 1; + } + +private: + const unsigned int width_; + const unsigned int height_; +}; // TestMask + +class TestNode : public ::testing::Test +{ +public: + TestNode() + : default_state_(false) {} + + ~TestNode() {} + +protected: + void createMaps(const std::string & mask_frame); + void publishMaps(uint8_t type, const char * mask_topic, double base, double multiplier); + void rePublishInfo(uint8_t type, const char * mask_topic, double base, double multiplier); + void rePublishMask(); + void setDefaultState(bool default_state); // NOTE: must be called before createBinaryFilter() + bool createBinaryFilter(const std::string & global_frame, double flip_threshold); + void createTFBroadcaster(const std::string & mask_frame, const std::string & global_frame); + void publishTransform(); + + // Test methods + void testFullMask( + double base, double multiplier, double flip_threshold, double tr_x, double tr_y); + void testSimpleMask( + double base, double multiplier, double flip_threshold, double tr_x, double tr_y); + void testOutOfMask(); + void testIncorrectTF(); + void testResetFilter(); + + void resetMaps(); + void reset(); + + std::shared_ptr binary_filter_; + std::shared_ptr master_grid_; + + bool default_state_; + +private: + void waitSome(const std::chrono::nanoseconds & duration); + std_msgs::msg::Bool::SharedPtr getBinaryState(); + std_msgs::msg::Bool::SharedPtr waitBinaryState(); + bool getSign( + unsigned int x, unsigned int y, double base, double multiplier, double flip_threshold); + void verifyBinaryState(bool sign, std_msgs::msg::Bool::SharedPtr state); + + const unsigned int width_ = 10; + const unsigned int height_ = 11; + const double resolution_ = 1.0; + + nav2_util::LifecycleNode::SharedPtr node_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::shared_ptr tf_broadcaster_; + std::unique_ptr transform_; + + std::shared_ptr mask_; + + std::shared_ptr info_publisher_; + std::shared_ptr mask_publisher_; + std::shared_ptr binary_state_subscriber_; +}; + +void TestNode::createMaps(const std::string & mask_frame) +{ + // Make map and mask put as follows: + // master_grid (12,13) + // *----------------* + // | | + // | mask (10,11) | + // | *-------* | + // | |///////| | + // | |///////| | + // | |///////| | + // | *-------* | + // | (0,0) | + // | | + // *----------------* + // (-2,-2) + + // Create master_grid_ + master_grid_ = std::make_shared( + width_ + 4, height_ + 4, resolution_, -2.0, -2.0, nav2_costmap_2d::FREE_SPACE); + + // Create mask_ + mask_ = std::make_shared(width_, height_, resolution_, mask_frame); +} + +void TestNode::publishMaps( + uint8_t type, const char * mask_topic, double base, double multiplier) +{ + info_publisher_ = std::make_shared(type, mask_topic, base, multiplier); + mask_publisher_ = std::make_shared(*mask_); +} + +void TestNode::rePublishInfo( + uint8_t type, const char * mask_topic, double base, double multiplier) +{ + info_publisher_.reset(); + info_publisher_ = std::make_shared(type, mask_topic, base, multiplier); + // Allow both CostmapFilterInfo and filter mask subscribers + // to receive a new message + waitSome(100ms); +} + +void TestNode::rePublishMask() +{ + mask_publisher_.reset(); + mask_publisher_ = std::make_shared(*mask_); + // Allow filter mask subscriber to receive a new message + waitSome(100ms); +} + +void TestNode::waitSome(const std::chrono::nanoseconds & duration) +{ + rclcpp::Time start_time = node_->now(); + while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(duration)) { + rclcpp::spin_some(node_->get_node_base_interface()); + rclcpp::spin_some(binary_state_subscriber_); + std::this_thread::sleep_for(10ms); + } +} + +std_msgs::msg::Bool::SharedPtr TestNode::getBinaryState() +{ + std::this_thread::sleep_for(100ms); + rclcpp::spin_some(binary_state_subscriber_); + return binary_state_subscriber_->getBinaryState(); +} + +std_msgs::msg::Bool::SharedPtr TestNode::waitBinaryState() +{ + const std::chrono::nanoseconds timeout = 500ms; + + rclcpp::Time start_time = node_->now(); + binary_state_subscriber_->resetBinaryStateIndicator(); + while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (binary_state_subscriber_->binaryStateUpdated()) { + binary_state_subscriber_->resetBinaryStateIndicator(); + return binary_state_subscriber_->getBinaryState(); + } + rclcpp::spin_some(binary_state_subscriber_); + std::this_thread::sleep_for(10ms); + } + return nullptr; +} + +void TestNode::setDefaultState(bool default_state) +{ + default_state_ = default_state; +} + +bool TestNode::createBinaryFilter(const std::string & global_frame, double flip_threshold) +{ + node_ = std::make_shared("test_node"); + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + tf_listener_ = std::make_shared(*tf_buffer_); + + nav2_costmap_2d::LayeredCostmap layers(global_frame, false, false); + + node_->declare_parameter( + std::string(FILTER_NAME) + ".transform_tolerance", rclcpp::ParameterValue(0.5)); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".transform_tolerance", 0.5)); + node_->declare_parameter( + std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC)); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", INFO_TOPIC)); + node_->declare_parameter( + std::string(FILTER_NAME) + ".default_state", rclcpp::ParameterValue(default_state_)); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".default_state", default_state_)); + node_->declare_parameter( + std::string(FILTER_NAME) + ".binary_state_topic", rclcpp::ParameterValue(BINARY_STATE_TOPIC)); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".binary_state_topic", BINARY_STATE_TOPIC)); + node_->declare_parameter( + std::string(FILTER_NAME) + ".flip_threshold", rclcpp::ParameterValue(flip_threshold)); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".flip_threshold", flip_threshold)); + + binary_filter_ = std::make_shared(); + binary_filter_->initialize(&layers, FILTER_NAME, tf_buffer_.get(), node_, nullptr); + binary_filter_->initializeFilter(INFO_TOPIC); + + binary_state_subscriber_ = + std::make_shared(BINARY_STATE_TOPIC, default_state_); + + // Wait until mask will be received by BinaryFilter + const std::chrono::nanoseconds timeout = 500ms; + rclcpp::Time start_time = node_->now(); + while (!binary_filter_->isActive()) { + if (node_->now() - start_time > rclcpp::Duration(timeout)) { + return false; + } + rclcpp::spin_some(node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return true; +} + +void TestNode::createTFBroadcaster(const std::string & mask_frame, const std::string & global_frame) +{ + tf_broadcaster_ = std::make_shared(node_); + + transform_ = std::make_unique(); + transform_->header.frame_id = mask_frame; + transform_->child_frame_id = global_frame; + + transform_->header.stamp = node_->now() + rclcpp::Duration(100ms); + transform_->transform.translation.x = TRANSLATION_X; + transform_->transform.translation.y = TRANSLATION_Y; + transform_->transform.translation.z = 0.0; + transform_->transform.rotation.x = 0.0; + transform_->transform.rotation.y = 0.0; + transform_->transform.rotation.z = 0.0; + transform_->transform.rotation.w = 1.0; + + tf_broadcaster_->sendTransform(*transform_); + + // Allow tf_buffer_ to be filled by listener + waitSome(100ms); +} + +void TestNode::publishTransform() +{ + if (tf_broadcaster_) { + transform_->header.stamp = node_->now() + rclcpp::Duration(100ms); + tf_broadcaster_->sendTransform(*transform_); + } +} + +bool TestNode::getSign( + unsigned int x, unsigned int y, double base, double multiplier, double flip_threshold) +{ + const int8_t cost = mask_->makeData(x, y); + return base + cost * multiplier > flip_threshold; +} + +void TestNode::verifyBinaryState(bool sign, std_msgs::msg::Bool::SharedPtr state) +{ + ASSERT_TRUE(state != nullptr); + if (sign) { + EXPECT_FALSE(state->data == default_state_); + } else { + EXPECT_TRUE(state->data == default_state_); + } +} + +void TestNode::testFullMask( + double base, double multiplier, double flip_threshold, double tr_x, double tr_y) +{ + const int min_i = 0; + const int min_j = 0; + const int max_i = width_ + 4; + const int max_j = height_ + 4; + + geometry_msgs::msg::Pose2D pose; + std_msgs::msg::Bool::SharedPtr binary_state; + + unsigned int x, y; + bool prev_sign = false; + bool sign; + + // data = 0 + x = 1; + y = 0; + pose.x = x - tr_x; + pose.y = y - tr_y; + publishTransform(); + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + sign = getSign(x, y, base, multiplier, flip_threshold); + if (sign != prev_sign) { + // Binary filter just flipped + binary_state = waitBinaryState(); + prev_sign = sign; + } else { + // Binary filter state should not be changed + binary_state = getBinaryState(); + } + verifyBinaryState(sign, binary_state); + + // data in range [1..100] (sparsed for testing speed) + for (y = 1; y < height_; y += 2) { + for (x = 0; x < width_; x += 2) { + pose.x = x - tr_x; + pose.y = y - tr_y; + publishTransform(); + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + + sign = getSign(x, y, base, multiplier, flip_threshold); + if (prev_sign != sign) { + // Binary filter just flipped + binary_state = waitBinaryState(); + prev_sign = sign; + } else { + // Binary filter state should not be changed + binary_state = getBinaryState(); + } + verifyBinaryState(sign, binary_state); + } + } + + // data = -1 (unknown) + bool prev_state = binary_state->data; + pose.x = -tr_x; + pose.y = -tr_y; + publishTransform(); + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + binary_state = getBinaryState(); + ASSERT_TRUE(binary_state != nullptr); + ASSERT_EQ(binary_state->data, prev_state); // Binary state won't be updated +} + +void TestNode::testSimpleMask( + double base, double multiplier, double flip_threshold, double tr_x, double tr_y) +{ + const int min_i = 0; + const int min_j = 0; + const int max_i = width_ + 4; + const int max_j = height_ + 4; + + geometry_msgs::msg::Pose2D pose; + std_msgs::msg::Bool::SharedPtr binary_state; + + unsigned int x, y; + bool prev_sign = false; + bool sign; + + // data = 0 + x = 1; + y = 0; + pose.x = x - tr_x; + pose.y = y - tr_y; + publishTransform(); + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + sign = getSign(x, y, base, multiplier, flip_threshold); + if (sign != prev_sign) { + // Binary filter just flipped + binary_state = waitBinaryState(); + prev_sign = sign; + } else { + // Binary filter state should not be changed + binary_state = getBinaryState(); + } + verifyBinaryState(sign, binary_state); + + // data = + x = width_ / 2 - 1; + y = height_ / 2 - 1; + pose.x = x - tr_x; + pose.y = y - tr_y; + publishTransform(); + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + + sign = getSign(x, y, base, multiplier, flip_threshold); + if (prev_sign != sign) { + // Binary filter just flipped + binary_state = waitBinaryState(); + prev_sign = sign; + } else { + // Binary filter state should not be changed + binary_state = getBinaryState(); + } + verifyBinaryState(sign, binary_state); + + // data = 100 + x = width_ - 1; + y = height_ - 1; + pose.x = x - tr_x; + pose.y = y - tr_y; + publishTransform(); + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + + sign = getSign(x, y, base, multiplier, flip_threshold); + if (prev_sign != sign) { + // Binary filter just flipped + binary_state = waitBinaryState(); + prev_sign = sign; + } else { + // Binary filter state should not be changed + binary_state = getBinaryState(); + } + verifyBinaryState(sign, binary_state); + + // data = -1 (unknown) + bool prev_state = binary_state->data; + pose.x = -tr_x; + pose.y = -tr_y; + publishTransform(); + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + binary_state = getBinaryState(); + ASSERT_TRUE(binary_state != nullptr); + ASSERT_EQ(binary_state->data, prev_state); // Binary state won't be updated +} + +void TestNode::testOutOfMask() +{ + // base, multiplier and flip_threshold should have values as below for this test + const double base = 0.0; + const double multiplier = 1.0; + const double flip_threshold = 10.0; + + const int min_i = 0; + const int min_j = 0; + const int max_i = width_ + 4; + const int max_j = height_ + 4; + + geometry_msgs::msg::Pose2D pose; + std_msgs::msg::Bool::SharedPtr binary_state; + + // data = + pose.x = width_ / 2 - 1; + pose.y = height_ / 2 - 1; + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + binary_state = waitBinaryState(); + verifyBinaryState(getSign(pose.x, pose.y, base, multiplier, flip_threshold), binary_state); + + // Then go to out of mask bounds and ensure that binary state is set back to default + pose.x = -2.0; + pose.y = -2.0; + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + binary_state = getBinaryState(); + ASSERT_TRUE(binary_state != nullptr); + ASSERT_EQ(binary_state->data, default_state_); + + pose.x = width_ + 1.0; + pose.y = height_ + 1.0; + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + binary_state = getBinaryState(); + ASSERT_TRUE(binary_state != nullptr); + ASSERT_EQ(binary_state->data, default_state_); +} + +void TestNode::testIncorrectTF() +{ + const int min_i = 0; + const int min_j = 0; + const int max_i = width_ + 4; + const int max_j = height_ + 4; + + geometry_msgs::msg::Pose2D pose; + std_msgs::msg::Bool::SharedPtr binary_state; + + // data = + pose.x = width_ / 2 - 1; + pose.y = height_ / 2 - 1; + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + binary_state = waitBinaryState(); + ASSERT_TRUE(binary_state == nullptr); +} + +void TestNode::testResetFilter() +{ + // base, multiplier and flip_threshold should have values as below for this test + const double base = 0.0; + const double multiplier = 1.0; + const double flip_threshold = 10.0; + + const int min_i = 0; + const int min_j = 0; + const int max_i = width_ + 4; + const int max_j = height_ + 4; + + geometry_msgs::msg::Pose2D pose; + std_msgs::msg::Bool::SharedPtr binary_state; + + // Switch-on binary filter + pose.x = width_ / 2 - 1; + pose.y = height_ / 2 - 1; + publishTransform(); + binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose); + binary_state = waitBinaryState(); + verifyBinaryState(getSign(pose.x, pose.y, base, multiplier, flip_threshold), binary_state); + + // Reset binary filter and check its state was resetted to default + binary_filter_->resetFilter(); + binary_state = waitBinaryState(); + ASSERT_TRUE(binary_state != nullptr); + ASSERT_EQ(binary_state->data, default_state_); +} + +void TestNode::resetMaps() +{ + mask_.reset(); + master_grid_.reset(); +} + +void TestNode::reset() +{ + resetMaps(); + info_publisher_.reset(); + mask_publisher_.reset(); + binary_state_subscriber_.reset(); + binary_filter_.reset(); + node_.reset(); + tf_listener_.reset(); + tf_broadcaster_.reset(); + tf_buffer_.reset(); +} + +TEST_F(TestNode, testBinaryState) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); + ASSERT_TRUE(createBinaryFilter("map", 10.0)); + + // Test BinaryFilter + testSimpleMask(0.0, 1.0, 10.0, NO_TRANSLATION, NO_TRANSLATION); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testBinaryStateScaled) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 100.0, -1.0); + ASSERT_TRUE(createBinaryFilter("map", 35.0)); + + // Test BinaryFilter + testFullMask(100.0, -1.0, 35.0, NO_TRANSLATION, NO_TRANSLATION); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testInvertedBinaryState) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); + setDefaultState(true); + ASSERT_TRUE(createBinaryFilter("map", 10.0)); + + // Test BinaryFilter + testSimpleMask(0.0, 1.0, 10.0, NO_TRANSLATION, NO_TRANSLATION); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testOutOfBounds) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); + ASSERT_TRUE(createBinaryFilter("map", 10.0)); + + // Test BinaryFilter + testOutOfMask(); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testInfoRePublish) +{ + // Initilize test system + createMaps("map"); + // Publish Info with incorrect dummy mask topic + publishMaps(nav2_costmap_2d::BINARY_FILTER, "dummy_topic", 0.0, 1.0); + ASSERT_FALSE(createBinaryFilter("map", 10.0)); + + // Re-publish filter info with correct mask topic + // and ensure that everything works fine + rePublishInfo(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); + + // Test BinaryFilter + testSimpleMask(0.0, 1.0, 10.0, NO_TRANSLATION, NO_TRANSLATION); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testMaskRePublish) +{ + // Create mask in incorrect frame + createMaps("dummy"); + publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); + EXPECT_TRUE(createBinaryFilter("map", 10.0)); + + // Create mask in correct frame + resetMaps(); + createMaps("map"); + // Re-publish correct filter mask and ensure that everything works fine + rePublishMask(); + + // Test BinaryFilter + testSimpleMask(0.0, 1.0, 10.0, NO_TRANSLATION, NO_TRANSLATION); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testIncorrectFilterType) +{ + // Initilize test system + createMaps("map"); + publishMaps(INCORRECT_TYPE, MASK_TOPIC, 0.0, 1.0); + ASSERT_FALSE(createBinaryFilter("map", 10.0)); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testDifferentFrame) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); + ASSERT_TRUE(createBinaryFilter("odom", 10.0)); + createTFBroadcaster("map", "odom"); + + // Test BinaryFilter + testSimpleMask(0.0, 1.0, 10.0, TRANSLATION_X, TRANSLATION_Y); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testIncorrectFrame) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); + ASSERT_TRUE(createBinaryFilter("odom", 10.0)); + // map->odom TF does not exit + + // Test BinaryFilter with incorrect TF chain + testIncorrectTF(); + + // Clean-up + binary_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testResetState) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::BINARY_FILTER, MASK_TOPIC, 0.0, 1.0); + ASSERT_TRUE(createBinaryFilter("map", 10.0)); + + testResetFilter(); + + // Clean-up + // do not need to resetFilter(): this was already done in testResetFilter() + reset(); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +} diff --git a/nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp b/nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp new file mode 100644 index 00000000000..2047acbd13f --- /dev/null +++ b/nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp @@ -0,0 +1,150 @@ +// Copyright (c) 2022 Samsung R&D Institute Russia +// +// 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. Reserved. + +#include + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" + +#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" +#include "std_srvs/srv/set_bool.hpp" + +static const char FILTER_NAME[]{"costmap_filter"}; + +class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter +{ +public: + // Dummy implementations of virtual methods + void initializeFilter( + const std::string &) {} + + void process( + nav2_costmap_2d::Costmap2D &, + int, int, int, int, + const geometry_msgs::msg::Pose2D &) {} + + void resetFilter() {} + + // Actual testing methods + void setName(const std::string & name) + { + name_ = name; + } + + void setNode(const nav2_util::LifecycleNode::WeakPtr & node) + { + node_ = node; + } + + bool getEnabled() + { + return enabled_; + } +}; + +class TestNode : public ::testing::Test +{ +public: + TestNode() + { + // Create new LifecycleNode + node_ = std::make_shared("test_node"); + + // Create new CostmapFilter + costmap_filter_ = std::make_shared(); + costmap_filter_->setNode(node_); + costmap_filter_->setName(FILTER_NAME); + + // Set CostmapFilter ROS-parameters + node_->declare_parameter( + std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue("filter_info")); + node_->set_parameter( + rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", "filter_info")); + } + + ~TestNode() + { + costmap_filter_.reset(); + node_.reset(); + } + + template + typename T::Response::SharedPtr send_request( + nav2_util::LifecycleNode::SharedPtr node, + typename rclcpp::Client::SharedPtr client, + typename T::Request::SharedPtr request) + { + auto result = client->async_send_request(request); + + // Wait for the result + if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) { + return result.get(); + } else { + return nullptr; + } + } + +protected: + nav2_util::LifecycleNode::SharedPtr node_; + std::shared_ptr costmap_filter_; +}; + +TEST_F(TestNode, testEnableService) +{ + costmap_filter_->onInitialize(); + + RCLCPP_INFO(node_->get_logger(), "Testing enabling service"); + auto req = std::make_shared(); + auto client = node_->create_client( + std::string(FILTER_NAME) + "/toggle_filter"); + + RCLCPP_INFO(node_->get_logger(), "Waiting for enabling service"); + ASSERT_TRUE(client->wait_for_service()); + + // Set costmap filter enabled + req->data = true; + auto resp = send_request(node_, client, req); + + ASSERT_NE(resp, nullptr); + ASSERT_TRUE(resp->success); + ASSERT_EQ(resp->message, "Enabled"); + ASSERT_TRUE(costmap_filter_->getEnabled()); + + // Set costmap filter disabled + req->data = false; + resp = send_request(node_, client, req); + + ASSERT_NE(resp, nullptr); + ASSERT_TRUE(resp->success); + ASSERT_EQ(resp->message, "Disabled"); + ASSERT_FALSE(costmap_filter_->getEnabled()); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +} diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 82fcdd6eaad..4216e878d63 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.2 + 1.1.3 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 d257c870004..61c9ad2c4aa 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.2 + 1.1.3 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 891a51e4dd0..01870d51d2d 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.2 + 1.1.3 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 5894e431256..b96cd4b6257 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.2 + 1.1.3 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 63fcf35c9ea..aab31ad8878 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.2 + 1.1.3 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 17cefffe2b5..e90c4c4a9cc 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.2 + 1.1.3 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 6cc4dc0fc1f..cabf4c19b33 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.2 + 1.1.3 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 242c5545442..76d7814f13a 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.2 + 1.1.3 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 fabcc9d56bb..20234d5f7ae 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.1.2 + 1.1.3 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/README.md b/nav2_map_server/README.md index c0e7a2cf6a8..94a6bd66af0 100644 --- a/nav2_map_server/README.md +++ b/nav2_map_server/README.md @@ -18,7 +18,7 @@ Currently map server divides into tree parts: - `map_io` library `map_server` is responsible for loading the map from a file through command-line interface -or by using serice requests. +or by using service requests. `map_saver` saves the map into a file. Like `map_server`, it has an ability to save the map from command-line or by calling a service. @@ -27,7 +27,7 @@ command-line or by calling a service. in order to allow easily save/load map from external code just by calling necessary function. This library is also used by `map_loader` and `map_saver` to work. Currently it contains OccupancyGrid saving/loading functions moved from the rest part of map server code. -It is designed to be replaceble for a new IO library (e.g. for library with new map encoding +It is designed to be replaceable for a new IO library (e.g. for library with new map encoding method or any other library supporting costmaps, multifloor maps, etc...). ### CLI-usage diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index db9a992af3a..864e627452a 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.1.2 + 1.1.3 Refactored map server for ROS2 Navigation diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index ce7376cebb1..2be81db0a74 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" + "action/AssistedTeleop.action" "action/BackUp.action" "action/ComputePathToPose.action" "action/ComputePathThroughPoses.action" diff --git a/nav2_msgs/action/AssistedTeleop.action b/nav2_msgs/action/AssistedTeleop.action new file mode 100644 index 00000000000..ba830ebc121 --- /dev/null +++ b/nav2_msgs/action/AssistedTeleop.action @@ -0,0 +1,8 @@ +#goal definition +builtin_interfaces/Duration time_allowance +--- +#result definition +builtin_interfaces/Duration total_elapsed_time +--- +#feedback +builtin_interfaces/Duration current_teleop_duration diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 76290187c34..07ead113892 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.1.2 + 1.1.3 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 25349c10966..7cea1555cc7 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.1.2 + 1.1.3 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 8a8b4e875f3..529a9e37cc1 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.1.2 + 1.1.3 TODO Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 379e2a56f27..dcdedcdc0e6 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -62,7 +62,8 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` | | `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal | | `approach_velocity_scaling_dist` | Integrated distance from end of transformed path at which to start applying velocity scaling. This defaults to the forward extent of the costmap minus one costmap cell length. | -| `max_allowed_time_to_collision_up_to_carrot` | The time to project a velocity command to check for collisions, limited to maximum distance of lookahead distance selected | +| `use_collision_detection` | Whether to enable collision detection. | +| `max_allowed_time_to_collision_up_to_carrot` | The time to project a velocity command to check for collisions when `use_collision_detection` is `true`. It is limited to maximum distance of lookahead distance selected. | | `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature | | `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles | | `cost_scaling_dist` | The minimum distance from an obstacle to trigger the scaling of linear velocity, if `use_cost_regulated_linear_velocity_scaling` is enabled. The value set should be smaller or equal to the `inflation_radius` set in the inflation layer of costmap, since inflation is used to compute the distance from obstacles | @@ -112,6 +113,7 @@ controller_server: min_approach_linear_velocity: 0.05 use_approach_linear_velocity_scaling: true approach_velocity_scaling_dist: 1.0 + use_collision_detection: true max_allowed_time_to_collision_up_to_carrot: 1.0 use_regulated_linear_velocity_scaling: true use_cost_regulated_linear_velocity_scaling: false diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 01a41df0ac1..7c244d93932 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -292,6 +292,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller double approach_velocity_scaling_dist_; double control_duration_; double max_allowed_time_to_collision_up_to_carrot_; + bool use_collision_detection_; bool use_regulated_linear_velocity_scaling_; bool use_cost_regulated_linear_velocity_scaling_; double cost_scaling_dist_; diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 2fdfc77e08f..80a036d89bf 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.2 + 1.1.3 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index d8721c646f7..4b52dd36166 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -85,6 +85,9 @@ void RegulatedPurePursuitController::configure( declare_parameter_if_not_declared( node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_collision_detection", + rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( @@ -142,6 +145,9 @@ void RegulatedPurePursuitController::configure( node->get_parameter( plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", max_allowed_time_to_collision_up_to_carrot_); + node->get_parameter( + plugin_name_ + ".use_collision_detection", + use_collision_detection_); node->get_parameter( plugin_name_ + ".use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_); @@ -346,7 +352,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Collision checking on this velocity heading const double & carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); - if (isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) { + if (use_collision_detection_ && isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) { throw nav2_core::PlannerException("RegulatedPurePursuitController detected collision ahead!"); } @@ -802,7 +808,7 @@ bool RegulatedPurePursuitController::transformPose( double RegulatedPurePursuitController::getCostmapMaxExtent() const { const double max_costmap_dim_meters = std::max( - costmap_->getSizeInMetersX(), costmap_->getSizeInMetersX()); + costmap_->getSizeInMetersX(), costmap_->getSizeInMetersY()); return max_costmap_dim_meters / 2.0; } diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 74605847795..2bb4f15ef76 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.2 + 1.1.3 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 96b48014723..3f79200a22f 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.1.2 + 1.1.3 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index c4f398f77d3..7af387eb02f 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -92,7 +92,7 @@ This will bring up the robot in the AWS Warehouse in a reasonable position, laun ### Manually -The main benefit of this is to be able to launch alternative robot models or different navigation configurations than the default for a specific technology demonstation. As long as Nav2 and the simulation (or physical robot) is running, the simple python commander examples / demos don't care what the robot is or how it got there. Since the examples / demos do contain hard-programmed item locations or routes, you should still utilize the AWS Warehouse. Obviously these are easy to update if you wish to adapt these examples / demos to another environment. +The main benefit of this is to be able to launch alternative robot models or different navigation configurations than the default for a specific technology demonstration. As long as Nav2 and the simulation (or physical robot) is running, the simple python commander examples / demos don't care what the robot is or how it got there. Since the examples / demos do contain hard-programmed item locations or routes, you should still utilize the AWS Warehouse. Obviously these are easy to update if you wish to adapt these examples / demos to another environment. ``` bash # Terminal 1: launch your robot navigation and simulation (or physical robot). For example diff --git a/nav2_simple_commander/launch/assisted_teleop_example_launch.py b/nav2_simple_commander/launch/assisted_teleop_example_launch.py new file mode 100644 index 00000000000..59bd6ea6290 --- /dev/null +++ b/nav2_simple_commander/launch/assisted_teleop_example_launch.py @@ -0,0 +1,78 @@ +# Copyright (c) 2021 Samsung Research America +# Copyright (c) 2022 Joshua Wallace +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_assisted_teleop', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py new file mode 100644 index 00000000000..82b603dd676 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py @@ -0,0 +1,192 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# Copyright 2022 Stevedan Ogochukwu Omodolor +# Copyright 2022 Jaehun Jackson Kim +# +# 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. + +""" +This is a Python3 API for costmap 2d messages from the stack. + +It provides the basic conversion, get/set, +and handling semantics found in the costmap 2d C++ API. +""" + +import numpy as np + + +class PyCostmap2D: + """ + PyCostmap2D. + + Costmap Python3 API for OccupancyGrids to populate from published messages + """ + + def __init__(self, occupancy_map): + """ + Initialize costmap2D. + + Args: + ---- + occupancy_map (OccupancyGrid): 2D OccupancyGrid Map + + """ + self.size_x = occupancy_map.info.width + self.size_y = occupancy_map.info.height + self.resolution = occupancy_map.info.resolution + self.origin_x = occupancy_map.info.origin.position.x + self.origin_y = occupancy_map.info.origin.position.y + self.global_frame_id = occupancy_map.header.frame_id + self.costmap_timestamp = occupancy_map.header.stamp + # Extract costmap + self.costmap = np.array(occupancy_map.data, dtype=np.uint8) + + def getSizeInCellsX(self): + """Get map width in cells.""" + return self.size_x + + def getSizeInCellsY(self): + """Get map height in cells.""" + return self.size_y + + def getSizeInMetersX(self): + """Get x axis map size in meters.""" + return (self.size_x - 1 + 0.5) * self.resolution + + def getSizeInMetersY(self): + """Get y axis map size in meters.""" + return (self.size_y - 1 + 0.5) * self.resolution + + def getOriginX(self): + """Get the origin x axis of the map [m].""" + return self.origin_x + + def getOriginY(self): + """Get the origin y axis of the map [m].""" + return self.origin_y + + def getResolution(self): + """Get map resolution [m/cell].""" + return self.resolution + + def getGlobalFrameID(self): + """Get global frame_id.""" + return self.global_frame_id + + def getCostmapTimestamp(self): + """Get costmap timestamp.""" + return self.costmap_timestamp + + def getCostXY(self, mx: int, my: int) -> np.uint8: + """ + Get the cost of a cell in the costmap using map coordinate XY. + + Args + ---- + mx (int): map coordinate X to get cost + my (int): map coordinate Y to get cost + + Returns + ------- + np.uint8: cost of a cell + + """ + return self.costmap[self.getIndex(mx, my)] + + def getCostIdx(self, index: int) -> np.uint8: + """ + Get the cost of a cell in the costmap using Index. + + Args + ---- + index (int): index of cell to get cost + + Returns + ------- + np.uint8: cost of a cell + + """ + return self.costmap[index] + + def setCost(self, mx: int, my: int, cost: np.uint8) -> None: + """ + Set the cost of a cell in the costmap using map coordinate XY. + + Args + ---- + mx (int): map coordinate X to get cost + my (int): map coordinate Y to get cost + cost (np.uint8): The cost to set the cell + + Returns + ------- + None + + """ + self.costmap[self.getIndex(mx, my)] = cost + + def mapToWorld(self, mx: int, my: int) -> tuple[float, float]: + """ + Get the world coordinate XY using map coordinate XY. + + Args + ---- + mx (int): map coordinate X to get world coordinate + my (int): map coordinate Y to get world coordinate + + Returns + ------- + tuple of float: wx, wy + wx (float) [m]: world coordinate X + wy (float) [m]: world coordinate Y + + """ + wx = self.origin_x + (mx + 0.5) * self.resolution + wy = self.origin_y + (my + 0.5) * self.resolution + return (wx, wy) + + def worldToMap(self, wx: float, wy: float) -> tuple[int, int]: + """ + Get the map coordinate XY using world coordinate XY. + + Args + ---- + wx (float) [m]: world coordinate X to get map coordinate + wy (float) [m]: world coordinate Y to get map coordinate + + Returns + ------- + tuple of int: mx, my + mx (int): map coordinate X + my (int): map coordinate Y + + """ + mx = int((wx - self.origin_x) // self.resolution) + my = int((wy - self.origin_y) // self.resolution) + return (mx, my) + + def getIndex(self, mx: int, my: int) -> int: + """ + Get the index of the cell using map coordinate XY. + + Args + ---- + mx (int): map coordinate X to get Index + my (int): map coordinate Y to get Index + + Returns + ------- + int: The index of the cell + + """ + return my * self.size_x + mx diff --git a/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py b/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py new file mode 100644 index 00000000000..564342e5417 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py @@ -0,0 +1,56 @@ +#! /usr/bin/env python3 +# Copyright 2022 Joshua Wallace +# +# 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. + +from time import sleep + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator +import rclpy + +""" +Basic navigation demo to go to pose. +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Wait for navigation to fully activate, since autostarting nav2 + navigator.waitUntilNav2Active() + + navigator.assistedTeleop(time_allowance=20) + while not navigator.isTaskComplete(): + # Publish twist commands to be filtered by the assisted teleop action + sleep(0.2) + pass + + navigator.lifecycleShutdown() + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/line_iterator.py b/nav2_simple_commander/nav2_simple_commander/line_iterator.py new file mode 100644 index 00000000000..db19d4180c0 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/line_iterator.py @@ -0,0 +1,177 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# Copyright 2022 Afif Swaidan +# +# 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. + +""" +This is a Python3 API for a line iterator. + +It provides the ability to iterate +through the points of a line. +""" + +from cmath import sqrt + + +class LineIterator(): + """ + LineIterator. + + LineIterator Python3 API for iterating along the points of a given line + """ + + def __init__(self, x0, y0, x1, y1, step_size=1.0): + """ + Initialize the LineIterator. + + Args + ---- + x0 (float): Abscissa of the initial point + y0 (float): Ordinate of the initial point + x1 (float): Abscissa of the final point + y1 (float): Ordinate of the final point + step_size (float): Optional, Increments' resolution, defaults to 1 + + Raises + ------ + TypeError: When one (or more) of the inputs is not a number + ValueError: When step_size is not a positive number + + """ + if type(x0) not in [int, float]: + raise TypeError("x0 must be a number (int or float)") + + if type(y0) not in [int, float]: + raise TypeError("y0 must be a number (int or float)") + + if type(x1) not in [int, float]: + raise TypeError("x1 must be a number (int or float)") + + if type(y1) not in [int, float]: + raise TypeError("y1 must be a number (int or float)") + + if type(step_size) not in [int, float]: + raise TypeError("step_size must be a number (int or float)") + + if step_size <= 0: + raise ValueError("step_size must be a positive number") + + self.x0_ = x0 + self.y0_ = y0 + self.x1_ = x1 + self.y1_ = y1 + self.x_ = x0 + self.y_ = y0 + self.step_size_ = step_size + + if x1 != x0 and y1 != y0: + self.valid_ = True + self.m_ = (y1-y0)/(x1-x0) + self.b_ = y1 - (self.m_*x1) + elif x1 == x0 and y1 != y0: + self.valid_ = True + elif y1 == y1 and x1 != x0: + self.valid_ = True + self.m_ = (y1-y0)/(x1-x0) + self.b_ = y1 - (self.m_*x1) + else: + self.valid_ = False + raise ValueError( + "Line has zero length (All 4 points have same coordinates)") + + def isValid(self): + """Check if line is valid.""" + return self.valid_ + + def advance(self): + """Advance to the next point in the line.""" + if self.x1_ > self.x0_: + if self.x_ < self.x1_: + self.x_ = round(self.clamp( + self.x_ + self.step_size_, self.x0_, self.x1_), 5) + self.y_ = round(self.m_ * self.x_ + self.b_, 5) + else: + self.valid_ = False + elif self.x1_ < self.x0_: + if self.x_ > self.x1_: + self.x_ = round(self.clamp( + self.x_ - self.step_size_, self.x1_, self.x0_), 5) + self.y_ = round(self.m_ * self.x_ + self.b_, 5) + else: + self.valid_ = False + else: + if self.y1_ > self.y0_: + if self.y_ < self.y1_: + self.y_ = round(self.clamp( + self.y_ + self.step_size_, self.y0_, self.y1_), 5) + else: + self.valid_ = False + elif self.y1_ < self.y0_: + if self.y_ > self.y1_: + self.y_ = round(self.clamp( + self.y_ - self.step_size_, self.y1_, self.y0_), 5) + else: + self.valid_ = False + else: + self.valid_ = False + + def getX(self): + """Get the abscissa of the current point.""" + return self.x_ + + def getY(self): + """Get the ordinate of the current point.""" + return self.y_ + + def getX0(self): + """Get the abscissa of the initial point.""" + return self.x0_ + + def getY0(self): + """Get the ordinate of the intial point.""" + return self.y0_ + + def getX1(self): + """Get the abscissa of the final point.""" + return self.x1_ + + def getY1(self): + """Get the ordinate of the final point.""" + return self.y1_ + + def get_line_length(self): + """Get the length of the line.""" + return sqrt(pow(self.x1_ - self.x0_, 2) + pow(self.y1_ - self.y0_, 2)) + + def clamp(self, n, min_n, max_n): + """ + Clamp n to be between min_n and max_n. + + Args + ---- + n (float): input value + min_n (float): minimum value + max_n (float): maximum value + + Returns + ------- + n (float): input value clamped between given min and max + + """ + if n < min_n: + return min_n + elif n > max_n: + return max_n + else: + return n diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 1026f8f75df..a844498ab83 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -23,7 +23,7 @@ from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import PoseWithCovarianceStamped from lifecycle_msgs.srv import GetState -from nav2_msgs.action import BackUp, Spin +from nav2_msgs.action import AssistedTeleop, BackUp, Spin from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose from nav2_msgs.action import FollowPath, FollowWaypoints, NavigateThroughPoses, NavigateToPose from nav2_msgs.action import SmoothPath @@ -75,6 +75,7 @@ def __init__(self): self.smoother_client = ActionClient(self, SmoothPath, 'smooth_path') self.spin_client = ActionClient(self, Spin, 'spin') self.backup_client = ActionClient(self, BackUp, 'backup') + self.assisted_teleop_client = ActionClient(self, AssistedTeleop, 'assisted_teleop') self.localization_pose_sub = self.create_subscription(PoseWithCovarianceStamped, 'amcl_pose', self._amclPoseCallback, @@ -222,6 +223,26 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): self.result_future = self.goal_handle.get_result_async() return True + def assistedTeleop(self, time_allowance=30): + self.debug("Wainting for 'assisted_teleop' action server") + while not self.assisted_teleop_client.wait_for_server(timeout_sec=1.0): + self.info("'assisted_teleop' action server not available, waiting...") + goal_msg = AssistedTeleop.Goal() + goal_msg.time_allowance = Duration(sec=time_allowance) + + self.info("Running 'assisted_teleop'....") + send_goal_future = \ + self.assisted_teleop_client.send_goal_async(goal_msg, self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Assisted Teleop request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + def followPath(self, path, controller_id='', goal_checker_id=''): """Send a `FollowPath` action request.""" self.debug("Waiting for 'FollowPath' action server") @@ -296,8 +317,12 @@ def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'): self.info('Nav2 is ready for use!') return - def getPath(self, start, goal, planner_id='', use_start=False): - """Send a `ComputePathToPose` action request.""" + def _getPathImpl(self, start, goal, planner_id='', use_start=False): + """ + Send a `ComputePathToPose` action request. + + Internal implementation to get the full result, not just the path. + """ self.debug("Waiting for 'ComputePathToPose' action server") while not self.compute_path_to_pose_client.wait_for_server(timeout_sec=1.0): self.info("'ComputePathToPose' action server not available, waiting...") @@ -324,7 +349,15 @@ def getPath(self, start, goal, planner_id='', use_start=False): self.warn(f'Getting path failed with status code: {self.status}') return None - return self.result_future.result().result.path + return self.result_future.result().result + + def getPath(self, start, goal, planner_id='', use_start=False): + """Send a `ComputePathToPose` action request.""" + rtn = self._getPathImpl(start, goal, planner_id='', use_start=False) + if not rtn: + return None + else: + return rtn.path def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): """Send a `ComputePathThroughPoses` action request.""" @@ -356,8 +389,12 @@ def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): return self.result_future.result().result.path - def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision=False): - """Send a `SmoothPath` action request.""" + def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_collision=False): + """ + Send a `SmoothPath` action request. + + Internal implementation to get the full result, not just the path. + """ self.debug("Waiting for 'SmoothPath' action server") while not self.smoother_client.wait_for_server(timeout_sec=1.0): self.info("'SmoothPath' action server not available, waiting...") @@ -384,7 +421,16 @@ def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision self.warn(f'Getting path failed with status code: {self.status}') return None - return self.result_future.result().result.path + return self.result_future.result().result + + def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision=False): + """Send a `SmoothPath` action request.""" + rtn = self._smoothPathImpl( + self, path, smoother_id='', max_duration=2.0, check_for_collision=False) + if not rtn: + return None + else: + return rtn.path def changeMap(self, map_filepath): """Change the current static map in the map server.""" diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index 2e03332efb5..4452af008a9 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.1.2 + 1.1.3 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_simple_commander/pytest.ini b/nav2_simple_commander/pytest.ini new file mode 100644 index 00000000000..50d6d012576 --- /dev/null +++ b/nav2_simple_commander/pytest.ini @@ -0,0 +1,2 @@ +[pytest] +junit_family=xunit2 \ No newline at end of file diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py index ab19a26e473..c7b12a3fcfa 100644 --- a/nav2_simple_commander/setup.py +++ b/nav2_simple_commander/setup.py @@ -33,6 +33,7 @@ 'demo_inspection = nav2_simple_commander.demo_inspection:main', 'demo_security = nav2_simple_commander.demo_security:main', 'demo_recoveries = nav2_simple_commander.demo_recoveries:main', + 'example_assisted_teleop = nav2_simple_commander.example_assisted_teleop:main', ], }, ) diff --git a/nav2_simple_commander/test/test_line_iterator.py b/nav2_simple_commander/test/test_line_iterator.py new file mode 100644 index 00000000000..4c5e97420a6 --- /dev/null +++ b/nav2_simple_commander/test/test_line_iterator.py @@ -0,0 +1,107 @@ +# Copyright 2022 Afif Swaidan +# +# 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. + +import unittest +from cmath import sqrt +from nav2_simple_commander.line_iterator import LineIterator + + +class TestLineIterator(unittest.TestCase): + + def test_type_error(self): + # Test if a type error raised when passing invalid arguements types + self.assertRaises(TypeError, LineIterator, 0, 0, '10', 10, '1') + + def test_value_error(self): + # Test if a value error raised when passing negative or zero step_size + self.assertRaises(ValueError, LineIterator, 0, 0, 10, 10, -2) + # Test if a value error raised when passing zero length line + self.assertRaises(ValueError, LineIterator, 2, 2, 2, 2, 1) + + def test_get_xy(self): + # Test if the initial and final coordinates are returned correctly + lt = LineIterator(0, 0, 5, 5, 1) + self.assertEqual(lt.getX0(), 0) + self.assertEqual(lt.getY0(), 0) + self.assertEqual(lt.getX1(), 5) + self.assertEqual(lt.getY1(), 5) + + def test_line_length(self): + # Test if the line length is calculated correctly + lt = LineIterator(0, 0, 5, 5, 1) + self.assertEqual(lt.get_line_length(), sqrt(pow(5, 2) + pow(5, 2))) + + def test_straight_line(self): + # Test if the calculations are correct for y = x + lt = LineIterator(0, 0, 5, 5, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0() + i) + self.assertEqual(lt.getY(), lt.getY0() + i) + lt.advance() + i += 1 + + # Test if the calculations are correct for y = 2x (positive slope) + lt = LineIterator(0, 0, 5, 10, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0() + i) + self.assertEqual(lt.getY(), lt.getY0() + (i*2)) + lt.advance() + i += 1 + + # Test if the calculations are correct for y = -2x (negative slope) + lt = LineIterator(0, 0, 5, -10, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0() + i) + self.assertEqual(lt.getY(), lt.getY0() + (-i*2)) + lt.advance() + i += 1 + + def test_hor_line(self): + # Test if the calculations are correct for y = 0x+b (horizontal line) + lt = LineIterator(0, 10, 5, 10, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0() + i) + self.assertEqual(lt.getY(), lt.getY0()) + lt.advance() + i += 1 + + def test_ver_line(self): + # Test if the calculations are correct for x = n (vertical line) + lt = LineIterator(5, 0, 5, 10, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0()) + self.assertEqual(lt.getY(), lt.getY0() + i) + lt.advance() + i += 1 + + def test_clamp(self): + # Test if the increments are clamped to avoid crossing the final points + # when step_size is large with respect to line length + lt = LineIterator(0, 0, 5, 5, 10) + self.assertEqual(lt.getX(), 0) + self.assertEqual(lt.getY(), 0) + lt.advance() + while lt.isValid(): + self.assertEqual(lt.getX(), 5) + self.assertEqual(lt.getY(), 5) + lt.advance() + + +if __name__ == '__main__': + unittest.main() diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index c21e856da8e..79a77ce0823 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -152,6 +152,7 @@ install(DIRECTORY lattice_primitives/sample_primitives DESTINATION share/${PROJE if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + set(AMENT_LINT_AUTO_FILE_EXCLUDE include/nav2_smac_planner/thirdparty/robin_hood.h) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) add_subdirectory(test) diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index be522d7ca4f..bb404afc3d3 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -37,7 +37,7 @@ The `SmacPlannerHybrid` is designed to work with: The `SmacPlannerLattice` is designed to work with: - Arbitrary shaped, non-circular robots requiring kinematically feasible planning with SE2 collision checking using the full capabilities of the drivetrain -- Flexibility to use other robot model types or with provided non-circular differental, ackermann, and omni support +- Flexibility to use other robot model types or with provided non-circular differential, ackermann, and omni support The `SmacPlanner2D` is designed to work with: - Circular, differential or omnidirectional robots @@ -104,12 +104,12 @@ planner_server: GridBased: plugin: "nav2_smac_planner/SmacPlannerHybrid" - tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters, for 2D node + tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters downsample_costmap: false # whether or not to downsample the map downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) allow_unknown: false # allow traveling in unknown space max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance, 2D only + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. motion_model_for_search: "DUBIN" # For Hybrid Dubin, Redds-Shepp cost_travel_multiplier: 2.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 2af454bb53e..4b4fe551112 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -26,6 +26,7 @@ #include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_smac_planner/thirdparty/robin_hood.h" #include "nav2_smac_planner/analytic_expansion.hpp" #include "nav2_smac_planner/node_2d.hpp" #include "nav2_smac_planner/node_hybrid.hpp" @@ -46,7 +47,7 @@ class AStarAlgorithm { public: typedef NodeT * NodePtr; - typedef std::unordered_map Graph; + typedef robin_hood::unordered_node_map Graph; typedef std::vector NodeVector; typedef std::pair> NodeElement; typedef typename NodeT::Coordinates Coordinates; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index b40624efd6a..7f6a82d31c7 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -408,6 +408,12 @@ class NodeLattice */ bool backtracePath(CoordinateVector & path); + /** + * \brief add node to the path + * \param current_node + */ + void addNodeToPath(NodePtr current_node, CoordinateVector & path); + NodeLattice * parent; Coordinates pose; static LatticeMotionTable motion_table; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index 51f6608b5ff..c782fe41e08 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -111,6 +111,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner unsigned int _angle_quantizations; bool _allow_unknown; int _max_iterations; + int _max_on_approach_iterations; SearchInfo _search_info; double _max_planning_time; double _lookup_table_size; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index c34204b18ea..f5207cd45c5 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -106,6 +106,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner SearchInfo _search_info; bool _allow_unknown; int _max_iterations; + int _max_on_approach_iterations; float _tolerance; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; double _max_planning_time; diff --git a/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h b/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h new file mode 100644 index 00000000000..5d3812a3a35 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h @@ -0,0 +1,2539 @@ +// Copyright (c) 2018-2021 Martin Ankerl +// ______ _____ ______ _________ +// ______________ ___ /_ ___(_)_______ ___ /_ ______ ______ ______ / +// __ ___/_ __ \__ __ \__ / __ __ \ __ __ \_ __ \_ __ \_ __ / +// _ / / /_/ /_ /_/ /_ / _ / / / _ / / // /_/ // /_/ // /_/ / +// /_/ \____/ /_.___/ /_/ /_/ /_/ ________/_/ /_/ \____/ \____/ \__,_/ +// _/_____/ +// +// Fast & memory efficient hashtable based on robin hood hashing for C++11/14/17/20 +// https://github.com/martinus/robin-hood-hashing +// +// Licensed under the MIT License . +// SPDX-License-Identifier: MIT +// Copyright (c) 2018-2021 Martin Ankerl +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ +#define NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ + +// see https://semver.org/ +#define ROBIN_HOOD_VERSION_MAJOR 3 // for incompatible API changes +#define ROBIN_HOOD_VERSION_MINOR 11 // for adding functionality in a backwards-compatible manner +#define ROBIN_HOOD_VERSION_PATCH 5 // for backwards-compatible bug fixes + +#include +#include +#include +#include +#include +#include // only to support hash of smart pointers +#include +#include +#include +#include +#include +#if __cplusplus >= 201703L +# include +#endif +/* *INDENT-OFF* */ + +// #define ROBIN_HOOD_LOG_ENABLED +#ifdef ROBIN_HOOD_LOG_ENABLED +# include +# define ROBIN_HOOD_LOG(...) \ + std::cout << __FUNCTION__ << "@" << __LINE__ << ": " << __VA_ARGS__ << std::endl; +#else +# define ROBIN_HOOD_LOG(x) +#endif + +// #define ROBIN_HOOD_TRACE_ENABLED +#ifdef ROBIN_HOOD_TRACE_ENABLED +# include +# define ROBIN_HOOD_TRACE(...) \ + std::cout << __FUNCTION__ << "@" << __LINE__ << ": " << __VA_ARGS__ << std::endl; +#else +# define ROBIN_HOOD_TRACE(x) +#endif + +// #define ROBIN_HOOD_COUNT_ENABLED +#ifdef ROBIN_HOOD_COUNT_ENABLED +# include +# define ROBIN_HOOD_COUNT(x) ++counts().x; +namespace robin_hood { +struct Counts { + uint64_t shiftUp{}; + uint64_t shiftDown{}; +}; +inline std::ostream& operator<<(std::ostream& os, Counts const& c) { + return os << c.shiftUp << " shiftUp" << std::endl << c.shiftDown << " shiftDown" << std::endl; +} + +static Counts& counts() { + static Counts counts{}; + return counts; +} +} // namespace robin_hood +#else +# define ROBIN_HOOD_COUNT(x) +#endif + +// all non-argument macros should use this facility. See +// https://www.fluentcpp.com/2019/05/28/better-macros-better-flags/ +#define ROBIN_HOOD(x) ROBIN_HOOD_PRIVATE_DEFINITION_##x() + +// mark unused members with this macro +#define ROBIN_HOOD_UNUSED(identifier) + +// bitness +#if SIZE_MAX == UINT32_MAX +# define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 32 +#elif SIZE_MAX == UINT64_MAX +# define ROBIN_HOOD_PRIVATE_DEFINITION_BITNESS() 64 +#else +# error Unsupported bitness +#endif + +// endianess +#ifdef _MSC_VER +# define ROBIN_HOOD_PRIVATE_DEFINITION_LITTLE_ENDIAN() 1 +# define ROBIN_HOOD_PRIVATE_DEFINITION_BIG_ENDIAN() 0 +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_LITTLE_ENDIAN() \ + (__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__) +# define ROBIN_HOOD_PRIVATE_DEFINITION_BIG_ENDIAN() (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__) +#endif + +// inline +#ifdef _MSC_VER +# define ROBIN_HOOD_PRIVATE_DEFINITION_NOINLINE() __declspec(noinline) +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_NOINLINE() __attribute__((noinline)) +#endif + +// exceptions +#if !defined(__cpp_exceptions) && !defined(__EXCEPTIONS) && !defined(_CPPUNWIND) +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_EXCEPTIONS() 0 +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_EXCEPTIONS() 1 +#endif + +// count leading/trailing bits +#if !defined(ROBIN_HOOD_DISABLE_INTRINSICS) +# ifdef _MSC_VER +# if ROBIN_HOOD(BITNESS) == 32 +# define ROBIN_HOOD_PRIVATE_DEFINITION_BITSCANFORWARD() _BitScanForward +# else +# define ROBIN_HOOD_PRIVATE_DEFINITION_BITSCANFORWARD() _BitScanForward64 +# endif +# include +# pragma intrinsic(ROBIN_HOOD(BITSCANFORWARD)) +# define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) \ + [](size_t mask) noexcept -> int { \ + unsigned long index; // NOLINT \ + return ROBIN_HOOD(BITSCANFORWARD)(&index, mask) ? static_cast(index) \ + : ROBIN_HOOD(BITNESS); \ + }(x) +# else +# if ROBIN_HOOD(BITNESS) == 32 +# define ROBIN_HOOD_PRIVATE_DEFINITION_CTZ() __builtin_ctzl +# define ROBIN_HOOD_PRIVATE_DEFINITION_CLZ() __builtin_clzl +# else +# define ROBIN_HOOD_PRIVATE_DEFINITION_CTZ() __builtin_ctzll +# define ROBIN_HOOD_PRIVATE_DEFINITION_CLZ() __builtin_clzll +# endif +# define ROBIN_HOOD_COUNT_LEADING_ZEROES(x) ((x) ? ROBIN_HOOD(CLZ)(x) : ROBIN_HOOD(BITNESS)) +# define ROBIN_HOOD_COUNT_TRAILING_ZEROES(x) ((x) ? ROBIN_HOOD(CTZ)(x) : ROBIN_HOOD(BITNESS)) +# endif +#endif + +// fallthrough +#ifndef __has_cpp_attribute // For backwards compatibility +# define __has_cpp_attribute(x) 0 +#endif +#if __has_cpp_attribute(clang::fallthrough) +# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() [[clang::fallthrough]] +#elif __has_cpp_attribute(gnu::fallthrough) +# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() [[gnu::fallthrough]] +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_FALLTHROUGH() +#endif + +// likely/unlikely +#ifdef _MSC_VER +# define ROBIN_HOOD_LIKELY(condition) condition +# define ROBIN_HOOD_UNLIKELY(condition) condition +#else +# define ROBIN_HOOD_LIKELY(condition) __builtin_expect(condition, 1) +# define ROBIN_HOOD_UNLIKELY(condition) __builtin_expect(condition, 0) +#endif + +// detect if native wchar_t type is availiable in MSVC +#ifdef _MSC_VER +# ifdef _NATIVE_WCHAR_T_DEFINED +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 1 +# else +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 0 +# endif +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 1 +#endif + +// detect if MSVC supports the pair(std::piecewise_construct_t,...) consructor being constexpr +#ifdef _MSC_VER +# if _MSC_VER <= 1900 +# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 1 +# else +# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 0 +# endif +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 0 +#endif + +// workaround missing "is_trivially_copyable" in g++ < 5.0 +// See https://stackoverflow.com/a/31798726/48181 +#if defined(__GNUC__) && __GNUC__ < 5 +# define ROBIN_HOOD_IS_TRIVIALLY_COPYABLE(...) __has_trivial_copy(__VA_ARGS__) +#else +# define ROBIN_HOOD_IS_TRIVIALLY_COPYABLE(...) std::is_trivially_copyable<__VA_ARGS__>::value +#endif + +// helpers for C++ versions, see https://gcc.gnu.org/onlinedocs/cpp/Standard-Predefined-Macros.html +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX() __cplusplus +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX98() 199711L +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX11() 201103L +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX14() 201402L +#define ROBIN_HOOD_PRIVATE_DEFINITION_CXX17() 201703L + +#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX17) +# define ROBIN_HOOD_PRIVATE_DEFINITION_NODISCARD() [[nodiscard]] +#else +# define ROBIN_HOOD_PRIVATE_DEFINITION_NODISCARD() +#endif + +namespace robin_hood { + +#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX14) +# define ROBIN_HOOD_STD std +#else + +// c++11 compatibility layer +namespace ROBIN_HOOD_STD { +template +struct alignment_of + : std::integral_constant::type)> {}; + +template +class integer_sequence { +public: + using value_type = T; + static_assert(std::is_integral::value, "not integral type"); + static constexpr std::size_t size() noexcept { + return sizeof...(Ints); + } +}; +template +using index_sequence = integer_sequence; + +namespace detail_ { +template +struct IntSeqImpl { + using TValue = T; + static_assert(std::is_integral::value, "not integral type"); + static_assert(Begin >= 0 && Begin < End, "unexpected argument (Begin<0 || Begin<=End)"); + + template + struct IntSeqCombiner; + + template + struct IntSeqCombiner, integer_sequence> { + using TResult = integer_sequence; + }; + + using TResult = + typename IntSeqCombiner::TResult, + typename IntSeqImpl::TResult>::TResult; +}; + +template +struct IntSeqImpl { + using TValue = T; + static_assert(std::is_integral::value, "not integral type"); + static_assert(Begin >= 0, "unexpected argument (Begin<0)"); + using TResult = integer_sequence; +}; + +template +struct IntSeqImpl { + using TValue = T; + static_assert(std::is_integral::value, "not integral type"); + static_assert(Begin >= 0, "unexpected argument (Begin<0)"); + using TResult = integer_sequence; +}; +} // namespace detail_ + +template +using make_integer_sequence = typename detail_::IntSeqImpl::TResult; + +template +using make_index_sequence = make_integer_sequence; + +template +using index_sequence_for = make_index_sequence; + +} // namespace ROBIN_HOOD_STD + +#endif + +namespace detail { + +// make sure we static_cast to the correct type for hash_int +#if ROBIN_HOOD(BITNESS) == 64 +using SizeT = uint64_t; +#else +using SizeT = uint32_t; +#endif + +template +T rotr(T x, unsigned k) { + return (x >> k) | (x << (8U * sizeof(T) - k)); +} + +// This cast gets rid of warnings like "cast from 'uint8_t*' {aka 'unsigned char*'} to +// 'uint64_t*' {aka 'long unsigned int*'} increases required alignment of target type". Use with +// care! +template +inline T reinterpret_cast_no_cast_align_warning(void* ptr) noexcept { + return reinterpret_cast(ptr); +} + +template +inline T reinterpret_cast_no_cast_align_warning(void const* ptr) noexcept { + return reinterpret_cast(ptr); +} + +// make sure this is not inlined as it is slow and dramatically enlarges code, thus making other +// inlinings more difficult. Throws are also generally the slow path. +template +[[noreturn]] ROBIN_HOOD(NOINLINE) +#if ROBIN_HOOD(HAS_EXCEPTIONS) + void doThrow(Args&&... args) { + throw E(std::forward(args)...); +} +#else + void doThrow(Args&&... ROBIN_HOOD_UNUSED(args) /*unused*/) { + abort(); +} +#endif + +template +T* assertNotNull(T* t, Args&&... args) { + if (ROBIN_HOOD_UNLIKELY(nullptr == t)) { + doThrow(std::forward(args)...); + } + return t; +} + +template +inline T unaligned_load(void const* ptr) noexcept { + // using memcpy so we don't get into unaligned load problems. + // compiler should optimize this very well anyways. + T t; + std::memcpy(&t, ptr, sizeof(T)); + return t; +} + +// Allocates bulks of memory for objects of type T. This deallocates the memory in the destructor, +// and keeps a linked list of the allocated memory around. Overhead per allocation is the size of a +// pointer. +template +class BulkPoolAllocator { +public: + BulkPoolAllocator() noexcept = default; + + // does not copy anything, just creates a new allocator. + BulkPoolAllocator(const BulkPoolAllocator& ROBIN_HOOD_UNUSED(o) /*unused*/) noexcept + : mHead(nullptr) + , mListForFree(nullptr) {} + + BulkPoolAllocator(BulkPoolAllocator&& o) noexcept + : mHead(o.mHead) + , mListForFree(o.mListForFree) { + o.mListForFree = nullptr; + o.mHead = nullptr; + } + + BulkPoolAllocator& operator=(BulkPoolAllocator&& o) noexcept { + reset(); + mHead = o.mHead; + mListForFree = o.mListForFree; + o.mListForFree = nullptr; + o.mHead = nullptr; + return *this; + } + + BulkPoolAllocator& + operator=(const BulkPoolAllocator& ROBIN_HOOD_UNUSED(o) /*unused*/) noexcept { + // does not do anything + return *this; + } + + ~BulkPoolAllocator() noexcept { + reset(); + } + + // Deallocates all allocated memory. + void reset() noexcept { + while (mListForFree) { + T* tmp = *mListForFree; + ROBIN_HOOD_LOG("std::free") + std::free(mListForFree); + mListForFree = reinterpret_cast_no_cast_align_warning(tmp); + } + mHead = nullptr; + } + + // allocates, but does NOT initialize. Use in-place new constructor, e.g. + // T* obj = pool.allocate(); + // ::new (static_cast(obj)) T(); + T* allocate() { + T* tmp = mHead; + if (!tmp) { + tmp = performAllocation(); + } + + mHead = *reinterpret_cast_no_cast_align_warning(tmp); + return tmp; + } + + // does not actually deallocate but puts it in store. + // make sure you have already called the destructor! e.g. with + // obj->~T(); + // pool.deallocate(obj); + void deallocate(T* obj) noexcept { + *reinterpret_cast_no_cast_align_warning(obj) = mHead; + mHead = obj; + } + + // Adds an already allocated block of memory to the allocator. This allocator is from now on + // responsible for freeing the data (with free()). If the provided data is not large enough to + // make use of, it is immediately freed. Otherwise it is reused and freed in the destructor. + void addOrFree(void* ptr, const size_t numBytes) noexcept { + // calculate number of available elements in ptr + if (numBytes < ALIGNMENT + ALIGNED_SIZE) { + // not enough data for at least one element. Free and return. + ROBIN_HOOD_LOG("std::free") + std::free(ptr); + } else { + ROBIN_HOOD_LOG("add to buffer") + add(ptr, numBytes); + } + } + + void swap(BulkPoolAllocator& other) noexcept { + using std::swap; + swap(mHead, other.mHead); + swap(mListForFree, other.mListForFree); + } + +private: + // iterates the list of allocated memory to calculate how many to alloc next. + // Recalculating this each time saves us a size_t member. + // This ignores the fact that memory blocks might have been added manually with addOrFree. In + // practice, this should not matter much. + ROBIN_HOOD(NODISCARD) size_t calcNumElementsToAlloc() const noexcept { + auto tmp = mListForFree; + size_t numAllocs = MinNumAllocs; + + while (numAllocs * 2 <= MaxNumAllocs && tmp) { + auto x = reinterpret_cast(tmp); // NOLINT + tmp = *x; + numAllocs *= 2; + } + + return numAllocs; + } + + // WARNING: Underflow if numBytes < ALIGNMENT! This is guarded in addOrFree(). + void add(void* ptr, const size_t numBytes) noexcept { + const size_t numElements = (numBytes - ALIGNMENT) / ALIGNED_SIZE; + auto data = reinterpret_cast(ptr); + + // link free list + auto x = reinterpret_cast(data); + *x = mListForFree; + mListForFree = data; + + // create linked list for newly allocated data + auto* const headT = + reinterpret_cast_no_cast_align_warning(reinterpret_cast(ptr) + ALIGNMENT); + + auto* const head = reinterpret_cast(headT); + + // Visual Studio compiler automatically unrolls this loop, which is pretty cool + for (size_t i = 0; i < numElements; ++i) { + *reinterpret_cast_no_cast_align_warning(head + i * ALIGNED_SIZE) = + head + (i + 1) * ALIGNED_SIZE; + } + + // last one points to 0 + *reinterpret_cast_no_cast_align_warning(head + (numElements - 1) * ALIGNED_SIZE) = + mHead; + mHead = headT; + } + + // Called when no memory is available (mHead == 0). + // Don't inline this slow path. + ROBIN_HOOD(NOINLINE) T* performAllocation() { + size_t const numElementsToAlloc = calcNumElementsToAlloc(); + + // alloc new memory: [prev |T, T, ... T] + size_t const bytes = ALIGNMENT + ALIGNED_SIZE * numElementsToAlloc; + ROBIN_HOOD_LOG("std::malloc " << bytes << " = " << ALIGNMENT << " + " << ALIGNED_SIZE + << " * " << numElementsToAlloc) + add(assertNotNull(std::malloc(bytes)), bytes); + return mHead; + } + + // enforce byte alignment of the T's +#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX14) + static constexpr size_t ALIGNMENT = + (std::max)(std::alignment_of::value, std::alignment_of::value); +#else + static const size_t ALIGNMENT = + (ROBIN_HOOD_STD::alignment_of::value > ROBIN_HOOD_STD::alignment_of::value) + ? ROBIN_HOOD_STD::alignment_of::value + : +ROBIN_HOOD_STD::alignment_of::value; // the + is for walkarround +#endif + + static constexpr size_t ALIGNED_SIZE = ((sizeof(T) - 1) / ALIGNMENT + 1) * ALIGNMENT; + + static_assert(MinNumAllocs >= 1, "MinNumAllocs"); + static_assert(MaxNumAllocs >= MinNumAllocs, "MaxNumAllocs"); + static_assert(ALIGNED_SIZE >= sizeof(T*), "ALIGNED_SIZE"); + static_assert(0 == (ALIGNED_SIZE % sizeof(T*)), "ALIGNED_SIZE mod"); + static_assert(ALIGNMENT >= sizeof(T*), "ALIGNMENT"); + + T* mHead{nullptr}; + T** mListForFree{nullptr}; +}; + +template +struct NodeAllocator; + +// dummy allocator that does nothing +template +struct NodeAllocator { + // we are not using the data, so just free it. + void addOrFree(void* ptr, size_t ROBIN_HOOD_UNUSED(numBytes)/*unused*/) noexcept { + ROBIN_HOOD_LOG("std::free") + std::free(ptr); + } +}; + +template +struct NodeAllocator : public BulkPoolAllocator {}; + +// c++14 doesn't have is_nothrow_swappable, and clang++ 6.0.1 doesn't like it either, so I'm making +// my own here. +namespace swappable { +#if ROBIN_HOOD(CXX) < ROBIN_HOOD(CXX17) +using std::swap; +template +struct nothrow { + static const bool value = noexcept(swap(std::declval(), std::declval())); +}; +#else +template +struct nothrow { + static const bool value = std::is_nothrow_swappable::value; +}; +#endif +} // namespace swappable + +} // namespace detail + +struct is_transparent_tag {}; + +// A custom pair implementation is used in the map because std::pair is not is_trivially_copyable, +// which means it would not be allowed to be used in std::memcpy. This struct is copyable, which is +// also tested. +template +struct pair { + using first_type = T1; + using second_type = T2; + + template ::value && + std::is_default_constructible::value>::type> + constexpr pair() noexcept(noexcept(U1()) && noexcept(U2())) + : first() + , second() {} + + // pair constructors are explicit so we don't accidentally call this ctor when we don't have to. + explicit constexpr pair(std::pair const& o) noexcept( + noexcept(T1(std::declval())) && noexcept(T2(std::declval()))) + : first(o.first) + , second(o.second) {} + + // pair constructors are explicit so we don't accidentally call this ctor when we don't have to. + explicit constexpr pair(std::pair&& o) noexcept(noexcept( + T1(std::move(std::declval()))) && noexcept(T2(std::move(std::declval())))) + : first(std::move(o.first)) + , second(std::move(o.second)) {} + + constexpr pair(T1&& a, T2&& b) noexcept(noexcept( + T1(std::move(std::declval()))) && noexcept(T2(std::move(std::declval())))) + : first(std::move(a)) + , second(std::move(b)) {} + + template + constexpr pair(U1&& a, U2&& b) noexcept(noexcept(T1(std::forward( + std::declval()))) && noexcept(T2(std::forward(std::declval())))) + : first(std::forward(a)) + , second(std::forward(b)) {} + + template + // MSVC 2015 produces error "C2476: ‘constexpr’ constructor does not initialize all members" + // if this constructor is constexpr +#if !ROBIN_HOOD(BROKEN_CONSTEXPR) + constexpr +#endif + pair(std::piecewise_construct_t /*unused*/, std::tuple a, + std::tuple + b) noexcept(noexcept(pair(std::declval&>(), + std::declval&>(), + ROBIN_HOOD_STD::index_sequence_for(), + ROBIN_HOOD_STD::index_sequence_for()))) + : pair(a, b, ROBIN_HOOD_STD::index_sequence_for(), + ROBIN_HOOD_STD::index_sequence_for()) { + } + + // constructor called from the std::piecewise_construct_t ctor + template + pair( + std::tuple& a, std::tuple& b, + ROBIN_HOOD_STD::index_sequence /*unused*/, + ROBIN_HOOD_STD::index_sequence /*unused*/) noexcept( + noexcept(T1(std::forward(std::get( + std::declval&>()))...)) && noexcept(T2(std:: + forward(std::get( + std::declval&>()))...))) + : first(std::forward(std::get(a))...) + , second(std::forward(std::get(b))...) { + // make visual studio compiler happy about warning about unused a & b. + // Visual studio's pair implementation disables warning 4100. + (void)a; + (void)b; + } + + void swap(pair& o) noexcept((detail::swappable::nothrow::value) && + (detail::swappable::nothrow::value)) { + using std::swap; + swap(first, o.first); + swap(second, o.second); + } + + T1 first; // NOLINT + T2 second; // NOLINT +}; + +template +inline void swap(pair& a, pair& b) noexcept( + noexcept(std::declval&>().swap(std::declval&>()))) { + a.swap(b); +} + +template +inline constexpr bool operator==(pair const& x, pair const& y) { + return (x.first == y.first) && (x.second == y.second); +} +template +inline constexpr bool operator!=(pair const& x, pair const& y) { + return !(x == y); +} +template +inline constexpr bool operator<(pair const& x, pair const& y) noexcept(noexcept( + std::declval() < std::declval()) && noexcept(std::declval() < + std::declval())) { + return x.first < y.first || (!(y.first < x.first) && x.second < y.second); +} +template +inline constexpr bool operator>(pair const& x, pair const& y) { + return y < x; +} +template +inline constexpr bool operator<=(pair const& x, pair const& y) { + return !(x > y); +} +template +inline constexpr bool operator>=(pair const& x, pair const& y) { + return !(x < y); +} + +inline size_t hash_bytes(void const* ptr, size_t len) noexcept { + static constexpr uint64_t m = UINT64_C(0xc6a4a7935bd1e995); + static constexpr uint64_t seed = UINT64_C(0xe17a1465); + static constexpr unsigned int r = 47; + + auto const* const data64 = static_cast(ptr); + uint64_t h = seed ^ (len * m); + + size_t const n_blocks = len / 8; + for (size_t i = 0; i < n_blocks; ++i) { + auto k = detail::unaligned_load(data64 + i); + + k *= m; + k ^= k >> r; + k *= m; + + h ^= k; + h *= m; + } + + auto const* const data8 = reinterpret_cast(data64 + n_blocks); + switch (len & 7U) { + case 7: + h ^= static_cast(data8[6]) << 48U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 6: + h ^= static_cast(data8[5]) << 40U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 5: + h ^= static_cast(data8[4]) << 32U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 4: + h ^= static_cast(data8[3]) << 24U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 3: + h ^= static_cast(data8[2]) << 16U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 2: + h ^= static_cast(data8[1]) << 8U; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + case 1: + h ^= static_cast(data8[0]); + h *= m; + ROBIN_HOOD(FALLTHROUGH); // FALLTHROUGH + default: + break; + } + + h ^= h >> r; + + // not doing the final step here, because this will be done by keyToIdx anyways + // h *= m; + // h ^= h >> r; + return static_cast(h); +} + +inline size_t hash_int(uint64_t x) noexcept { + // tried lots of different hashes, let's stick with murmurhash3. It's simple, fast, well tested, + // and doesn't need any special 128bit operations. + x ^= x >> 33U; + x *= UINT64_C(0xff51afd7ed558ccd); + x ^= x >> 33U; + + // not doing the final step here, because this will be done by keyToIdx anyways + // x *= UINT64_C(0xc4ceb9fe1a85ec53); + // x ^= x >> 33U; + return static_cast(x); +} + +// A thin wrapper around std::hash, performing an additional simple mixing step of the result. +template +struct hash : public std::hash { + size_t operator()(T const& obj) const + noexcept(noexcept(std::declval>().operator()(std::declval()))) { + // call base hash + auto result = std::hash::operator()(obj); + // return mixed of that, to be save against identity has + return hash_int(static_cast(result)); + } +}; + +template +struct hash> { + size_t operator()(std::basic_string const& str) const noexcept { + return hash_bytes(str.data(), sizeof(CharT) * str.size()); + } +}; + +#if ROBIN_HOOD(CXX) >= ROBIN_HOOD(CXX17) +template +struct hash> { + size_t operator()(std::basic_string_view const& sv) const noexcept { + return hash_bytes(sv.data(), sizeof(CharT) * sv.size()); + } +}; +#endif + +template +struct hash { + size_t operator()(T* ptr) const noexcept { + return hash_int(reinterpret_cast(ptr)); + } +}; + +template +struct hash> { + size_t operator()(std::unique_ptr const& ptr) const noexcept { + return hash_int(reinterpret_cast(ptr.get())); + } +}; + +template +struct hash> { + size_t operator()(std::shared_ptr const& ptr) const noexcept { + return hash_int(reinterpret_cast(ptr.get())); + } +}; + +template +struct hash::value>::type> { + size_t operator()(Enum e) const noexcept { + using Underlying = typename std::underlying_type::type; + return hash{}(static_cast(e)); + } +}; + +#define ROBIN_HOOD_HASH_INT(T) \ + template <> \ + struct hash { \ + size_t operator()(T const& obj) const noexcept { \ + return hash_int(static_cast(obj)); \ + } \ + } + +#if defined(__GNUC__) && !defined(__clang__) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wuseless-cast" +#endif +// see https://en.cppreference.com/w/cpp/utility/hash +ROBIN_HOOD_HASH_INT(bool); +ROBIN_HOOD_HASH_INT(char); +ROBIN_HOOD_HASH_INT(signed char); +ROBIN_HOOD_HASH_INT(unsigned char); +ROBIN_HOOD_HASH_INT(char16_t); +ROBIN_HOOD_HASH_INT(char32_t); +#if ROBIN_HOOD(HAS_NATIVE_WCHART) +ROBIN_HOOD_HASH_INT(wchar_t); +#endif +ROBIN_HOOD_HASH_INT(short); // NOLINT +ROBIN_HOOD_HASH_INT(unsigned short); // NOLINT +ROBIN_HOOD_HASH_INT(int); +ROBIN_HOOD_HASH_INT(unsigned int); +ROBIN_HOOD_HASH_INT(long); // NOLINT +ROBIN_HOOD_HASH_INT(long long); // NOLINT +ROBIN_HOOD_HASH_INT(unsigned long); // NOLINT +ROBIN_HOOD_HASH_INT(unsigned long long); // NOLINT +#if defined(__GNUC__) && !defined(__clang__) +# pragma GCC diagnostic pop +#endif +namespace detail { + +template +struct void_type { + using type = void; +}; + +template +struct has_is_transparent : public std::false_type {}; + +template +struct has_is_transparent::type> + : public std::true_type {}; + +// using wrapper classes for hash and key_equal prevents the diamond problem when the same type +// is used. see https://stackoverflow.com/a/28771920/48181 +template +struct WrapHash : public T { + WrapHash() = default; + explicit WrapHash(T const& o) noexcept(noexcept(T(std::declval()))) + : T(o) {} +}; + +template +struct WrapKeyEqual : public T { + WrapKeyEqual() = default; + explicit WrapKeyEqual(T const& o) noexcept(noexcept(T(std::declval()))) + : T(o) {} +}; + +// A highly optimized hashmap implementation, using the Robin Hood algorithm. +// +// In most cases, this map should be usable as a drop-in replacement for std::unordered_map, but +// be about 2x faster in most cases and require much less allocations. +// +// This implementation uses the following memory layout: +// +// [Node, Node, ... Node | info, info, ... infoSentinel ] +// +// * Node: either a DataNode that directly has the std::pair as member, +// or a DataNode with a pointer to std::pair. Which DataNode representation to use +// depends on how fast the swap() operation is. Heuristically, this is automatically choosen +// based on sizeof(). there are always 2^n Nodes. +// +// * info: Each Node in the map has a corresponding info byte, so there are 2^n info bytes. +// Each byte is initialized to 0, meaning the corresponding Node is empty. Set to 1 means the +// corresponding node contains data. Set to 2 means the corresponding Node is filled, but it +// actually belongs to the previous position and was pushed out because that place is already +// taken. +// +// * infoSentinel: Sentinel byte set to 1, so that iterator's ++ can stop at end() without the +// need for a idx variable. +// +// According to STL, order of templates has effect on throughput. That's why I've moved the +// boolean to the front. +// https://www.reddit.com/r/cpp/comments/ahp6iu/compile_time_binary_size_reductions_and_cs_future/eeguck4/ +template +class Table + : public WrapHash, + public WrapKeyEqual, + detail::NodeAllocator< + typename std::conditional< + std::is_void::value, Key, + robin_hood::pair::type, T>>::type, + 4, 16384, IsFlat> { +public: + static constexpr bool is_flat = IsFlat; + static constexpr bool is_map = !std::is_void::value; + static constexpr bool is_set = !is_map; + static constexpr bool is_transparent = + has_is_transparent::value && has_is_transparent::value; + + using key_type = Key; + using mapped_type = T; + using value_type = typename std::conditional< + is_set, Key, + robin_hood::pair::type, T>>::type; + using size_type = size_t; + using hasher = Hash; + using key_equal = KeyEqual; + using Self = Table; + +private: + static_assert(MaxLoadFactor100 > 10 && MaxLoadFactor100 < 100, + "MaxLoadFactor100 needs to be >10 && < 100"); + + using WHash = WrapHash; + using WKeyEqual = WrapKeyEqual; + + // configuration defaults + + // make sure we have 8 elements, needed to quickly rehash mInfo + static constexpr size_t InitialNumElements = sizeof(uint64_t); + static constexpr uint32_t InitialInfoNumBits = 5; + static constexpr uint8_t InitialInfoInc = 1U << InitialInfoNumBits; + static constexpr size_t InfoMask = InitialInfoInc - 1U; + static constexpr uint8_t InitialInfoHashShift = 0; + using DataPool = detail::NodeAllocator; + + // type needs to be wider than uint8_t. + using InfoType = uint32_t; + + // DataNode //////////////////////////////////////////////////////// + + // Primary template for the data node. We have special implementations for small and big + // objects. For large objects it is assumed that swap() is fairly slow, so we allocate these + // on the heap so swap merely swaps a pointer. + template + class DataNode {}; + + // Small: just allocate on the stack. + template + class DataNode final { + public: + template + explicit DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, Args&&... args) noexcept( + noexcept(value_type(std::forward(args)...))) + : mData(std::forward(args)...) {} + + DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, DataNode&& n) noexcept( + std::is_nothrow_move_constructible::value) + : mData(std::move(n.mData)) {} + + // doesn't do anything + void destroy(M& ROBIN_HOOD_UNUSED(map) /*unused*/) noexcept {} + void destroyDoNotDeallocate() noexcept {} + + value_type const* operator->() const noexcept { + return &mData; + } + value_type* operator->() noexcept { + return &mData; + } + + const value_type& operator*() const noexcept { + return mData; + } + + value_type& operator*() noexcept { + return mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() noexcept { + return mData.first; + } + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() noexcept { + return mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type + getFirst() const noexcept { + return mData.first; + } + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() const noexcept { + return mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getSecond() noexcept { + return mData.second; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getSecond() const noexcept { + return mData.second; + } + + void swap(DataNode& o) noexcept( + noexcept(std::declval().swap(std::declval()))) { + mData.swap(o.mData); + } + + private: + value_type mData; + }; + + // big object: allocate on heap. + template + class DataNode { + public: + template + explicit DataNode(M& map, Args&&... args) + : mData(map.allocate()) { + ::new (static_cast(mData)) value_type(std::forward(args)...); + } + + DataNode(M& ROBIN_HOOD_UNUSED(map) /*unused*/, DataNode&& n) noexcept + : mData(std::move(n.mData)) {} + + void destroy(M& map) noexcept { + // don't deallocate, just put it into list of datapool. + mData->~value_type(); + map.deallocate(mData); + } + + void destroyDoNotDeallocate() noexcept { + mData->~value_type(); + } + + value_type const* operator->() const noexcept { + return mData; + } + + value_type* operator->() noexcept { + return mData; + } + + const value_type& operator*() const { + return *mData; + } + + value_type& operator*() { + return *mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() noexcept { + return mData->first; + } + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() noexcept { + return *mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type + getFirst() const noexcept { + return mData->first; + } + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getFirst() const noexcept { + return *mData; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getSecond() noexcept { + return mData->second; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::type getSecond() const noexcept { + return mData->second; + } + + void swap(DataNode& o) noexcept { + using std::swap; + swap(mData, o.mData); + } + + private: + value_type* mData; + }; + + using Node = DataNode; + + // helpers for insertKeyPrepareEmptySpot: extract first entry (only const required) + ROBIN_HOOD(NODISCARD) key_type const& getFirstConst(Node const& n) const noexcept { + return n.getFirst(); + } + + // in case we have void mapped_type, we are not using a pair, thus we just route k through. + // No need to disable this because it's just not used if not applicable. + ROBIN_HOOD(NODISCARD) key_type const& getFirstConst(key_type const& k) const noexcept { + return k; + } + + // in case we have non-void mapped_type, we have a standard robin_hood::pair + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::value, key_type const&>::type + getFirstConst(value_type const& vt) const noexcept { + return vt.first; + } + + // Cloner ////////////////////////////////////////////////////////// + + template + struct Cloner; + + // fast path: Just copy data, without allocating anything. + template + struct Cloner { + void operator()(M const& source, M& target) const { + auto const* const src = reinterpret_cast(source.mKeyVals); + auto* tgt = reinterpret_cast(target.mKeyVals); + auto const numElementsWithBuffer = target.calcNumElementsWithBuffer(target.mMask + 1); + std::copy(src, src + target.calcNumBytesTotal(numElementsWithBuffer), tgt); + } + }; + + template + struct Cloner { + void operator()(M const& s, M& t) const { + auto const numElementsWithBuffer = t.calcNumElementsWithBuffer(t.mMask + 1); + std::copy(s.mInfo, s.mInfo + t.calcNumBytesInfo(numElementsWithBuffer), t.mInfo); + + for (size_t i = 0; i < numElementsWithBuffer; ++i) { + if (t.mInfo[i]) { + ::new (static_cast(t.mKeyVals + i)) Node(t, *s.mKeyVals[i]); + } + } + } + }; + + // Destroyer /////////////////////////////////////////////////////// + + template + struct Destroyer {}; + + template + struct Destroyer { + void nodes(M& m) const noexcept { + m.mNumElements = 0; + } + + void nodesDoNotDeallocate(M& m) const noexcept { + m.mNumElements = 0; + } + }; + + template + struct Destroyer { + void nodes(M& m) const noexcept { + m.mNumElements = 0; + // clear also resets mInfo to 0, that's sometimes not necessary. + auto const numElementsWithBuffer = m.calcNumElementsWithBuffer(m.mMask + 1); + + for (size_t idx = 0; idx < numElementsWithBuffer; ++idx) { + if (0 != m.mInfo[idx]) { + Node& n = m.mKeyVals[idx]; + n.destroy(m); + n.~Node(); + } + } + } + + void nodesDoNotDeallocate(M& m) const noexcept { + m.mNumElements = 0; + // clear also resets mInfo to 0, that's sometimes not necessary. + auto const numElementsWithBuffer = m.calcNumElementsWithBuffer(m.mMask + 1); + for (size_t idx = 0; idx < numElementsWithBuffer; ++idx) { + if (0 != m.mInfo[idx]) { + Node& n = m.mKeyVals[idx]; + n.destroyDoNotDeallocate(); + n.~Node(); + } + } + } + }; + + // Iter //////////////////////////////////////////////////////////// + + struct fast_forward_tag {}; + + // generic iterator for both const_iterator and iterator. + template + class Iter { + private: + using NodePtr = typename std::conditional::type; + + public: + using difference_type = std::ptrdiff_t; + using value_type = typename Self::value_type; + using reference = typename std::conditional::type; + using pointer = typename std::conditional::type; + using iterator_category = std::forward_iterator_tag; + + // default constructed iterator can be compared to itself, but WON'T return true when + // compared to end(). + Iter() = default; + + // Rule of zero: nothing specified. The conversion constructor is only enabled for + // iterator to const_iterator, so it doesn't accidentally work as a copy ctor. + + // Conversion constructor from iterator to const_iterator. + template ::type> + Iter(Iter const& other) noexcept + : mKeyVals(other.mKeyVals) + , mInfo(other.mInfo) {} + + Iter(NodePtr valPtr, uint8_t const* infoPtr) noexcept + : mKeyVals(valPtr) + , mInfo(infoPtr) {} + + Iter(NodePtr valPtr, uint8_t const* infoPtr, + fast_forward_tag ROBIN_HOOD_UNUSED(tag) /*unused*/) noexcept + : mKeyVals(valPtr) + , mInfo(infoPtr) { + fastForward(); + } + + template ::type> + Iter& operator=(Iter const& other) noexcept { + mKeyVals = other.mKeyVals; + mInfo = other.mInfo; + return *this; + } + + // prefix increment. Undefined behavior if we are at end()! + Iter& operator++() noexcept { + mInfo++; + mKeyVals++; + fastForward(); + return *this; + } + + Iter operator++(int) noexcept { + Iter tmp = *this; + ++(*this); + return tmp; + } + + reference operator*() const { + return **mKeyVals; + } + + pointer operator->() const { + return &**mKeyVals; + } + + template + bool operator==(Iter const& o) const noexcept { + return mKeyVals == o.mKeyVals; + } + + template + bool operator!=(Iter const& o) const noexcept { + return mKeyVals != o.mKeyVals; + } + + private: + // fast forward to the next non-free info byte + // I've tried a few variants that don't depend on intrinsics, but unfortunately they are + // quite a bit slower than this one. So I've reverted that change again. See map_benchmark. + void fastForward() noexcept { + size_t n = 0; + while (0U == (n = detail::unaligned_load(mInfo))) { + mInfo += sizeof(size_t); + mKeyVals += sizeof(size_t); + } +#if defined(ROBIN_HOOD_DISABLE_INTRINSICS) + // we know for certain that within the next 8 bytes we'll find a non-zero one. + if (ROBIN_HOOD_UNLIKELY(0U == detail::unaligned_load(mInfo))) { + mInfo += 4; + mKeyVals += 4; + } + if (ROBIN_HOOD_UNLIKELY(0U == detail::unaligned_load(mInfo))) { + mInfo += 2; + mKeyVals += 2; + } + if (ROBIN_HOOD_UNLIKELY(0U == *mInfo)) { + mInfo += 1; + mKeyVals += 1; + } +#else +# if ROBIN_HOOD(LITTLE_ENDIAN) + auto inc = ROBIN_HOOD_COUNT_TRAILING_ZEROES(n) / 8; +# else + auto inc = ROBIN_HOOD_COUNT_LEADING_ZEROES(n) / 8; +# endif + mInfo += inc; + mKeyVals += inc; +#endif + } + + friend class Table; + NodePtr mKeyVals{nullptr}; + uint8_t const* mInfo{nullptr}; + }; + + //////////////////////////////////////////////////////////////////// + + // highly performance relevant code. + // Lower bits are used for indexing into the array (2^n size) + // The upper 1-5 bits need to be a reasonable good hash, to save comparisons. + template + void keyToIdx(HashKey&& key, size_t* idx, InfoType* info) const { + // In addition to whatever hash is used, add another mul & shift so we get better hashing. + // This serves as a bad hash prevention, if the given data is + // badly mixed. + auto h = static_cast(WHash::operator()(key)); + + h *= mHashMultiplier; + h ^= h >> 33U; + + // the lower InitialInfoNumBits are reserved for info. + *info = mInfoInc + static_cast((h & InfoMask) >> mInfoHashShift); + *idx = (static_cast(h) >> InitialInfoNumBits) & mMask; + } + + // forwards the index by one, wrapping around at the end + void next(InfoType* info, size_t* idx) const noexcept { + *idx = *idx + 1; + *info += mInfoInc; + } + + void nextWhileLess(InfoType* info, size_t* idx) const noexcept { + // unrolling this by hand did not bring any speedups. + while (*info < mInfo[*idx]) { + next(info, idx); + } + } + + // Shift everything up by one element. Tries to move stuff around. + void + shiftUp(size_t startIdx, + size_t const insertion_idx) noexcept(std::is_nothrow_move_assignable::value) { + auto idx = startIdx; + ::new (static_cast(mKeyVals + idx)) Node(std::move(mKeyVals[idx - 1])); + while (--idx != insertion_idx) { + mKeyVals[idx] = std::move(mKeyVals[idx - 1]); + } + + idx = startIdx; + while (idx != insertion_idx) { + ROBIN_HOOD_COUNT(shiftUp) + mInfo[idx] = static_cast(mInfo[idx - 1] + mInfoInc); + if (ROBIN_HOOD_UNLIKELY(mInfo[idx] + mInfoInc > 0xFF)) { + mMaxNumElementsAllowed = 0; + } + --idx; + } + } + + void shiftDown(size_t idx) noexcept(std::is_nothrow_move_assignable::value) { + // until we find one that is either empty or has zero offset. + // TODO(martinus) we don't need to move everything, just the last one for the same + // bucket. + mKeyVals[idx].destroy(*this); + + // until we find one that is either empty or has zero offset. + while (mInfo[idx + 1] >= 2 * mInfoInc) { + ROBIN_HOOD_COUNT(shiftDown) + mInfo[idx] = static_cast(mInfo[idx + 1] - mInfoInc); + mKeyVals[idx] = std::move(mKeyVals[idx + 1]); + ++idx; + } + + mInfo[idx] = 0; + // don't destroy, we've moved it + // mKeyVals[idx].destroy(*this); + mKeyVals[idx].~Node(); + } + + // copy of find(), except that it returns iterator instead of const_iterator. + template + ROBIN_HOOD(NODISCARD) + size_t findIdx(Other const& key) const { + size_t idx{}; + InfoType info{}; + keyToIdx(key, &idx, &info); + + do { + // unrolling this twice gives a bit of a speedup. More unrolling did not help. + if (info == mInfo[idx] && + ROBIN_HOOD_LIKELY(WKeyEqual::operator()(key, mKeyVals[idx].getFirst()))) { + return idx; + } + next(&info, &idx); + if (info == mInfo[idx] && + ROBIN_HOOD_LIKELY(WKeyEqual::operator()(key, mKeyVals[idx].getFirst()))) { + return idx; + } + next(&info, &idx); + } while (info <= mInfo[idx]); + + // nothing found! + return mMask == 0 ? 0 + : static_cast(std::distance( + mKeyVals, reinterpret_cast_no_cast_align_warning(mInfo))); + } + + void cloneData(const Table& o) { + Cloner()(o, *this); + } + + // inserts a keyval that is guaranteed to be new, e.g. when the hashmap is resized. + // @return True on success, false if something went wrong + void insert_move(Node&& keyval) { + // we don't retry, fail if overflowing + // don't need to check max num elements + if (0 == mMaxNumElementsAllowed && !try_increase_info()) { + throwOverflowError(); + } + + size_t idx{}; + InfoType info{}; + keyToIdx(keyval.getFirst(), &idx, &info); + + // skip forward. Use <= because we are certain that the element is not there. + while (info <= mInfo[idx]) { + idx = idx + 1; + info += mInfoInc; + } + + // key not found, so we are now exactly where we want to insert it. + auto const insertion_idx = idx; + auto const insertion_info = static_cast(info); + if (ROBIN_HOOD_UNLIKELY(insertion_info + mInfoInc > 0xFF)) { + mMaxNumElementsAllowed = 0; + } + + // find an empty spot + while (0 != mInfo[idx]) { + next(&info, &idx); + } + + auto& l = mKeyVals[insertion_idx]; + if (idx == insertion_idx) { + ::new (static_cast(&l)) Node(std::move(keyval)); + } else { + shiftUp(idx, insertion_idx); + l = std::move(keyval); + } + + // put at empty spot + mInfo[insertion_idx] = insertion_info; + + ++mNumElements; + } + +public: + using iterator = Iter; + using const_iterator = Iter; + + Table() noexcept(noexcept(Hash()) && noexcept(KeyEqual())) + : WHash() + , WKeyEqual() { + ROBIN_HOOD_TRACE(this) + } + + // Creates an empty hash map. Nothing is allocated yet, this happens at the first insert. + // This tremendously speeds up ctor & dtor of a map that never receives an element. The + // penalty is payed at the first insert, and not before. Lookup of this empty map works + // because everybody points to DummyInfoByte::b. parameter bucket_count is dictated by the + // standard, but we can ignore it. + explicit Table( + size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/, const Hash& h = Hash{}, + const KeyEqual& equal = KeyEqual{}) noexcept(noexcept(Hash(h)) && noexcept(KeyEqual(equal))) + : WHash(h) + , WKeyEqual(equal) { + ROBIN_HOOD_TRACE(this) + } + + template + Table(Iter first, Iter last, size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/ = 0, + const Hash& h = Hash{}, const KeyEqual& equal = KeyEqual{}) + : WHash(h) + , WKeyEqual(equal) { + ROBIN_HOOD_TRACE(this) + insert(first, last); + } + + Table(std::initializer_list initlist, + size_t ROBIN_HOOD_UNUSED(bucket_count) /*unused*/ = 0, const Hash& h = Hash{}, + const KeyEqual& equal = KeyEqual{}) + : WHash(h) + , WKeyEqual(equal) { + ROBIN_HOOD_TRACE(this) + insert(initlist.begin(), initlist.end()); + } + + Table(Table&& o) noexcept + : WHash(std::move(static_cast(o))) + , WKeyEqual(std::move(static_cast(o))) + , DataPool(std::move(static_cast(o))) { + ROBIN_HOOD_TRACE(this) + if (o.mMask) { + mHashMultiplier = std::move(o.mHashMultiplier); + mKeyVals = std::move(o.mKeyVals); + mInfo = std::move(o.mInfo); + mNumElements = std::move(o.mNumElements); + mMask = std::move(o.mMask); + mMaxNumElementsAllowed = std::move(o.mMaxNumElementsAllowed); + mInfoInc = std::move(o.mInfoInc); + mInfoHashShift = std::move(o.mInfoHashShift); + // set other's mask to 0 so its destructor won't do anything + o.init(); + } + } + + Table& operator=(Table&& o) noexcept { + ROBIN_HOOD_TRACE(this) + if (&o != this) { + if (o.mMask) { + // only move stuff if the other map actually has some data + destroy(); + mHashMultiplier = std::move(o.mHashMultiplier); + mKeyVals = std::move(o.mKeyVals); + mInfo = std::move(o.mInfo); + mNumElements = std::move(o.mNumElements); + mMask = std::move(o.mMask); + mMaxNumElementsAllowed = std::move(o.mMaxNumElementsAllowed); + mInfoInc = std::move(o.mInfoInc); + mInfoHashShift = std::move(o.mInfoHashShift); + WHash::operator=(std::move(static_cast(o))); + WKeyEqual::operator=(std::move(static_cast(o))); + DataPool::operator=(std::move(static_cast(o))); + + o.init(); + + } else { + // nothing in the other map => just clear us. + clear(); + } + } + return *this; + } + + Table(const Table& o) + : WHash(static_cast(o)) + , WKeyEqual(static_cast(o)) + , DataPool(static_cast(o)) { + ROBIN_HOOD_TRACE(this) + if (!o.empty()) { + // not empty: create an exact copy. it is also possible to just iterate through all + // elements and insert them, but copying is probably faster. + + auto const numElementsWithBuffer = calcNumElementsWithBuffer(o.mMask + 1); + auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); + + ROBIN_HOOD_LOG("std::malloc " << numBytesTotal << " = calcNumBytesTotal(" + << numElementsWithBuffer << ")") + mHashMultiplier = o.mHashMultiplier; + mKeyVals = static_cast( + detail::assertNotNull(std::malloc(numBytesTotal))); + // no need for calloc because clonData does memcpy + mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); + mNumElements = o.mNumElements; + mMask = o.mMask; + mMaxNumElementsAllowed = o.mMaxNumElementsAllowed; + mInfoInc = o.mInfoInc; + mInfoHashShift = o.mInfoHashShift; + cloneData(o); + } + } + + // Creates a copy of the given map. Copy constructor of each entry is used. + // Not sure why clang-tidy thinks this doesn't handle self assignment, it does + Table& operator=(Table const& o) { + ROBIN_HOOD_TRACE(this) + if (&o == this) { + // prevent assigning of itself + return *this; + } + + // we keep using the old allocator and not assign the new one, because we want to keep + // the memory available. when it is the same size. + if (o.empty()) { + if (0 == mMask) { + // nothing to do, we are empty too + return *this; + } + + // not empty: destroy what we have there + // clear also resets mInfo to 0, that's sometimes not necessary. + destroy(); + init(); + WHash::operator=(static_cast(o)); + WKeyEqual::operator=(static_cast(o)); + DataPool::operator=(static_cast(o)); + + return *this; + } + + // clean up old stuff + Destroyer::value>{}.nodes(*this); + + if (mMask != o.mMask) { + // no luck: we don't have the same array size allocated, so we need to realloc. + if (0 != mMask) { + // only deallocate if we actually have data! + ROBIN_HOOD_LOG("std::free") + std::free(mKeyVals); + } + + auto const numElementsWithBuffer = calcNumElementsWithBuffer(o.mMask + 1); + auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); + ROBIN_HOOD_LOG("std::malloc " << numBytesTotal << " = calcNumBytesTotal(" + << numElementsWithBuffer << ")") + mKeyVals = static_cast( + detail::assertNotNull(std::malloc(numBytesTotal))); + + // no need for calloc here because cloneData performs a memcpy. + mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); + // sentinel is set in cloneData + } + WHash::operator=(static_cast(o)); + WKeyEqual::operator=(static_cast(o)); + DataPool::operator=(static_cast(o)); + mHashMultiplier = o.mHashMultiplier; + mNumElements = o.mNumElements; + mMask = o.mMask; + mMaxNumElementsAllowed = o.mMaxNumElementsAllowed; + mInfoInc = o.mInfoInc; + mInfoHashShift = o.mInfoHashShift; + cloneData(o); + + return *this; + } + + // Swaps everything between the two maps. + void swap(Table& o) { + ROBIN_HOOD_TRACE(this) + using std::swap; + swap(o, *this); + } + + // Clears all data, without resizing. + void clear() { + ROBIN_HOOD_TRACE(this) + if (empty()) { + // don't do anything! also important because we don't want to write to + // DummyInfoByte::b, even though we would just write 0 to it. + return; + } + + Destroyer::value>{}.nodes(*this); + + auto const numElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); + // clear everything, then set the sentinel again + uint8_t const z = 0; + std::fill(mInfo, mInfo + calcNumBytesInfo(numElementsWithBuffer), z); + mInfo[numElementsWithBuffer] = 1; + + mInfoInc = InitialInfoInc; + mInfoHashShift = InitialInfoHashShift; + } + + // Destroys the map and all it's contents. + ~Table() { + ROBIN_HOOD_TRACE(this) + destroy(); + } + + // Checks if both tables contain the same entries. Order is irrelevant. + bool operator==(const Table& other) const { + ROBIN_HOOD_TRACE(this) + if (other.size() != size()) { + return false; + } + for (auto const& otherEntry : other) { + if (!has(otherEntry)) { + return false; + } + } + + return true; + } + + bool operator!=(const Table& other) const { + ROBIN_HOOD_TRACE(this) + return !operator==(other); + } + + template + typename std::enable_if::value, Q&>::type operator[](const key_type& key) { + ROBIN_HOOD_TRACE(this) + auto idxAndState = insertKeyPrepareEmptySpot(key); + switch (idxAndState.second) { + case InsertionState::key_found: + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) + Node(*this, std::piecewise_construct, std::forward_as_tuple(key), + std::forward_as_tuple()); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, + std::forward_as_tuple(key), std::forward_as_tuple()); + break; + + case InsertionState::overflow_error: + throwOverflowError(); + } + + return mKeyVals[idxAndState.first].getSecond(); + } + + template + typename std::enable_if::value, Q&>::type operator[](key_type&& key) { + ROBIN_HOOD_TRACE(this) + auto idxAndState = insertKeyPrepareEmptySpot(key); + switch (idxAndState.second) { + case InsertionState::key_found: + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) + Node(*this, std::piecewise_construct, std::forward_as_tuple(std::move(key)), + std::forward_as_tuple()); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = + Node(*this, std::piecewise_construct, std::forward_as_tuple(std::move(key)), + std::forward_as_tuple()); + break; + + case InsertionState::overflow_error: + throwOverflowError(); + } + + return mKeyVals[idxAndState.first].getSecond(); + } + + template + void insert(Iter first, Iter last) { + for (; first != last; ++first) { + // value_type ctor needed because this might be called with std::pair's + insert(value_type(*first)); + } + } + + void insert(std::initializer_list ilist) { + for (auto&& vt : ilist) { + insert(std::move(vt)); + } + } + + template + std::pair emplace(Args&&... args) { + ROBIN_HOOD_TRACE(this) + Node n{*this, std::forward(args)...}; + auto idxAndState = insertKeyPrepareEmptySpot(getFirstConst(n)); + switch (idxAndState.second) { + case InsertionState::key_found: + n.destroy(*this); + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) Node(*this, std::move(n)); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = std::move(n); + break; + + case InsertionState::overflow_error: + n.destroy(*this); + throwOverflowError(); + break; + } + + return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), + InsertionState::key_found != idxAndState.second); + } + + template + iterator emplace_hint(const_iterator position, Args&&... args) { + (void)position; + return emplace(std::forward(args)...).first; + } + + template + std::pair try_emplace(const key_type& key, Args&&... args) { + return try_emplace_impl(key, std::forward(args)...); + } + + template + std::pair try_emplace(key_type&& key, Args&&... args) { + return try_emplace_impl(std::move(key), std::forward(args)...); + } + + template + iterator try_emplace(const_iterator hint, const key_type& key, Args&&... args) { + (void)hint; + return try_emplace_impl(key, std::forward(args)...).first; + } + + template + iterator try_emplace(const_iterator hint, key_type&& key, Args&&... args) { + (void)hint; + return try_emplace_impl(std::move(key), std::forward(args)...).first; + } + + template + std::pair insert_or_assign(const key_type& key, Mapped&& obj) { + return insertOrAssignImpl(key, std::forward(obj)); + } + + template + std::pair insert_or_assign(key_type&& key, Mapped&& obj) { + return insertOrAssignImpl(std::move(key), std::forward(obj)); + } + + template + iterator insert_or_assign(const_iterator hint, const key_type& key, Mapped&& obj) { + (void)hint; + return insertOrAssignImpl(key, std::forward(obj)).first; + } + + template + iterator insert_or_assign(const_iterator hint, key_type&& key, Mapped&& obj) { + (void)hint; + return insertOrAssignImpl(std::move(key), std::forward(obj)).first; + } + + std::pair insert(const value_type& keyval) { + ROBIN_HOOD_TRACE(this) + return emplace(keyval); + } + + iterator insert(const_iterator hint, const value_type& keyval) { + (void)hint; + return emplace(keyval).first; + } + + std::pair insert(value_type&& keyval) { + return emplace(std::move(keyval)); + } + + iterator insert(const_iterator hint, value_type&& keyval) { + (void)hint; + return emplace(std::move(keyval)).first; + } + + // Returns 1 if key is found, 0 otherwise. + size_t count(const key_type& key) const { // NOLINT + ROBIN_HOOD_TRACE(this) + auto kv = mKeyVals + findIdx(key); + if (kv != reinterpret_cast_no_cast_align_warning(mInfo)) { + return 1; + } + return 0; + } + + template + typename std::enable_if::type count(const OtherKey& key) const { + ROBIN_HOOD_TRACE(this) + auto kv = mKeyVals + findIdx(key); + if (kv != reinterpret_cast_no_cast_align_warning(mInfo)) { + return 1; + } + return 0; + } + + bool contains(const key_type& key) const { // NOLINT + return 1U == count(key); + } + + template + typename std::enable_if::type contains(const OtherKey& key) const { + return 1U == count(key); + } + + // Returns a reference to the value found for key. + // Throws std::out_of_range if element cannot be found + template + typename std::enable_if::value, Q&>::type at(key_type const& key) { + ROBIN_HOOD_TRACE(this) + auto kv = mKeyVals + findIdx(key); + if (kv == reinterpret_cast_no_cast_align_warning(mInfo)) { + doThrow("key not found"); + } + return kv->getSecond(); + } + + // Returns a reference to the value found for key. + // Throws std::out_of_range if element cannot be found + template + typename std::enable_if::value, Q const&>::type at(key_type const& key) const { + ROBIN_HOOD_TRACE(this) + auto kv = mKeyVals + findIdx(key); + if (kv == reinterpret_cast_no_cast_align_warning(mInfo)) { + doThrow("key not found"); + } + return kv->getSecond(); + } + + const_iterator find(const key_type& key) const { // NOLINT + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return const_iterator{mKeyVals + idx, mInfo + idx}; + } + + template + const_iterator find(const OtherKey& key, is_transparent_tag /*unused*/) const { + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return const_iterator{mKeyVals + idx, mInfo + idx}; + } + + template + typename std::enable_if::type // NOLINT + find(const OtherKey& key) const { // NOLINT + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return const_iterator{mKeyVals + idx, mInfo + idx}; + } + + iterator find(const key_type& key) { + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return iterator{mKeyVals + idx, mInfo + idx}; + } + + template + iterator find(const OtherKey& key, is_transparent_tag/*unused*/) { + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return iterator{mKeyVals + idx, mInfo + idx}; + } + + template + typename std::enable_if::type find(const OtherKey& key) { + ROBIN_HOOD_TRACE(this) + const size_t idx = findIdx(key); + return iterator{mKeyVals + idx, mInfo + idx}; + } + + iterator begin() { + ROBIN_HOOD_TRACE(this) + if (empty()) { + return end(); + } + return iterator(mKeyVals, mInfo, fast_forward_tag{}); + } + const_iterator begin() const { // NOLINT + ROBIN_HOOD_TRACE(this) + return cbegin(); + } + const_iterator cbegin() const { // NOLINT + ROBIN_HOOD_TRACE(this) + if (empty()) { + return cend(); + } + return const_iterator(mKeyVals, mInfo, fast_forward_tag{}); + } + + iterator end() { + ROBIN_HOOD_TRACE(this) + // no need to supply valid info pointer: end() must not be dereferenced, and only node + // pointer is compared. + return iterator{reinterpret_cast_no_cast_align_warning(mInfo), nullptr}; + } + const_iterator end() const { // NOLINT + ROBIN_HOOD_TRACE(this) + return cend(); + } + const_iterator cend() const { // NOLINT + ROBIN_HOOD_TRACE(this) + return const_iterator{reinterpret_cast_no_cast_align_warning(mInfo), nullptr}; + } + + iterator erase(const_iterator pos) { + ROBIN_HOOD_TRACE(this) + // its safe to perform const cast here + return erase(iterator{const_cast(pos.mKeyVals), const_cast(pos.mInfo)}); + } + + // Erases element at pos, returns iterator to the next element. + iterator erase(iterator pos) { + ROBIN_HOOD_TRACE(this) + // we assume that pos always points to a valid entry, and not end(). + auto const idx = static_cast(pos.mKeyVals - mKeyVals); + + shiftDown(idx); + --mNumElements; + + if (*pos.mInfo) { + // we've backward shifted, return this again + return pos; + } + + // no backward shift, return next element + return ++pos; + } + + size_t erase(const key_type& key) { + ROBIN_HOOD_TRACE(this) + size_t idx{}; + InfoType info{}; + keyToIdx(key, &idx, &info); + + // check while info matches with the source idx + do { + if (info == mInfo[idx] && WKeyEqual::operator()(key, mKeyVals[idx].getFirst())) { + shiftDown(idx); + --mNumElements; + return 1; + } + next(&info, &idx); + } while (info <= mInfo[idx]); + + // nothing found to delete + return 0; + } + + // reserves space for the specified number of elements. Makes sure the old data fits. + // exactly the same as reserve(c). + void rehash(size_t c) { + // forces a reserve + reserve(c, true); + } + + // reserves space for the specified number of elements. Makes sure the old data fits. + // Exactly the same as rehash(c). Use rehash(0) to shrink to fit. + void reserve(size_t c) { + // reserve, but don't force rehash + reserve(c, false); + } + + // If possible reallocates the map to a smaller one. This frees the underlying table. + // Does not do anything if load_factor is too large for decreasing the table's size. + void compact() { + ROBIN_HOOD_TRACE(this) + auto newSize = InitialNumElements; + while (calcMaxNumElementsAllowed(newSize) < mNumElements && newSize != 0) { + newSize *= 2; + } + if (ROBIN_HOOD_UNLIKELY(newSize == 0)) { + throwOverflowError(); + } + + ROBIN_HOOD_LOG("newSize > mMask + 1: " << newSize << " > " << mMask << " + 1") + + // only actually do anything when the new size is bigger than the old one. This prevents to + // continuously allocate for each reserve() call. + if (newSize < mMask + 1) { + rehashPowerOfTwo(newSize, true); + } + } + + size_type size() const noexcept { // NOLINT + ROBIN_HOOD_TRACE(this) + return mNumElements; + } + + size_type max_size() const noexcept { // NOLINT + ROBIN_HOOD_TRACE(this) + return static_cast(-1); + } + + ROBIN_HOOD(NODISCARD) bool empty() const noexcept { + ROBIN_HOOD_TRACE(this) + return 0 == mNumElements; + } + + float max_load_factor() const noexcept { // NOLINT + ROBIN_HOOD_TRACE(this) + return MaxLoadFactor100 / 100.0F; + } + + // Average number of elements per bucket. Since we allow only 1 per bucket + float load_factor() const noexcept { // NOLINT + ROBIN_HOOD_TRACE(this) + return static_cast(size()) / static_cast(mMask + 1); + } + + ROBIN_HOOD(NODISCARD) size_t mask() const noexcept { + ROBIN_HOOD_TRACE(this) + return mMask; + } + + ROBIN_HOOD(NODISCARD) size_t calcMaxNumElementsAllowed(size_t maxElements) const noexcept { + if (ROBIN_HOOD_LIKELY(maxElements <= (std::numeric_limits::max)() / 100)) { + return maxElements * MaxLoadFactor100 / 100; + } + + // we might be a bit inprecise, but since maxElements is quite large that doesn't matter + return (maxElements / 100) * MaxLoadFactor100; + } + + ROBIN_HOOD(NODISCARD) size_t calcNumBytesInfo(size_t numElements) const noexcept { + // we add a uint64_t, which houses the sentinel (first byte) and padding so we can load + // 64bit types. + return numElements + sizeof(uint64_t); + } + + ROBIN_HOOD(NODISCARD) + size_t calcNumElementsWithBuffer(size_t numElements) const noexcept { + auto maxNumElementsAllowed = calcMaxNumElementsAllowed(numElements); + return numElements + (std::min)(maxNumElementsAllowed, (static_cast(0xFF))); + } + + // calculation only allowed for 2^n values + ROBIN_HOOD(NODISCARD) size_t calcNumBytesTotal(size_t numElements) const { +#if ROBIN_HOOD(BITNESS) == 64 + return numElements * sizeof(Node) + calcNumBytesInfo(numElements); +#else + // make sure we're doing 64bit operations, so we are at least safe against 32bit overflows. + auto const ne = static_cast(numElements); + auto const s = static_cast(sizeof(Node)); + auto const infos = static_cast(calcNumBytesInfo(numElements)); + + auto const total64 = ne * s + infos; + auto const total = static_cast(total64); + + if (ROBIN_HOOD_UNLIKELY(static_cast(total) != total64)) { + throwOverflowError(); + } + return total; +#endif + } + +private: + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::value, bool>::type has(const value_type& e) const { + ROBIN_HOOD_TRACE(this) + auto it = find(e.first); + return it != end() && it->second == e.second; + } + + template + ROBIN_HOOD(NODISCARD) + typename std::enable_if::value, bool>::type has(const value_type& e) const { + ROBIN_HOOD_TRACE(this) + return find(e) != end(); + } + + void reserve(size_t c, bool forceRehash) { + ROBIN_HOOD_TRACE(this) + auto const minElementsAllowed = (std::max)(c, mNumElements); + auto newSize = InitialNumElements; + while (calcMaxNumElementsAllowed(newSize) < minElementsAllowed && newSize != 0) { + newSize *= 2; + } + if (ROBIN_HOOD_UNLIKELY(newSize == 0)) { + throwOverflowError(); + } + + ROBIN_HOOD_LOG("newSize > mMask + 1: " << newSize << " > " << mMask << " + 1") + + // only actually do anything when the new size is bigger than the old one. This prevents to + // continuously allocate for each reserve() call. + if (forceRehash || newSize > mMask + 1) { + rehashPowerOfTwo(newSize, false); + } + } + + // reserves space for at least the specified number of elements. + // only works if numBuckets if power of two + // True on success, false otherwise + void rehashPowerOfTwo(size_t numBuckets, bool forceFree) { + ROBIN_HOOD_TRACE(this) + + Node* const oldKeyVals = mKeyVals; + uint8_t const* const oldInfo = mInfo; + + const size_t oldMaxElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); + + // resize operation: move stuff + initData(numBuckets); + if (oldMaxElementsWithBuffer > 1) { + for (size_t i = 0; i < oldMaxElementsWithBuffer; ++i) { + if (oldInfo[i] != 0) { + // might throw an exception, which is really bad since we are in the middle of + // moving stuff. + insert_move(std::move(oldKeyVals[i])); + // destroy the node but DON'T destroy the data. + oldKeyVals[i].~Node(); + } + } + + // this check is not necessary as it's guarded by the previous if, but it helps + // silence g++'s overeager "attempt to free a non-heap object 'map' + // [-Werror=free-nonheap-object]" warning. + if (oldKeyVals != reinterpret_cast_no_cast_align_warning(&mMask)) { + // don't destroy old data: put it into the pool instead + if (forceFree) { + std::free(oldKeyVals); + } else { + DataPool::addOrFree(oldKeyVals, calcNumBytesTotal(oldMaxElementsWithBuffer)); + } + } + } + } + + ROBIN_HOOD(NOINLINE) void throwOverflowError() const { +#if ROBIN_HOOD(HAS_EXCEPTIONS) + throw std::overflow_error("robin_hood::map overflow"); +#else + abort(); +#endif + } + + template + std::pair try_emplace_impl(OtherKey&& key, Args&&... args) { + ROBIN_HOOD_TRACE(this) + auto idxAndState = insertKeyPrepareEmptySpot(key); + switch (idxAndState.second) { + case InsertionState::key_found: + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) Node( + *this, std::piecewise_construct, std::forward_as_tuple(std::forward(key)), + std::forward_as_tuple(std::forward(args)...)); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, + std::forward_as_tuple(std::forward(key)), + std::forward_as_tuple(std::forward(args)...)); + break; + + case InsertionState::overflow_error: + throwOverflowError(); + break; + } + + return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), + InsertionState::key_found != idxAndState.second); + } + + template + std::pair insertOrAssignImpl(OtherKey&& key, Mapped&& obj) { + ROBIN_HOOD_TRACE(this) + auto idxAndState = insertKeyPrepareEmptySpot(key); + switch (idxAndState.second) { + case InsertionState::key_found: + mKeyVals[idxAndState.first].getSecond() = std::forward(obj); + break; + + case InsertionState::new_node: + ::new (static_cast(&mKeyVals[idxAndState.first])) Node( + *this, std::piecewise_construct, std::forward_as_tuple(std::forward(key)), + std::forward_as_tuple(std::forward(obj))); + break; + + case InsertionState::overwrite_node: + mKeyVals[idxAndState.first] = Node(*this, std::piecewise_construct, + std::forward_as_tuple(std::forward(key)), + std::forward_as_tuple(std::forward(obj))); + break; + + case InsertionState::overflow_error: + throwOverflowError(); + break; + } + + return std::make_pair(iterator(mKeyVals + idxAndState.first, mInfo + idxAndState.first), + InsertionState::key_found != idxAndState.second); + } + + void initData(size_t max_elements) { + mNumElements = 0; + mMask = max_elements - 1; + mMaxNumElementsAllowed = calcMaxNumElementsAllowed(max_elements); + + auto const numElementsWithBuffer = calcNumElementsWithBuffer(max_elements); + + // malloc & zero mInfo. Faster than calloc everything. + auto const numBytesTotal = calcNumBytesTotal(numElementsWithBuffer); + ROBIN_HOOD_LOG("std::calloc " << numBytesTotal << " = calcNumBytesTotal(" + << numElementsWithBuffer << ")") + mKeyVals = reinterpret_cast( + detail::assertNotNull(std::malloc(numBytesTotal))); + mInfo = reinterpret_cast(mKeyVals + numElementsWithBuffer); + std::memset(mInfo, 0, numBytesTotal - numElementsWithBuffer * sizeof(Node)); + + // set sentinel + mInfo[numElementsWithBuffer] = 1; + + mInfoInc = InitialInfoInc; + mInfoHashShift = InitialInfoHashShift; + } + + enum class InsertionState { overflow_error, key_found, new_node, overwrite_node }; + + // Finds key, and if not already present prepares a spot where to pot the key & value. + // This potentially shifts nodes out of the way, updates mInfo and number of inserted + // elements, so the only operation left to do is create/assign a new node at that spot. + template + std::pair insertKeyPrepareEmptySpot(OtherKey&& key) { + for (int i = 0; i < 256; ++i) { + size_t idx{}; + InfoType info{}; + keyToIdx(key, &idx, &info); + nextWhileLess(&info, &idx); + + // while we potentially have a match + while (info == mInfo[idx]) { + if (WKeyEqual::operator()(key, mKeyVals[idx].getFirst())) { + // key already exists, do NOT insert. + // see http://en.cppreference.com/w/cpp/container/unordered_map/insert + return std::make_pair(idx, InsertionState::key_found); + } + next(&info, &idx); + } + + // unlikely that this evaluates to true + if (ROBIN_HOOD_UNLIKELY(mNumElements >= mMaxNumElementsAllowed)) { + if (!increase_size()) { + return std::make_pair(size_t(0), InsertionState::overflow_error); + } + continue; + } + + // key not found, so we are now exactly where we want to insert it. + auto const insertion_idx = idx; + auto const insertion_info = info; + if (ROBIN_HOOD_UNLIKELY(insertion_info + mInfoInc > 0xFF)) { + mMaxNumElementsAllowed = 0; + } + + // find an empty spot + while (0 != mInfo[idx]) { + next(&info, &idx); + } + + if (idx != insertion_idx) { + shiftUp(idx, insertion_idx); + } + // put at empty spot + mInfo[insertion_idx] = static_cast(insertion_info); + ++mNumElements; + return std::make_pair(insertion_idx, idx == insertion_idx + ? InsertionState::new_node + : InsertionState::overwrite_node); + } + + // enough attempts failed, so finally give up. + return std::make_pair(size_t(0), InsertionState::overflow_error); + } + + bool try_increase_info() { + ROBIN_HOOD_LOG("mInfoInc=" << mInfoInc << ", numElements=" << mNumElements + << ", maxNumElementsAllowed=" + << calcMaxNumElementsAllowed(mMask + 1)) + if (mInfoInc <= 2) { + // need to be > 2 so that shift works (otherwise undefined behavior!) + return false; + } + // we got space left, try to make info smaller + mInfoInc = static_cast(mInfoInc >> 1U); + + // remove one bit of the hash, leaving more space for the distance info. + // This is extremely fast because we can operate on 8 bytes at once. + ++mInfoHashShift; + auto const numElementsWithBuffer = calcNumElementsWithBuffer(mMask + 1); + + for (size_t i = 0; i < numElementsWithBuffer; i += 8) { + auto val = unaligned_load(mInfo + i); + val = (val >> 1U) & UINT64_C(0x7f7f7f7f7f7f7f7f); + std::memcpy(mInfo + i, &val, sizeof(val)); + } + // update sentinel, which might have been cleared out! + mInfo[numElementsWithBuffer] = 1; + + mMaxNumElementsAllowed = calcMaxNumElementsAllowed(mMask + 1); + return true; + } + + // True if resize was possible, false otherwise + bool increase_size() { + // nothing allocated yet? just allocate InitialNumElements + if (0 == mMask) { + initData(InitialNumElements); + return true; + } + + auto const maxNumElementsAllowed = calcMaxNumElementsAllowed(mMask + 1); + if (mNumElements < maxNumElementsAllowed && try_increase_info()) { + return true; + } + + ROBIN_HOOD_LOG("mNumElements=" << mNumElements << ", maxNumElementsAllowed=" + << maxNumElementsAllowed << ", load=" + << (static_cast(mNumElements) * 100.0 / + (static_cast(mMask) + 1))) + + if (mNumElements * 2 < calcMaxNumElementsAllowed(mMask + 1)) { + // we have to resize, even though there would still be plenty of space left! + // Try to rehash instead. Delete freed memory so we don't steadyily increase mem in case + // we have to rehash a few times + nextHashMultiplier(); + rehashPowerOfTwo(mMask + 1, true); + } else { + // we've reached the capacity of the map, so the hash seems to work nice. Keep using it. + rehashPowerOfTwo((mMask + 1) * 2, false); + } + return true; + } + + void nextHashMultiplier() { + // adding an *even* number, so that the multiplier will always stay odd. This is necessary + // so that the hash stays a mixing function (and thus doesn't have any information loss). + mHashMultiplier += UINT64_C(0xc4ceb9fe1a85ec54); + } + + void destroy() { + if (0 == mMask) { + // don't deallocate! + return; + } + + Destroyer::value>{} + .nodesDoNotDeallocate(*this); + + // This protection against not deleting mMask shouldn't be needed as it's sufficiently + // protected with the 0==mMask check, but I have this anyways because g++ 7 otherwise + // reports a compile error: attempt to free a non-heap object 'fm' + // [-Werror=free-nonheap-object] + if (mKeyVals != reinterpret_cast_no_cast_align_warning(&mMask)) { + ROBIN_HOOD_LOG("std::free") + std::free(mKeyVals); + } + } + + void init() noexcept { + mKeyVals = reinterpret_cast_no_cast_align_warning(&mMask); + mInfo = reinterpret_cast(&mMask); + mNumElements = 0; + mMask = 0; + mMaxNumElementsAllowed = 0; + mInfoInc = InitialInfoInc; + mInfoHashShift = InitialInfoHashShift; + } + + // members are sorted so no padding occurs + uint64_t mHashMultiplier = UINT64_C(0xc4ceb9fe1a85ec53); // 8 byte 8 + Node* mKeyVals = reinterpret_cast_no_cast_align_warning(&mMask); // 8 byte 16 + uint8_t* mInfo = reinterpret_cast(&mMask); // 8 byte 24 + size_t mNumElements = 0; // 8 byte 32 + size_t mMask = 0; // 8 byte 40 + size_t mMaxNumElementsAllowed = 0; // 8 byte 48 + InfoType mInfoInc = InitialInfoInc; // 4 byte 52 + InfoType mInfoHashShift = InitialInfoHashShift; // 4 byte 56 + // 16 byte 56 if NodeAllocator +}; + +} // namespace detail + +// map + +template , + typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> +using unordered_flat_map = detail::Table; + +template , + typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> +using unordered_node_map = detail::Table; + +template , + typename KeyEqual = std::equal_to, size_t MaxLoadFactor100 = 80> +using unordered_map = + detail::Table) <= sizeof(size_t) * 6 && + std::is_nothrow_move_constructible>::value && + std::is_nothrow_move_assignable>::value, + MaxLoadFactor100, Key, T, Hash, KeyEqual>; + +// set + +template , typename KeyEqual = std::equal_to, + size_t MaxLoadFactor100 = 80> +using unordered_flat_set = detail::Table; + +template , typename KeyEqual = std::equal_to, + size_t MaxLoadFactor100 = 80> +using unordered_node_set = detail::Table; + +template , typename KeyEqual = std::equal_to, + size_t MaxLoadFactor100 = 80> +using unordered_set = detail::Table < sizeof(Key) <= sizeof(size_t) * 6 && + std::is_nothrow_move_constructible::value && + std::is_nothrow_move_assignable::value, + MaxLoadFactor100, Key, void, Hash, KeyEqual>; + +} // namespace robin_hood +/* *INDENT-ON* */ + +#endif // NAV2_SMAC_PLANNER__THIRDPARTY__ROBIN_HOOD_H_ diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 8624ae2bcdb..772d19e86ed 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.1.2 + 1.1.3 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index deaa597b923..06bc0520d43 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -117,8 +117,11 @@ template typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( const unsigned int & index) { - // Emplace will only create a new object if it doesn't already exist. - // If an element exists, it will return the existing object, not create a new one. + auto iter = _graph.find(index); + if (iter != _graph.end()) { + return &(iter->second); + } + return &(_graph.emplace(index, NodeT(index)).first->second); } @@ -323,6 +326,11 @@ bool AStarAlgorithm::createPath( } } + if (_best_heuristic_node.first < getToleranceHeuristic()) { + // If we run out of serach options, return the path that is closest, if within tolerance. + return _graph.at(_best_heuristic_node.second).backtracePath(path); + } + return false; } diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index 862b1feca68..14a901adedf 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -86,7 +86,9 @@ float Node2D::getHeuristicCost( { // Using Moore distance as it more accurately represents the distances // even a Van Neumann neighborhood robot can navigate. - return hypotf(goal_coordinates.x - node_coords.x, goal_coordinates.y - node_coords.y); + auto dx = goal_coordinates.x - node_coords.x; + auto dy = goal_coordinates.y - node_coords.y; + return std::sqrt(dx * dx + dy * dy); } void Node2D::initMotionModel( @@ -154,13 +156,16 @@ bool Node2D::backtracePath(CoordinateVector & path) NodePtr current_node = this; - do { + while (current_node->parent) { path.push_back( Node2D::getCoords(current_node->getIndex())); current_node = current_node->parent; - } while (current_node->parent); + } + + // add the start pose + path.push_back(Node2D::getCoords(current_node->getIndex())); - return path.size() > 0; + return true; } } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 3ccc17ad0bc..24b32f527ba 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -381,9 +381,9 @@ inline float distanceHeuristic2D( const unsigned int idx, const unsigned int size_x, const unsigned int target_x, const unsigned int target_y) { - return std::hypotf( - static_cast(idx % size_x) - static_cast(target_x), - static_cast(idx / size_x) - static_cast(target_y)); + int dx = static_cast(idx % size_x) - static_cast(target_x); + int dy = static_cast(idx / size_x) - static_cast(target_y); + return std::sqrt(dx * dx + dy * dy); } void NodeHybrid::resetObstacleHeuristic( @@ -705,14 +705,19 @@ bool NodeHybrid::backtracePath(CoordinateVector & path) NodePtr current_node = this; - do { + while (current_node->parent) { path.push_back(current_node->pose); // Convert angle to radians path.back().theta = NodeHybrid::motion_table.getAngleFromBin(path.back().theta); current_node = current_node->parent; - } while (current_node->parent); + } + + // add the start pose + path.push_back(current_node->pose); + // Convert angle to radians + path.back().theta = NodeHybrid::motion_table.getAngleFromBin(path.back().theta); - return path.size() > 0; + return true; } } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index 98e9b519ae9..11fb0a699fb 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -542,44 +542,53 @@ bool NodeLattice::backtracePath(CoordinateVector & path) return false; } - Coordinates initial_pose, prim_pose; NodePtr current_node = this; + + while (current_node->parent) { + addNodeToPath(current_node, path); + current_node = current_node->parent; + } + + // add start to path + addNodeToPath(current_node, path); + + return true; +} + +void NodeLattice::addNodeToPath( + NodeLattice::NodePtr current_node, + NodeLattice::CoordinateVector & path) +{ + Coordinates initial_pose, prim_pose; MotionPrimitive * prim = nullptr; const float & grid_resolution = NodeLattice::motion_table.lattice_metadata.grid_resolution; const float pi_2 = 2.0 * M_PI; - - do { - prim = current_node->getMotionPrimitive(); - // if motion primitive is valid, then was searched (rather than analytically expanded), - // include dense path of subpoints making up the primitive at grid resolution - if (prim) { - initial_pose.x = current_node->pose.x - (prim->poses.back()._x / grid_resolution); - initial_pose.y = current_node->pose.y - (prim->poses.back()._y / grid_resolution); - initial_pose.theta = NodeLattice::motion_table.getAngleFromBin(prim->start_angle); - - for (auto it = prim->poses.crbegin(); it != prim->poses.crend(); ++it) { - // Convert primitive pose into grid space if it should be checked - prim_pose.x = initial_pose.x + (it->_x / grid_resolution); - prim_pose.y = initial_pose.y + (it->_y / grid_resolution); - // If reversing, invert the angle because the robot is backing into the primitive - // not driving forward with it - if (current_node->isBackward()) { - prim_pose.theta = std::fmod(it->_theta + M_PI, pi_2); - } else { - prim_pose.theta = it->_theta; - } - path.push_back(prim_pose); + prim = current_node->getMotionPrimitive(); + // if motion primitive is valid, then was searched (rather than analytically expanded), + // include dense path of subpoints making up the primitive at grid resolution + if (prim) { + initial_pose.x = current_node->pose.x - (prim->poses.back()._x / grid_resolution); + initial_pose.y = current_node->pose.y - (prim->poses.back()._y / grid_resolution); + initial_pose.theta = NodeLattice::motion_table.getAngleFromBin(prim->start_angle); + + for (auto it = prim->poses.crbegin(); it != prim->poses.crend(); ++it) { + // Convert primitive pose into grid space if it should be checked + prim_pose.x = initial_pose.x + (it->_x / grid_resolution); + prim_pose.y = initial_pose.y + (it->_y / grid_resolution); + // If reversing, invert the angle because the robot is backing into the primitive + // not driving forward with it + if (current_node->isBackward()) { + prim_pose.theta = std::fmod(it->_theta + M_PI, pi_2); + } else { + prim_pose.theta = it->_theta; } - } else { - // For analytic expansion nodes where there is no valid motion primitive - path.push_back(current_node->pose); - path.back().theta = NodeLattice::motion_table.getAngleFromBin(path.back().theta); + path.push_back(prim_pose); } - - current_node = current_node->parent; - } while (current_node->parent); - - return path.size() > 0; + } else { + // For analytic expansion nodes where there is no valid motion primitive + path.push_back(current_node->pose); + path.back().theta = NodeLattice::motion_table.getAngleFromBin(path.back().theta); + } } } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 279af1804c3..ee614c7b946 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -80,12 +80,18 @@ void SmacPlannerHybrid::configure( _angle_bin_size = 2.0 * M_PI / angle_quantizations; _angle_quantizations = static_cast(angle_quantizations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".tolerance", rclcpp::ParameterValue(0.25)); + _tolerance = static_cast(node->get_parameter(name + ".tolerance").as_double()); nav2_util::declare_parameter_if_not_declared( node, name + ".allow_unknown", rclcpp::ParameterValue(true)); node->get_parameter(name + ".allow_unknown", _allow_unknown); nav2_util::declare_parameter_if_not_declared( node, name + ".max_iterations", rclcpp::ParameterValue(1000000)); node->get_parameter(name + ".max_iterations", _max_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); + node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations); nav2_util::declare_parameter_if_not_declared( node, name + ".smooth_path", rclcpp::ParameterValue(true)); node->get_parameter(name + ".smooth_path", smooth_path); @@ -139,6 +145,13 @@ void SmacPlannerHybrid::configure( _motion_model_for_search.c_str()); } + if (_max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } + if (_max_iterations <= 0) { RCLCPP_INFO( _logger, "maximum iteration selected as <= 0, " @@ -180,7 +193,7 @@ void SmacPlannerHybrid::configure( _a_star->initialize( _allow_unknown, _max_iterations, - std::numeric_limits::max(), + _max_on_approach_iterations, _max_planning_time, _lookup_table_dim, _angle_quantizations); @@ -205,10 +218,11 @@ void SmacPlannerHybrid::configure( RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlannerHybrid with " - "maximum iterations %i, and %s. Using motion model: %s.", - _name.c_str(), _max_iterations, + "maximum iterations %i, max on approach iterations %i, and %s. Tolerance %.2f." + "Using motion model: %s.", + _name.c_str(), _max_iterations, _max_on_approach_iterations, _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", - toString(_motion_model).c_str()); + _tolerance, toString(_motion_model).c_str()); } void SmacPlannerHybrid::activate() @@ -315,7 +329,9 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( int num_iterations = 0; std::string error; try { - if (!_a_star->createPath(path, num_iterations, 0.0)) { + if (!_a_star->createPath( + path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) + { if (num_iterations < _a_star->getMaxIterations()) { error = std::string("no valid path found"); } else { @@ -392,6 +408,8 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para if (name == _name + ".max_planning_time") { reinit_a_star = true; _max_planning_time = parameter.as_double(); + } else if (name == _name + ".tolerance") { + _tolerance = static_cast(parameter.as_double()); } else if (name == _name + ".lookup_table_size") { reinit_a_star = true; _lookup_table_size = parameter.as_double(); @@ -452,6 +470,15 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "disabling maximum iterations."); _max_iterations = std::numeric_limits::max(); } + } else if (name == _name + ".max_on_approach_iterations") { + reinit_a_star = true; + _max_on_approach_iterations = parameter.as_int(); + if (_max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } } else if (name == _name + ".angle_quantization_bins") { reinit_collision_checker = true; reinit_a_star = true; @@ -504,7 +531,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para _a_star->initialize( _allow_unknown, _max_iterations, - std::numeric_limits::max(), + _max_on_approach_iterations, _max_planning_time, _lookup_table_dim, _angle_quantizations); diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 694f52e4659..2fdfe5e7f3c 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -65,12 +65,18 @@ void SmacPlannerLattice::configure( double analytic_expansion_max_length_m; bool smooth_path; + nav2_util::declare_parameter_if_not_declared( + node, name + ".tolerance", rclcpp::ParameterValue(0.25)); + _tolerance = static_cast(node->get_parameter(name + ".tolerance").as_double()); nav2_util::declare_parameter_if_not_declared( node, name + ".allow_unknown", rclcpp::ParameterValue(true)); node->get_parameter(name + ".allow_unknown", _allow_unknown); nav2_util::declare_parameter_if_not_declared( node, name + ".max_iterations", rclcpp::ParameterValue(1000000)); node->get_parameter(name + ".max_iterations", _max_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); + node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations); nav2_util::declare_parameter_if_not_declared( node, name + ".smooth_path", rclcpp::ParameterValue(true)); node->get_parameter(name + ".smooth_path", smooth_path); @@ -127,6 +133,13 @@ void SmacPlannerLattice::configure( _metadata.min_turning_radius / (_costmap->getResolution()); _motion_model = MotionModel::STATE_LATTICE; + if (_max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } + if (_max_iterations <= 0) { RCLCPP_INFO( _logger, "maximum iteration selected as <= 0, " @@ -168,7 +181,7 @@ void SmacPlannerLattice::configure( _a_star->initialize( _allow_unknown, _max_iterations, - std::numeric_limits::max(), + _max_on_approach_iterations, _max_planning_time, lookup_table_dim, _metadata.number_of_headings); @@ -183,11 +196,11 @@ void SmacPlannerLattice::configure( RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlannerLattice with " - "maximum iterations %i, " - "and %s. Using motion model: %s. State lattice file: %s.", - _name.c_str(), _max_iterations, + "maximum iterations %i, max on approach iterations %i, " + "and %s. Tolerance %.2f. Using motion model: %s. State lattice file: %s.", + _name.c_str(), _max_iterations, _max_on_approach_iterations, _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", - toString(_motion_model).c_str(), _search_info.lattice_filepath.c_str()); + _tolerance, toString(_motion_model).c_str(), _search_info.lattice_filepath.c_str()); } void SmacPlannerLattice::activate() @@ -263,7 +276,9 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( int num_iterations = 0; std::string error; try { - if (!_a_star->createPath(path, num_iterations, 0 /*no tolerance*/)) { + if (!_a_star->createPath( + path, num_iterations, _tolerance / static_cast(_costmap->getResolution()))) + { if (num_iterations < _a_star->getMaxIterations()) { error = std::string("no valid path found"); } else { @@ -350,6 +365,8 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par if (name == _name + ".max_planning_time") { reinit_a_star = true; _max_planning_time = parameter.as_double(); + } else if (name == _name + ".tolerance") { + _tolerance = static_cast(parameter.as_double()); } else if (name == _name + ".lookup_table_size") { reinit_a_star = true; _lookup_table_size = parameter.as_double(); @@ -404,6 +421,15 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _max_iterations = std::numeric_limits::max(); } } + } else if (name == _name + ".max_on_approach_iterations") { + reinit_a_star = true; + _max_on_approach_iterations = parameter.as_int(); + if (_max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } } else if (type == ParameterType::PARAMETER_STRING) { if (name == _name + ".lattice_filepath") { reinit_a_star = true; @@ -454,7 +480,7 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _a_star->initialize( _allow_unknown, _max_iterations, - std::numeric_limits::max(), + _max_on_approach_iterations, _max_planning_time, lookup_table_dim, _metadata.number_of_headings); diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 00852e5298e..72a6ba29683 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -72,7 +72,7 @@ TEST(AStarTest, test_a_star_2d) EXPECT_EQ(num_it, 2414); // check path is the right size and collision free - EXPECT_EQ(path.size(), 81u); + EXPECT_EQ(path.size(), 82u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -104,7 +104,7 @@ TEST(AStarTest, test_a_star_2d) a_star_2.setStart(20, 20, 0); // valid a_star_2.setGoal(50, 50, 0); // invalid EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance)); - EXPECT_EQ(path.size(), 20u); + EXPECT_EQ(path.size(), 21u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -164,7 +164,7 @@ TEST(AStarTest, test_a_star_se2) // check path is the right size and collision free EXPECT_EQ(num_it, 3222); - EXPECT_EQ(path.size(), 62u); + EXPECT_EQ(path.size(), 63u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -225,7 +225,7 @@ TEST(AStarTest, test_a_star_lattice) // check path is the right size and collision free EXPECT_EQ(num_it, 21); - EXPECT_EQ(path.size(), 48u); + EXPECT_EQ(path.size(), 49u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index 3eb077cfb0f..3be1a9e897f 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -110,12 +110,14 @@ TEST(SmacTest, test_smac_se2_reconfigure) rclcpp::Parameter("test.change_penalty", 1.0), rclcpp::Parameter("test.non_straight_penalty", 2.0), rclcpp::Parameter("test.cost_penalty", 2.0), + rclcpp::Parameter("test.tolerance", 0.2), rclcpp::Parameter("test.retrospective_penalty", 0.2), rclcpp::Parameter("test.analytic_expansion_ratio", 4.0), rclcpp::Parameter("test.max_planning_time", 10.0), rclcpp::Parameter("test.lookup_table_size", 30.0), rclcpp::Parameter("test.smooth_path", false), rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), + rclcpp::Parameter("test.max_on_approach_iterations", 42), rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP"))}); rclcpp::spin_until_future_complete( @@ -134,11 +136,13 @@ TEST(SmacTest, test_smac_se2_reconfigure) EXPECT_EQ(nodeSE2->get_parameter("test.non_straight_penalty").as_double(), 2.0); EXPECT_EQ(nodeSE2->get_parameter("test.cost_penalty").as_double(), 2.0); EXPECT_EQ(nodeSE2->get_parameter("test.retrospective_penalty").as_double(), 0.2); + EXPECT_EQ(nodeSE2->get_parameter("test.tolerance").as_double(), 0.2); EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_ratio").as_double(), 4.0); EXPECT_EQ(nodeSE2->get_parameter("test.smooth_path").as_bool(), false); EXPECT_EQ(nodeSE2->get_parameter("test.max_planning_time").as_double(), 10.0); EXPECT_EQ(nodeSE2->get_parameter("test.lookup_table_size").as_double(), 30.0); EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_max_length").as_double(), 42.0); + EXPECT_EQ(nodeSE2->get_parameter("test.max_on_approach_iterations").as_int(), 42); EXPECT_EQ( nodeSE2->get_parameter("test.motion_model_for_search").as_string(), std::string("REEDS_SHEPP")); diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index ff813f08c17..9ce99a46b1c 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -124,7 +124,9 @@ TEST(SmacTest, test_smac_lattice_reconfigure) rclcpp::Parameter("test.lookup_table_size", 30.0), rclcpp::Parameter("test.smooth_path", false), rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), + rclcpp::Parameter("test.tolerance", 42.0), rclcpp::Parameter("test.rotation_penalty", 42.0), + rclcpp::Parameter("test.max_on_approach_iterations", 42), rclcpp::Parameter("test.allow_reverse_expansion", true)}); try { diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt index 0df85f54bdb..0ab7c83628f 100644 --- a/nav2_smoother/CMakeLists.txt +++ b/nav2_smoother/CMakeLists.txt @@ -68,8 +68,18 @@ add_library(simple_smoother SHARED ament_target_dependencies(simple_smoother ${dependencies} ) + +# Savitzky Golay Smoother plugin +add_library(savitzky_golay_smoother SHARED + src/savitzky_golay_smoother.cpp +) +ament_target_dependencies(savitzky_golay_smoother + ${dependencies} +) + pluginlib_export_plugin_description_file(nav2_core plugins.xml) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -82,7 +92,7 @@ endif() rclcpp_components_register_nodes(${library_name} "nav2_smoother::SmootherServer") install( - TARGETS ${library_name} simple_smoother + TARGETS ${library_name} simple_smoother savitzky_golay_smoother ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -104,6 +114,6 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(${library_name} simple_smoother) +ament_export_libraries(${library_name} simple_smoother savitzky_golay_smoother) ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_smoother/README.md b/nav2_smoother/README.md index da007a9096f..c612dba43cd 100644 --- a/nav2_smoother/README.md +++ b/nav2_smoother/README.md @@ -7,3 +7,5 @@ A smoothing module implementing the `nav2_behavior_tree::SmoothPath` interface i See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available smoother plugins. See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-smoother-server.html) for additional parameter descriptions. + +This package contains the Simple Smoother and Savitzky-Golay Smoother plugins. diff --git a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp new file mode 100644 index 00000000000..090aa07af85 --- /dev/null +++ b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp @@ -0,0 +1,108 @@ +// Copyright (c) 2022, Samsung Research America +// +// 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. Reserved. + +#ifndef NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_ +#define NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "nav2_core/smoother.hpp" +#include "nav2_smoother/smoother_utils.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav_msgs/msg/path.hpp" +#include "angles/angles.h" +#include "tf2/utils.h" + +namespace nav2_smoother +{ + +/** + * @class nav2_smoother::SavitzkyGolaySmoother + * @brief A path smoother implementation using Savitzky Golay filters + */ +class SavitzkyGolaySmoother : public nav2_core::Smoother +{ +public: + /** + * @brief A constructor for nav2_smoother::SavitzkyGolaySmoother + */ + SavitzkyGolaySmoother() = default; + + /** + * @brief A destructor for nav2_smoother::SavitzkyGolaySmoother + */ + ~SavitzkyGolaySmoother() override = default; + + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, + std::string name, std::shared_ptr, + std::shared_ptr, + std::shared_ptr) override; + + /** + * @brief Method to cleanup resources. + */ + void cleanup() override {} + + /** + * @brief Method to activate smoother and any threads involved in execution. + */ + void activate() override {} + + /** + * @brief Method to deactivate smoother and any threads involved in execution. + */ + void deactivate() override {} + + /** + * @brief Method to smooth given path + * + * @param path In-out path to be smoothed + * @param max_time Maximum duration smoothing should take + * @return If smoothing was completed (true) or interrupted by time limit (false) + */ + bool smooth( + nav_msgs::msg::Path & path, + const rclcpp::Duration & max_time) override; + +protected: + /** + * @brief Smoother method - does the smoothing on a segment + * @param path Reference to path + * @param reversing_segment Return if this is a reversing segment + * @param costmap Pointer to minimal costmap + * @param max_time Maximum time to compute, stop early if over limit + * @return If smoothing was successful + */ + bool smoothImpl( + nav_msgs::msg::Path & path, + bool & reversing_segment); + + bool do_refinement_; + int refinement_num_; + rclcpp::Logger logger_{rclcpp::get_logger("SGSmoother")}; +}; + +} // namespace nav2_smoother + +#endif // NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_ diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index 4b841b3c09e..95f19dc54c9 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -24,6 +24,7 @@ #include #include "nav2_core/smoother.hpp" +#include "nav2_smoother/smoother_utils.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/geometry_utils.hpp" @@ -35,19 +36,6 @@ namespace nav2_smoother { -/** - * @class nav2_smoother::PathSegment - * @brief A segment of a path in start/end indices - */ -struct PathSegment -{ - unsigned int start; - unsigned int end; -}; - -typedef std::vector::iterator PathIterator; -typedef std::vector::reverse_iterator ReversePathIterator; - /** * @class nav2_smoother::SimpleSmoother * @brief A path smoother implementation @@ -132,23 +120,6 @@ class SimpleSmoother : public nav2_core::Smoother geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, const double & value); - /** - * @brief Finds the starting and end indices of path segments where - * the robot is traveling in the same direction (e.g. forward vs reverse) - * @param path Path in which to look for cusps - * @return Set of index pairs for each segment of the path in a given direction - */ - std::vector findDirectionalPathSegments(const nav_msgs::msg::Path & path); - - /** - * @brief For a given path, update the path point orientations based on smoothing - * @param path Path to approximate the path orientation in - * @param reversing_segment Return if this is a reversing segment - */ - inline void updateApproximatePathOrientations( - nav_msgs::msg::Path & path, - bool & reversing_segment); - double tolerance_, data_w_, smooth_w_; int max_its_, refinement_ctr_; bool do_refinement_; diff --git a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp new file mode 100644 index 00000000000..67d7296353f --- /dev/null +++ b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp @@ -0,0 +1,132 @@ +// Copyright (c) 2022, Samsung Research America +// +// 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. Reserved. + +#ifndef NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_ +#define NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "nav2_core/smoother.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav_msgs/msg/path.hpp" +#include "angles/angles.h" +#include "tf2/utils.h" + +namespace smoother_utils +{ + +/** + * @class nav2_smoother::PathSegment + * @brief A segment of a path in start/end indices + */ +struct PathSegment +{ + unsigned int start; + unsigned int end; +}; + +typedef std::vector::iterator PathIterator; +typedef std::vector::reverse_iterator ReversePathIterator; + +inline std::vector findDirectionalPathSegments( + const nav_msgs::msg::Path & path) +{ + std::vector segments; + PathSegment curr_segment; + curr_segment.start = 0; + + // Iterating through the path to determine the position of the cusp + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + double oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + double oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + double ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + double ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + // Checking for the existance of cusp, in the path, using the dot product. + double dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0) { + curr_segment.end = idx; + segments.push_back(curr_segment); + curr_segment.start = idx; + } + + // Checking for the existance of a differential rotation in place. + double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation); + double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation); + double dtheta = angles::shortest_angular_distance(cur_theta, next_theta); + if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) { + curr_segment.end = idx; + segments.push_back(curr_segment); + curr_segment.start = idx; + } + } + + curr_segment.end = path.poses.size() - 1; + segments.push_back(curr_segment); + return segments; +} + +inline void updateApproximatePathOrientations( + nav_msgs::msg::Path & path, + bool & reversing_segment) +{ + double dx, dy, theta, pt_yaw; + reversing_segment = false; + + // Find if this path segment is in reverse + dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x; + dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y; + theta = atan2(dy, dx); + pt_yaw = tf2::getYaw(path.poses[1].pose.orientation); + if (fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) { + reversing_segment = true; + } + + // Find the angle relative the path position vectors + for (unsigned int i = 0; i != path.poses.size() - 1; i++) { + dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x; + dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y; + theta = atan2(dy, dx); + + // If points are overlapping, pass + if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) { + continue; + } + + // Flip the angle if this path segment is in reverse + if (reversing_segment) { + theta += M_PI; // orientationAroundZAxis will normalize + } + + path.poses[i].pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta); + } +} + +} // namespace smoother_utils + +#endif // NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_ diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index f9860a6cc46..413a9b012af 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.1.2 + 1.1.3 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_smoother/plugins.xml b/nav2_smoother/plugins.xml index c72f7c25c88..90960a51efc 100644 --- a/nav2_smoother/plugins.xml +++ b/nav2_smoother/plugins.xml @@ -4,4 +4,10 @@ Does a simple smoothing process with collision checking + + + + Does Savitzky-Golay smoothing + + diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index ada1f664b02..83590304bff 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -1,5 +1,6 @@ // Copyright (c) 2019 RoboTech Vision // Copyright (c) 2019 Intel Corporation +// Copyright (c) 2022 Samsung Research America // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp new file mode 100644 index 00000000000..6a61196ab32 --- /dev/null +++ b/nav2_smoother/src/savitzky_golay_smoother.cpp @@ -0,0 +1,198 @@ +// Copyright (c) 2022, Samsung Research America +// +// 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. Reserved. + +#include +#include +#include "nav2_smoother/savitzky_golay_smoother.hpp" + +namespace nav2_smoother +{ + +using namespace smoother_utils; // NOLINT +using namespace nav2_util::geometry_utils; // NOLINT +using namespace std::chrono; // NOLINT +using nav2_util::declare_parameter_if_not_declared; + +void SavitzkyGolaySmoother::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr/*tf*/, + std::shared_ptr/*costmap_sub*/, + std::shared_ptr/*footprint_sub*/) +{ + auto node = parent.lock(); + logger_ = node->get_logger(); + + declare_parameter_if_not_declared( + node, name + ".do_refinement", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, name + ".refinement_num", rclcpp::ParameterValue(2)); + node->get_parameter(name + ".do_refinement", do_refinement_); + node->get_parameter(name + ".refinement_num", refinement_num_); +} + +bool SavitzkyGolaySmoother::smooth( + nav_msgs::msg::Path & path, + const rclcpp::Duration & max_time) +{ + steady_clock::time_point start = steady_clock::now(); + double time_remaining = max_time.seconds(); + + bool success = true, reversing_segment; + nav_msgs::msg::Path curr_path_segment; + curr_path_segment.header = path.header; + + std::vector path_segments = findDirectionalPathSegments(path); + + for (unsigned int i = 0; i != path_segments.size(); i++) { + if (path_segments[i].end - path_segments[i].start > 9) { + // Populate path segment + curr_path_segment.poses.clear(); + std::copy( + path.poses.begin() + path_segments[i].start, + path.poses.begin() + path_segments[i].end + 1, + std::back_inserter(curr_path_segment.poses)); + + // Make sure we're still able to smooth with time remaining + steady_clock::time_point now = steady_clock::now(); + time_remaining = max_time.seconds() - duration_cast>(now - start).count(); + + if (time_remaining <= 0.0) { + RCLCPP_WARN( + logger_, + "Smoothing time exceeded allowed duration of %0.2f.", max_time.seconds()); + return false; + } + + // Smooth path segment + success = success && smoothImpl(curr_path_segment, reversing_segment); + + // Assemble the path changes to the main path + std::copy( + curr_path_segment.poses.begin(), + curr_path_segment.poses.end(), + path.poses.begin() + path_segments[i].start); + } + } + + return success; +} + +bool SavitzkyGolaySmoother::smoothImpl( + nav_msgs::msg::Path & path, + bool & reversing_segment) +{ + // Must be at least 10 in length to enter function + const unsigned int & path_size = path.poses.size(); + + // 7-point SG filter + const std::array filter = { + -2.0 / 21.0, + 3.0 / 21.0, + 6.0 / 21.0, + 7.0 / 21.0, + 6.0 / 21.0, + 3.0 / 21.0, + -2.0 / 21.0}; + + auto applyFilter = [&](const std::vector & data) + -> geometry_msgs::msg::Point + { + geometry_msgs::msg::Point val; + for (unsigned int i = 0; i != filter.size(); i++) { + val.x += filter[i] * data[i].x; + val.y += filter[i] * data[i].y; + } + return val; + }; + + auto applyFilterOverAxes = + [&](std::vector & plan_pts) -> void + { + // Handle initial boundary conditions, first point is fixed + unsigned int idx = 1; + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 1].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 2].pose.position, + plan_pts[idx + 3].pose.position}); + + idx++; + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 2].pose.position, + plan_pts[idx - 2].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 2].pose.position, + plan_pts[idx + 3].pose.position}); + + // Apply nominal filter + for (idx = 3; idx < path_size - 4; ++idx) { + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 3].pose.position, + plan_pts[idx - 2].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 2].pose.position, + plan_pts[idx + 3].pose.position}); + } + + // Handle terminal boundary conditions, last point is fixed + idx++; + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 3].pose.position, + plan_pts[idx - 2].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 2].pose.position, + plan_pts[idx + 2].pose.position}); + + idx++; + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 3].pose.position, + plan_pts[idx - 2].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 1].pose.position}); + }; + + applyFilterOverAxes(path.poses); + + // Lets do additional refinement, it shouldn't take more than a couple milliseconds + if (do_refinement_) { + for (int i = 0; i < refinement_num_; i++) { + applyFilterOverAxes(path.poses); + } + } + + updateApproximatePathOrientations(path, reversing_segment); + return true; +} + +} // namespace nav2_smoother + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_smoother::SavitzkyGolaySmoother, nav2_core::Smoother) diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 59a81fd3b11..d04d88bc61b 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -18,6 +18,7 @@ namespace nav2_smoother { +using namespace smoother_utils; // NOLINT using namespace nav2_util::geometry_utils; // NOLINT using namespace std::chrono; // NOLINT using nav2_util::declare_parameter_if_not_declared; @@ -214,85 +215,6 @@ void SimpleSmoother::setFieldByDim( } } -std::vector SimpleSmoother::findDirectionalPathSegments( - const nav_msgs::msg::Path & path) -{ - std::vector segments; - PathSegment curr_segment; - curr_segment.start = 0; - - // Iterating through the path to determine the position of the cusp - for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { - // We have two vectors for the dot product OA and AB. Determining the vectors. - double oa_x = path.poses[idx].pose.position.x - - path.poses[idx - 1].pose.position.x; - double oa_y = path.poses[idx].pose.position.y - - path.poses[idx - 1].pose.position.y; - double ab_x = path.poses[idx + 1].pose.position.x - - path.poses[idx].pose.position.x; - double ab_y = path.poses[idx + 1].pose.position.y - - path.poses[idx].pose.position.y; - - // Checking for the existance of cusp, in the path, using the dot product. - double dot_product = (oa_x * ab_x) + (oa_y * ab_y); - if (dot_product < 0.0) { - curr_segment.end = idx; - segments.push_back(curr_segment); - curr_segment.start = idx; - } - - // Checking for the existance of a differential rotation in place. - double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation); - double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation); - double dtheta = angles::shortest_angular_distance(cur_theta, next_theta); - if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) { - curr_segment.end = idx; - segments.push_back(curr_segment); - curr_segment.start = idx; - } - } - - curr_segment.end = path.poses.size() - 1; - segments.push_back(curr_segment); - return segments; -} - -void SimpleSmoother::updateApproximatePathOrientations( - nav_msgs::msg::Path & path, - bool & reversing_segment) -{ - double dx, dy, theta, pt_yaw; - reversing_segment = false; - - // Find if this path segment is in reverse - dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x; - dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y; - theta = atan2(dy, dx); - pt_yaw = tf2::getYaw(path.poses[1].pose.orientation); - if (fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) { - reversing_segment = true; - } - - // Find the angle relative the path position vectors - for (unsigned int i = 0; i != path.poses.size() - 1; i++) { - dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x; - dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y; - theta = atan2(dy, dx); - - // If points are overlapping, pass - if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) { - continue; - } - - // Flip the angle if this path segment is in reverse - if (reversing_segment) { - theta += M_PI; // orientationAroundZAxis will normalize - } - - path.poses[i].pose.orientation = orientationAroundZAxis(theta); - } -} - } // namespace nav2_smoother #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_smoother/test/CMakeLists.txt b/nav2_smoother/test/CMakeLists.txt index 67c98e3e43e..623447b5f92 100644 --- a/nav2_smoother/test/CMakeLists.txt +++ b/nav2_smoother/test/CMakeLists.txt @@ -21,3 +21,16 @@ target_link_libraries(test_simple_smoother ament_target_dependencies(test_simple_smoother ${dependencies} ) + + +ament_add_gtest(test_savitzky_golay_smoother + test_savitzky_golay_smoother.cpp +) + +target_link_libraries(test_savitzky_golay_smoother + savitzky_golay_smoother +) + +ament_target_dependencies(test_savitzky_golay_smoother + ${dependencies} +) diff --git a/nav2_smoother/test/test_savitzky_golay_smoother.cpp b/nav2_smoother/test/test_savitzky_golay_smoother.cpp new file mode 100644 index 00000000000..58860d67db9 --- /dev/null +++ b/nav2_smoother/test/test_savitzky_golay_smoother.cpp @@ -0,0 +1,332 @@ +// Copyright (c) 2022, Samsung Research America +// +// 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. Reserved. + +#include +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_msgs/msg/costmap.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_smoother/savitzky_golay_smoother.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +using namespace smoother_utils; // NOLINT +using namespace nav2_smoother; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(SmootherTest, test_sg_smoother_basics) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = + std::make_shared("SmacSGSmootherTest"); + + std::shared_ptr costmap_msg = + std::make_shared(); + costmap_msg->header.stamp = node->now(); + costmap_msg->header.frame_id = "map"; + costmap_msg->data.resize(100 * 100); + costmap_msg->metadata.resolution = 0.05; + costmap_msg->metadata.size_x = 100; + costmap_msg->metadata.size_y = 100; + + std::weak_ptr parent = node; + std::shared_ptr dummy_costmap; + dummy_costmap = std::make_shared(parent, "dummy_topic"); + dummy_costmap->costmapCallback(costmap_msg); + + // Make smoother + std::shared_ptr dummy_tf; + std::shared_ptr dummy_footprint; + node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false)); + auto smoother = std::make_unique(); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + smoother->activate(); + rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds + + // Test regular path, should see no effective change + nav_msgs::msg::Path straight_regular_path, straight_regular_path_baseline; + straight_regular_path.header.frame_id = "map"; + straight_regular_path.header.stamp = node->now(); + straight_regular_path.poses.resize(11); + straight_regular_path.poses[0].pose.position.x = 0.5; + straight_regular_path.poses[0].pose.position.y = 0.1; + straight_regular_path.poses[1].pose.position.x = 0.5; + straight_regular_path.poses[1].pose.position.y = 0.2; + straight_regular_path.poses[2].pose.position.x = 0.5; + straight_regular_path.poses[2].pose.position.y = 0.3; + straight_regular_path.poses[3].pose.position.x = 0.5; + straight_regular_path.poses[3].pose.position.y = 0.4; + straight_regular_path.poses[4].pose.position.x = 0.5; + straight_regular_path.poses[4].pose.position.y = 0.5; + straight_regular_path.poses[5].pose.position.x = 0.5; + straight_regular_path.poses[5].pose.position.y = 0.6; + straight_regular_path.poses[6].pose.position.x = 0.5; + straight_regular_path.poses[6].pose.position.y = 0.7; + straight_regular_path.poses[7].pose.position.x = 0.5; + straight_regular_path.poses[7].pose.position.y = 0.8; + straight_regular_path.poses[8].pose.position.x = 0.5; + straight_regular_path.poses[8].pose.position.y = 0.9; + straight_regular_path.poses[9].pose.position.x = 0.5; + straight_regular_path.poses[9].pose.position.y = 1.0; + straight_regular_path.poses[10].pose.position.x = 0.5; + straight_regular_path.poses[10].pose.position.y = 1.1; + straight_regular_path_baseline = straight_regular_path; + + EXPECT_TRUE(smoother->smooth(straight_regular_path, max_time)); + for (uint i = 0; i != straight_regular_path.poses.size() - 1; i++) { + // Check distances are still the same + EXPECT_NEAR( + fabs( + straight_regular_path.poses[i].pose.position.y - + straight_regular_path_baseline.poses[i].pose.position.y), 0.0, 0.011); + } + + // Attempt smoothing with no time given, should fail + rclcpp::Duration no_time = rclcpp::Duration::from_seconds(-1.0); // 0 seconds + EXPECT_FALSE(smoother->smooth(straight_regular_path, no_time)); + + smoother->deactivate(); + smoother->cleanup(); +} + +TEST(SmootherTest, test_sg_smoother_noisey_path) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = + std::make_shared("SmacSGSmootherTest"); + + std::shared_ptr costmap_msg = + std::make_shared(); + costmap_msg->header.stamp = node->now(); + costmap_msg->header.frame_id = "map"; + costmap_msg->data.resize(100 * 100); + costmap_msg->metadata.resolution = 0.05; + costmap_msg->metadata.size_x = 100; + costmap_msg->metadata.size_y = 100; + + std::weak_ptr parent = node; + std::shared_ptr dummy_costmap; + dummy_costmap = std::make_shared(parent, "dummy_topic"); + dummy_costmap->costmapCallback(costmap_msg); + + // Make smoother + std::shared_ptr dummy_tf; + std::shared_ptr dummy_footprint; + node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false)); + auto smoother = std::make_unique(); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds + + // Given nominal irregular/noisey path, test that the output is shorter and smoother + nav_msgs::msg::Path noisey_path, noisey_path_baseline; + noisey_path.header.frame_id = "map"; + noisey_path.header.stamp = node->now(); + noisey_path.poses.resize(11); + noisey_path.poses[0].pose.position.x = 0.5; + noisey_path.poses[0].pose.position.y = 0.1; + noisey_path.poses[1].pose.position.x = 0.5; + noisey_path.poses[1].pose.position.y = 0.2; + noisey_path.poses[2].pose.position.x = 0.5; + noisey_path.poses[2].pose.position.y = 0.3; + noisey_path.poses[3].pose.position.x = 0.5; + noisey_path.poses[3].pose.position.y = 0.4; + noisey_path.poses[4].pose.position.x = 0.5; + noisey_path.poses[4].pose.position.y = 0.5; + noisey_path.poses[5].pose.position.x = 0.5; + noisey_path.poses[5].pose.position.y = 0.6; + noisey_path.poses[6].pose.position.x = 0.5; + noisey_path.poses[6].pose.position.y = 0.7; + noisey_path.poses[7].pose.position.x = 0.5; + noisey_path.poses[7].pose.position.y = 0.8; + noisey_path.poses[8].pose.position.x = 0.5; + noisey_path.poses[8].pose.position.y = 0.9; + noisey_path.poses[9].pose.position.x = 0.5; + noisey_path.poses[9].pose.position.y = 1.0; + noisey_path.poses[10].pose.position.x = 0.5; + noisey_path.poses[10].pose.position.y = 1.1; + + // Add random but deterministic noises + std::random_device rd{}; + std::mt19937 gen{rd()}; + std::normal_distribution<> normal_distribution{0.0, 0.02}; + for (unsigned int i = 0; i != noisey_path.poses.size(); i++) { + auto noise = normal_distribution(gen); + noisey_path.poses[i].pose.position.x += noise; + } + + noisey_path_baseline = noisey_path; + EXPECT_TRUE(smoother->smooth(noisey_path, max_time)); + + // Compute metric, should be shorter if smoother + double length = 0; + double base_length = 0; + for (unsigned int i = 0; i != noisey_path.poses.size() - 1; i++) { + length += std::hypot( + noisey_path.poses[i + 1].pose.position.x - noisey_path.poses[i].pose.position.x, + noisey_path.poses[i + 1].pose.position.y - noisey_path.poses[i].pose.position.y); + base_length += std::hypot( + noisey_path_baseline.poses[i + 1].pose.position.x - + noisey_path_baseline.poses[i].pose.position.x, + noisey_path_baseline.poses[i + 1].pose.position.y - + noisey_path_baseline.poses[i].pose.position.y); + } + + EXPECT_LT(length, base_length); + + // Test again with refinement, even shorter and smoother + node->set_parameter(rclcpp::Parameter("test.do_refinement", rclcpp::ParameterValue(true))); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + nav_msgs::msg::Path noisey_path_refined = noisey_path_baseline; + EXPECT_TRUE(smoother->smooth(noisey_path_refined, max_time)); + + length = 0; + double non_refined_length = 0; + for (unsigned int i = 0; i != noisey_path.poses.size() - 1; i++) { + length += std::hypot( + noisey_path_refined.poses[i + 1].pose.position.x - + noisey_path_refined.poses[i].pose.position.x, + noisey_path_refined.poses[i + 1].pose.position.y - + noisey_path_refined.poses[i].pose.position.y); + non_refined_length += std::hypot( + noisey_path.poses[i + 1].pose.position.x - noisey_path_baseline.poses[i].pose.position.x, + noisey_path.poses[i + 1].pose.position.y - noisey_path_baseline.poses[i].pose.position.y); + } + + EXPECT_LT(length, base_length); +} + +TEST(SmootherTest, test_sg_smoother_reversing) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = + std::make_shared("SmacSGSmootherTest"); + + std::shared_ptr costmap_msg = + std::make_shared(); + costmap_msg->header.stamp = node->now(); + costmap_msg->header.frame_id = "map"; + costmap_msg->data.resize(100 * 100); + costmap_msg->metadata.resolution = 0.05; + costmap_msg->metadata.size_x = 100; + costmap_msg->metadata.size_y = 100; + + std::weak_ptr parent = node; + std::shared_ptr dummy_costmap; + dummy_costmap = std::make_shared(parent, "dummy_topic"); + dummy_costmap->costmapCallback(costmap_msg); + + // Make smoother + std::shared_ptr dummy_tf; + std::shared_ptr dummy_footprint; + node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false)); + auto smoother = std::make_unique(); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds + + // Test reversing / multiple segments via a cusp + nav_msgs::msg::Path cusp_path, cusp_path_baseline; + cusp_path.header.frame_id = "map"; + cusp_path.header.stamp = node->now(); + cusp_path.poses.resize(22); + cusp_path.poses[0].pose.position.x = 0.5; + cusp_path.poses[0].pose.position.y = 0.1; + cusp_path.poses[1].pose.position.x = 0.5; + cusp_path.poses[1].pose.position.y = 0.2; + cusp_path.poses[2].pose.position.x = 0.5; + cusp_path.poses[2].pose.position.y = 0.3; + cusp_path.poses[3].pose.position.x = 0.5; + cusp_path.poses[3].pose.position.y = 0.4; + cusp_path.poses[4].pose.position.x = 0.5; + cusp_path.poses[4].pose.position.y = 0.5; + cusp_path.poses[5].pose.position.x = 0.5; + cusp_path.poses[5].pose.position.y = 0.6; + cusp_path.poses[6].pose.position.x = 0.5; + cusp_path.poses[6].pose.position.y = 0.7; + cusp_path.poses[7].pose.position.x = 0.5; + cusp_path.poses[7].pose.position.y = 0.8; + cusp_path.poses[8].pose.position.x = 0.5; + cusp_path.poses[8].pose.position.y = 0.9; + cusp_path.poses[9].pose.position.x = 0.5; + cusp_path.poses[9].pose.position.y = 1.0; + cusp_path.poses[10].pose.position.x = 0.5; + cusp_path.poses[10].pose.position.y = 1.1; + cusp_path.poses[11].pose.position.x = 0.5; + cusp_path.poses[11].pose.position.y = 1.0; + cusp_path.poses[12].pose.position.x = 0.5; + cusp_path.poses[12].pose.position.y = 0.9; + cusp_path.poses[13].pose.position.x = 0.5; + cusp_path.poses[13].pose.position.y = 0.8; + cusp_path.poses[14].pose.position.x = 0.5; + cusp_path.poses[14].pose.position.y = 0.7; + cusp_path.poses[15].pose.position.x = 0.5; + cusp_path.poses[15].pose.position.y = 0.6; + cusp_path.poses[16].pose.position.x = 0.5; + cusp_path.poses[16].pose.position.y = 0.5; + cusp_path.poses[17].pose.position.x = 0.5; + cusp_path.poses[17].pose.position.y = 0.4; + cusp_path.poses[18].pose.position.x = 0.5; + cusp_path.poses[18].pose.position.y = 0.3; + cusp_path.poses[19].pose.position.x = 0.5; + cusp_path.poses[19].pose.position.y = 0.2; + cusp_path.poses[20].pose.position.x = 0.5; + cusp_path.poses[20].pose.position.y = 0.1; + cusp_path.poses[21].pose.position.x = 0.5; + cusp_path.poses[21].pose.position.y = 0.0; + + // Add random but deterministic noises + std::random_device rd{}; + std::mt19937 gen{rd()}; + std::normal_distribution<> normal_distribution{0.0, 0.02}; + for (unsigned int i = 0; i != cusp_path.poses.size(); i++) { + auto noise = normal_distribution(gen); + cusp_path.poses[i].pose.position.x += noise; + } + + cusp_path_baseline = cusp_path; + + EXPECT_TRUE(smoother->smooth(cusp_path, max_time)); + + // If it detected the cusp, the cusp point should be fixed + EXPECT_EQ(cusp_path.poses[10].pose.position.x, cusp_path_baseline.poses[10].pose.position.x); + EXPECT_EQ(cusp_path.poses[10].pose.position.y, cusp_path_baseline.poses[10].pose.position.y); + + // But the path also should be smoother / shorter + double length = 0; + double base_length = 0; + for (unsigned int i = 0; i != cusp_path.poses.size() - 1; i++) { + length += std::hypot( + cusp_path.poses[i + 1].pose.position.x - cusp_path.poses[i].pose.position.x, + cusp_path.poses[i + 1].pose.position.y - cusp_path.poses[i].pose.position.y); + base_length += std::hypot( + cusp_path_baseline.poses[i + 1].pose.position.x - + cusp_path_baseline.poses[i].pose.position.x, + cusp_path_baseline.poses[i + 1].pose.position.y - + cusp_path_baseline.poses[i].pose.position.y); + } + + EXPECT_LT(length, base_length); +} diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp index fccc2c2a7c5..7d9b56d457e 100644 --- a/nav2_smoother/test/test_simple_smoother.cpp +++ b/nav2_smoother/test/test_simple_smoother.cpp @@ -28,6 +28,7 @@ #include "nav2_smoother/simple_smoother.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" +using namespace smoother_utils; // NOLINT using namespace nav2_smoother; // NOLINT using namespace std::chrono_literals; // NOLINT diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 6cc6a32e496..6d56df1ef7e 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -65,6 +65,7 @@ if(BUILD_TESTING) add_subdirectory(src/behaviors/wait) add_subdirectory(src/behaviors/backup) add_subdirectory(src/behaviors/drive_on_heading) + add_subdirectory(src/behaviors/assisted_teleop) add_subdirectory(src/costmap_filters) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index cc27d2802a3..6d3044d3805 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.1.2 + 1.1.3 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index b5bb56a6f1d..d7ede55cd18 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -58,6 +58,7 @@ class BehaviorTreeHandler "nav2_follow_path_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", + "nav2_assisted_teleop_action_bt_node", "nav2_back_up_action_bt_node", "nav2_drive_on_heading_bt_node", "nav2_clear_costmap_service_bt_node", @@ -91,6 +92,7 @@ class BehaviorTreeHandler "nav2_goal_checker_selector_bt_node", "nav2_controller_cancel_bt_node", "nav2_path_longer_on_approach_bt_node", + "nav2_assisted_teleop_cancel_bt_node", "nav2_wait_cancel_bt_node", "nav2_spin_cancel_bt_node", "nav2_back_up_cancel_bt_node", diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt b/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt new file mode 100644 index 00000000000..c2210b6fcb2 --- /dev/null +++ b/nav2_system_tests/src/behaviors/assisted_teleop/CMakeLists.txt @@ -0,0 +1,23 @@ +set(test_assisted_teleop_behavior test_assisted_teleop_behavior_node) + +ament_add_gtest_executable(${test_assisted_teleop_behavior} + test_assisted_teleop_behavior_node.cpp + assisted_teleop_behavior_tester.cpp +) + +ament_target_dependencies(${test_assisted_teleop_behavior} + ${dependencies} +) + +ament_add_test(test_assisted_teleop_behavior + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_assisted_teleop_behavior_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_EXECUTABLE=$ + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp new file mode 100644 index 00000000000..bcc1950bde0 --- /dev/null +++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp @@ -0,0 +1,277 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2018 Intel Corporation +// +// 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. Reserved. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "assisted_teleop_behavior_tester.hpp" +#include "nav2_util/geometry_utils.hpp" + +using namespace std::chrono_literals; +using namespace std::chrono; // NOLINT + +namespace nav2_system_tests +{ + +AssistedTeleopBehaviorTester::AssistedTeleopBehaviorTester() +: is_active_(false), + initial_pose_received_(false) +{ + node_ = rclcpp::Node::make_shared("assisted_teleop_behavior_test"); + + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + client_ptr_ = rclcpp_action::create_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), + "assisted_teleop"); + + initial_pose_pub_ = + node_->create_publisher("initialpose", 10); + + preempt_pub_ = + node_->create_publisher("preempt_teleop", 10); + + cmd_vel_pub_ = + node_->create_publisher("cmd_vel_teleop", 10); + + subscription_ = node_->create_subscription( + "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&AssistedTeleopBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); + + filtered_vel_sub_ = node_->create_subscription( + "cmd_vel", + rclcpp::SystemDefaultsQoS(), + std::bind(&AssistedTeleopBehaviorTester::filteredVelCallback, this, std::placeholders::_1)); + + std::string costmap_topic = "/local_costmap/costmap_raw"; + std::string footprint_topic = "/local_costmap/published_footprint"; + + costmap_sub_ = std::make_shared( + node_, + costmap_topic); + + footprint_sub_ = std::make_shared( + node_, + footprint_topic, + *tf_buffer_); + + collision_checker_ = std::make_unique( + *costmap_sub_, + *footprint_sub_ + ); + + stamp_ = node_->now(); +} + +AssistedTeleopBehaviorTester::~AssistedTeleopBehaviorTester() +{ + if (is_active_) { + deactivate(); + } +} + +void AssistedTeleopBehaviorTester::activate() +{ + if (is_active_) { + throw std::runtime_error("Trying to activate while already active"); + return; + } + + while (!initial_pose_received_) { + RCLCPP_WARN(node_->get_logger(), "Initial pose not received"); + sendInitialPose(); + std::this_thread::sleep_for(100ms); + rclcpp::spin_some(node_); + } + + // Wait for lifecycle_manager_navigation to activate behavior_server + std::this_thread::sleep_for(10s); + + if (!client_ptr_) { + RCLCPP_ERROR(node_->get_logger(), "Action client not initialized"); + is_active_ = false; + return; + } + + if (!client_ptr_->wait_for_action_server(10s)) { + RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting"); + is_active_ = false; + return; + } + + RCLCPP_INFO(this->node_->get_logger(), "Assisted Teleop action server is ready"); + is_active_ = true; +} + +void AssistedTeleopBehaviorTester::deactivate() +{ + if (!is_active_) { + throw std::runtime_error("Trying to deactivate while already inactive"); + } + is_active_ = false; +} + +bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest( + const float lin_vel, + const float ang_vel) +{ + if (!is_active_) { + RCLCPP_ERROR(node_->get_logger(), "Not activated"); + return false; + } + + RCLCPP_INFO(node_->get_logger(), "Sending goal"); + + auto goal_handle_future = client_ptr_->async_send_goal(nav2_msgs::action::AssistedTeleop::Goal()); + + if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + return false; + } + + // Wait for the server to be done with the goal + auto result_future = client_ptr_->async_get_result(goal_handle); + + rclcpp::Rate r(1); + + counter_ = 0; + auto start_time = std::chrono::system_clock::now(); + while (rclcpp::ok()) { + geometry_msgs::msg::Twist cmd_vel = geometry_msgs::msg::Twist(); + cmd_vel.linear.x = lin_vel; + cmd_vel.angular.z = ang_vel; + cmd_vel_pub_->publish(cmd_vel); + + if (counter_ > 1) { + break; + } + + auto current_time = std::chrono::system_clock::now(); + if (current_time - start_time > 25s) { + RCLCPP_ERROR(node_->get_logger(), "Exceeded Timeout"); + return false; + } + + rclcpp::spin_some(node_); + r.sleep(); + } + + auto preempt_msg = std_msgs::msg::Empty(); + preempt_pub_->publish(preempt_msg); + + RCLCPP_INFO(node_->get_logger(), "Waiting for result"); + if (rclcpp::spin_until_future_complete(node_, result_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::WrappedResult + wrapped_result = result_future.get(); + + switch (wrapped_result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: break; + case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was aborted"); + return false; + case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was canceled"); + return false; + default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); + return false; + } + + RCLCPP_INFO(node_->get_logger(), "result received"); + + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) { + RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); + return false; + } + + geometry_msgs::msg::Pose2D pose_2d; + pose_2d.x = current_pose.pose.position.x; + pose_2d.y = current_pose.pose.position.y; + pose_2d.theta = tf2::getYaw(current_pose.pose.orientation); + + if (!collision_checker_->isCollisionFree(pose_2d)) { + RCLCPP_ERROR(node_->get_logger(), "Ended in collision"); + return false; + } + + return true; +} + +void AssistedTeleopBehaviorTester::sendInitialPose() +{ + geometry_msgs::msg::PoseWithCovarianceStamped pose; + pose.header.frame_id = "map"; + pose.header.stamp = stamp_; + pose.pose.pose.position.x = -2.0; + pose.pose.pose.position.y = -0.5; + pose.pose.pose.position.z = 0.0; + pose.pose.pose.orientation.x = 0.0; + pose.pose.pose.orientation.y = 0.0; + pose.pose.pose.orientation.z = 0.0; + pose.pose.pose.orientation.w = 1.0; + for (int i = 0; i < 35; i++) { + pose.pose.covariance[i] = 0.0; + } + pose.pose.covariance[0] = 0.08; + pose.pose.covariance[7] = 0.08; + pose.pose.covariance[35] = 0.05; + + initial_pose_pub_->publish(pose); + RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); +} + +void AssistedTeleopBehaviorTester::amclPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) +{ + initial_pose_received_ = true; +} + +void AssistedTeleopBehaviorTester::filteredVelCallback( + geometry_msgs::msg::Twist::SharedPtr msg) +{ + if (msg->linear.x == 0.0f) { + counter_++; + } else { + counter_ = 0; + } +} + +} // namespace nav2_system_tests diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp new file mode 100644 index 00000000000..2cf46827e89 --- /dev/null +++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp @@ -0,0 +1,104 @@ +// Copyright (c) 2020 Samsung Research +// Copyright (c) 2018 Intel Corporation +// +// 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. Reserved. + +#ifndef BEHAVIORS__ASSISTED_TELEOP__ASSISTED_TELEOP_BEHAVIOR_TESTER_HPP_ +#define BEHAVIORS__ASSISTED_TELEOP__ASSISTED_TELEOP_BEHAVIOR_TESTER_HPP_ + +#include +#include +#include +#include +#include + +#include "angles/angles.h" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp" +#include "nav2_msgs/action/assisted_teleop.hpp" +#include "nav2_util/node_thread.hpp" +#include "nav2_util/robot_utils.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/empty.hpp" + +#include "tf2/utils.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +namespace nav2_system_tests +{ + +class AssistedTeleopBehaviorTester +{ +public: + using AssistedTeleop = nav2_msgs::action::AssistedTeleop; + + AssistedTeleopBehaviorTester(); + ~AssistedTeleopBehaviorTester(); + + // Runs a single test with given target yaw + bool defaultAssistedTeleopTest( + const float lin_vel, + const float ang_vel); + + void activate(); + + void deactivate(); + + bool isActive() const + { + return is_active_; + } + +private: + void sendInitialPose(); + + void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr); + + void filteredVelCallback(geometry_msgs::msg::Twist::SharedPtr msg); + + unsigned int counter_; + bool is_active_; + bool initial_pose_received_; + rclcpp::Time stamp_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + rclcpp::Node::SharedPtr node_; + + // Publishers + rclcpp::Publisher::SharedPtr initial_pose_pub_; + rclcpp::Publisher::SharedPtr preempt_pub_; + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + + // Subscribers + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Subscription::SharedPtr filtered_vel_sub_; + + // Action client to call AssistedTeleop action + rclcpp_action::Client::SharedPtr client_ptr_; + + // collision checking + std::shared_ptr costmap_sub_; + std::shared_ptr footprint_sub_; + std::unique_ptr collision_checker_; +}; + +} // namespace nav2_system_tests + +#endif // BEHAVIORS__ASSISTED_TELEOP__ASSISTED_TELEOP_BEHAVIOR_TESTER_HPP_ diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py new file mode 100755 index 00000000000..976eb02c2fc --- /dev/null +++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py @@ -0,0 +1,103 @@ +#! /usr/bin/env python3 +# Copyright (c) 2012 Samsung Research America +# +# 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. + +import os +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + map_yaml_file = os.getenv('TEST_MAP') + world = os.getenv('TEST_WORLD') + + bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML')) + + bringup_dir = get_package_share_directory('nav2_bringup') + params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') + + # Replace the `use_astar` setting on the params file + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites='', + convert_types=True) + + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + + # Launch gazebo server for simulation + ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '--minimal_comms', world], + output='screen'), + + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), + + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'autostart': 'True'}.items()), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + testExecutable = os.getenv('TEST_EXECUTABLE') + + test1_action = ExecuteProcess( + cmd=[testExecutable], + name='test_assisted_teleop_behavior_node', + output='screen') + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_node.cpp b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_node.cpp new file mode 100644 index 00000000000..eb847272eb8 --- /dev/null +++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_node.cpp @@ -0,0 +1,114 @@ +// Copyright (c) 2020 Samsung Research +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2022 Joshua Wallace +// +// 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. Reserved. + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "assisted_teleop_behavior_tester.hpp" +#include "nav2_msgs/action/back_up.hpp" + +using namespace std::chrono_literals; + +using nav2_system_tests::AssistedTeleopBehaviorTester; + +struct TestParameters +{ + float lin_vel; + float ang_vel; +}; + + +std::string testNameGenerator(const testing::TestParamInfo &) +{ + static int test_index = 0; + std::string name = "AssistedTeleopTest" + std::to_string(test_index); + ++test_index; + return name; +} + +class AssistedTeleopBehaviorTestFixture + : public ::testing::TestWithParam +{ +public: + static void SetUpTestCase() + { + assisted_teleop_behavior_tester = new AssistedTeleopBehaviorTester(); + if (!assisted_teleop_behavior_tester->isActive()) { + assisted_teleop_behavior_tester->activate(); + } + } + + static void TearDownTestCase() + { + delete assisted_teleop_behavior_tester; + assisted_teleop_behavior_tester = nullptr; + } + +protected: + static AssistedTeleopBehaviorTester * assisted_teleop_behavior_tester; +}; + +AssistedTeleopBehaviorTester * +AssistedTeleopBehaviorTestFixture::assisted_teleop_behavior_tester = nullptr; + +TEST_P(AssistedTeleopBehaviorTestFixture, testAssistedTeleopBehavior) +{ + auto test_params = GetParam(); + + if (!assisted_teleop_behavior_tester->isActive()) { + assisted_teleop_behavior_tester->activate(); + } + + bool success = false; + success = assisted_teleop_behavior_tester->defaultAssistedTeleopTest( + test_params.lin_vel, + test_params.ang_vel); + + EXPECT_TRUE(success); +} + +std::vector test_params = {TestParameters{-0.1, 0.0}, + TestParameters{0.35, 0.05}}; + +INSTANTIATE_TEST_SUITE_P( + TestAssistedTeleopBehavior, + AssistedTeleopBehaviorTestFixture, + ::testing::Values( + test_params[0], + test_params[1]), + testNameGenerator +); + + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 22b3b14e6be..8a3c9511c54 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -56,6 +56,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -88,6 +89,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node bt_navigator_navigate_through_poses_rclcpp_node: diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index ff0de1abdaa..ea274b64463 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -56,6 +56,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -89,6 +90,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node bt_navigator_navigate_through_poses_rclcpp_node: diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index a0a2024abb2..c3ea076693e 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -56,6 +56,7 @@ bt_navigator: - nav2_follow_path_action_bt_node - nav2_spin_action_bt_node - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node - nav2_back_up_action_bt_node - nav2_drive_on_heading_bt_node - nav2_clear_costmap_service_bt_node @@ -89,6 +90,7 @@ bt_navigator: - nav2_wait_cancel_bt_node - nav2_spin_cancel_bt_node - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node - nav2_drive_on_heading_cancel_bt_node bt_navigator_navigate_through_poses_rclcpp_node: diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index edaf5e5299e..5409acd6627 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -75,9 +75,9 @@ This planner uses the costs associated with each cell from the `global_costmap` Providing a gentle potential field over the region allows the planner to exchange the increase in path lengths for a decrease in the accumulated traversal cost, thus leading to an increase in the distance from the obstacles. This around a corner allows for naturally smoothing the turns and removes the requirement for an external path smoother. -`w_heuristic_cost` is an internal setting that is not user changable. It has been provided to have an admissible heuristic, restricting its value to the minimum of `w_euc_cost` and `1.0` to make sure the heuristic and travel costs are admissible and consistent. +`w_heuristic_cost` is an internal setting that is not user changeable. It has been provided to have an admissible heuristic, restricting its value to the minimum of `w_euc_cost` and `1.0` to make sure the heuristic and travel costs are admissible and consistent. -To begin with, you can set the parameters to its default values and then try increasing the value of `w_traversal_cost` to achieve those middling paths, but this would adversly make the paths less taut. So it is recommend ed that you simultaneously tune the value of `w_euc_cost`. Increasing `w_euc_cost` increases the tautness of the path, which leads to more straight line like paths (any-angled paths). Do note that the query time from the planner would increase for higher values of `w_traversal_cost` as more nodes would have to be expanded to lower the cost of the path, to counteract this you may also try setting `w_euc_cost` to a higher value and check if it reduces the query time. +To begin with, you can set the parameters to its default values and then try increasing the value of `w_traversal_cost` to achieve those middling paths, but this would adversely make the paths less taut. So it is recommended that you simultaneously tune the value of `w_euc_cost`. Increasing `w_euc_cost` increases the tautness of the path, which leads to more straight line like paths (any-angled paths). Do note that the query time from the planner would increase for higher values of `w_traversal_cost` as more nodes would have to be expanded to lower the cost of the path, to counteract this you may also try setting `w_euc_cost` to a higher value and check if it reduces the query time. Also note that changes to `w_traversal_cost` might cause slow downs, in case the number of node expanisions increase thus tuning it along with `w_euc_cost` is the way to go to. diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 936dfcdd812..4e30bbae542 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.2 + 1.1.3 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 0aba4a4dfe1..63dd80a0ca8 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.1.2 + 1.1.3 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index 35043d5df2f..26c64b1c7dc 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -75,12 +75,12 @@ class FibonacciServerNode : public rclcpp::Node void on_term() { // when nothing's running make sure everything's dead. - const std::shared_ptr a = action_server_->accept_pending_goal(); - const std::shared_ptr b = action_server_->get_current_goal(); - assert(a == b); - assert(action_server_->is_cancel_requested() == false); - auto feedback = std::make_shared(); - action_server_->publish_feedback(feedback); + // const std::shared_ptr a = action_server_->accept_pending_goal(); + // const std::shared_ptr b = action_server_->get_current_goal(); + // assert(a == b); + // assert(action_server_->is_cancel_requested() == false); + // auto feedback = std::make_shared(); + // action_server_->publish_feedback(feedback); action_server_.reset(); } diff --git a/nav2_velocity_smoother/README.md b/nav2_velocity_smoother/README.md index bdd55277888..6ffbe5d372e 100644 --- a/nav2_velocity_smoother/README.md +++ b/nav2_velocity_smoother/README.md @@ -3,7 +3,7 @@ The ``nav2_velocity_smoother`` is a package containing a lifecycle-component node for smoothing velocities sent by Nav2 to robot controllers. The aim of this package is to implement velocity, acceleration, and deadband smoothing from Nav2 to reduce wear-and-tear on robot motors and hardware controllers by smoothing out the accelerations/jerky movements that might be present with some local trajectory planners' control efforts. -It supports differential drive and omnidirectional robot platforms primarily, but is applicable to ackermann as well with some intepretations of ``Twist``. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). +It supports differential drive and omnidirectional robot platforms primarily, but is applicable to ackermann as well with some interpretations of ``Twist``. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-velocity-smoother.html) for additional parameter descriptions. @@ -39,7 +39,7 @@ There are two primary operation modes: open and closed loop. In open-loop, the node assumes that the robot was able to achieve the velocity send to it in the last command which was smoothed (which should be a good assumption if acceleration limits are set properly). This is useful when robot odometry is not particularly accurate or has significant latency relative to `smoothing_frequency` so there isn't a delay in the feedback loop. In closed-loop, the node will read from the odometry topic and apply a smoother over it to obtain the robot's current speed. -This will be used to determine the robot's current velocity and therefore achivable velocity targets by the velocity, acceleration, and deadband constraints using live data. +This will be used to determine the robot's current velocity and therefore achievable velocity targets by the velocity, acceleration, and deadband constraints using live data. ## Parameters diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 72991bfa6db..ba5b283840f 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.1.2 + 1.1.3 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 bcda3cb7709..f7769dc1ef0 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.1.2 + 1.1.3 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 e6cf58c43b8..a25b16b109b 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.1.2 + 1.1.3 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 7c4f73fd21c..378b2202be4 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.1.2 + 1.1.3 ROS2 Navigation Stack @@ -16,6 +16,7 @@ nav2_amcl nav2_behavior_tree nav2_bt_navigator + nav2_collision_monitor nav2_constrained_smoother nav2_controller nav2_core diff --git a/tools/code_coverage_report.bash b/tools/code_coverage_report.bash index f4510238c65..a1e963a1df5 100755 --- a/tools/code_coverage_report.bash +++ b/tools/code_coverage_report.bash @@ -60,7 +60,7 @@ INCLUDE_PACKAGES=$( # Capture executed code data. fastcov --lcov \ -d build \ - --exclude test/ $EXCLUDE_PACKAGES \ + --exclude test/ $EXCLUDE_PACKAGES thirdparty/ \ --include $INCLUDE_PACKAGES \ --process-gcno \ --validate-sources \ diff --git a/tools/planner_benchmarking/100by100_10.yaml b/tools/planner_benchmarking/100by100_10.yaml new file mode 100644 index 00000000000..104cb9a6fdf --- /dev/null +++ b/tools/planner_benchmarking/100by100_10.yaml @@ -0,0 +1,6 @@ +image: 100by100_10.pgm +resolution: 0.05 +origin: [0.0, 0.000000, 0.000000] +negate: 1 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/tools/planner_benchmarking/100by100_15.yaml b/tools/planner_benchmarking/100by100_15.yaml new file mode 100644 index 00000000000..82277cf15b3 --- /dev/null +++ b/tools/planner_benchmarking/100by100_15.yaml @@ -0,0 +1,6 @@ +image: 100by100_15.pgm +resolution: 0.05 +origin: [0.0, 0.000000, 0.000000] +negate: 1 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/tools/planner_benchmarking/100by100_20.yaml b/tools/planner_benchmarking/100by100_20.yaml new file mode 100644 index 00000000000..763431b9e4e --- /dev/null +++ b/tools/planner_benchmarking/100by100_20.yaml @@ -0,0 +1,6 @@ +image: 100by100_20.pgm +resolution: 0.05 +origin: [0.0, 0.000000, 0.000000] +negate: 1 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/tools/planner_benchmarking/README.md b/tools/planner_benchmarking/README.md new file mode 100644 index 00000000000..c10e44003d7 --- /dev/null +++ b/tools/planner_benchmarking/README.md @@ -0,0 +1,30 @@ +# Planning Benchmark + +This experiment runs a set of planners over randomly generated maps, with randomly generated goals for objective benchmarking. + +To use, modify the Nav2 bringup parameters to include the planners of interest: + +``` +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["SmacHybrid", "Smac2d", "SmacLattice", "Navfn", "ThetaStar"] + SmacHybrid: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + Smac2d: + plugin: "nav2_smac_planner/SmacPlanner2D" + SmacLattice: + plugin: "nav2_smac_planner/SmacPlannerLattice" + Navfn: + plugin: "nav2_navfn_planner/NavfnPlanner" + ThetaStar: + plugin: "nav2_theta_star_planner/ThetaStarPlanner" +``` + +Set global costmap settings to those desired for benchmarking. The global map will be automatically set in the script. Inside of `metrics.py`, you can modify the map or set of planners to use. + +Launch the benchmark via `ros2 launch ./planning_benchmark_bringup.py` to launch the planner and map servers, then run each script in this directory: + +- `metrics.py` to capture data in `.pickle` files. +- `process_data.py` to take the metric files and process them into key results (and plots) diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py index 3865b0bbdbe..c4b556e5d76 100644 --- a/tools/planner_benchmarking/metrics.py +++ b/tools/planner_benchmarking/metrics.py @@ -21,6 +21,8 @@ import math import os import pickle +import glob +import time import numpy as np from random import seed @@ -29,15 +31,15 @@ from transforms3d.euler import euler2quat -# Note: Map origin is assumed to be (0,0) - def getPlannerResults(navigator, initial_pose, goal_pose, planners): results = [] for planner in planners: - path = navigator.getPath(initial_pose, goal_pose, planner) + path = navigator._getPathImpl(initial_pose, goal_pose, planner, use_start=True) if path is not None: results.append(path) + else: + return results return results @@ -98,25 +100,17 @@ def main(): navigator = BasicNavigator() - # Set our experiment's initial pose - initial_pose = PoseStamped() - initial_pose.header.frame_id = 'map' - initial_pose.header.stamp = navigator.get_clock().now().to_msg() - initial_pose.pose.position.x = 1.0 - initial_pose.pose.position.y = 1.0 - initial_pose.pose.orientation.z = 0.0 - initial_pose.pose.orientation.w = 1.0 - navigator.setInitialPose(initial_pose) - - # Wait for navigation to fully activate - navigator.waitUntilNav2Active() + # Set map to use, other options: 100by100_15, 100by100_10 + map_path = os.getcwd() + '/' + glob.glob('**/100by100_20.yaml', recursive=True)[0] + navigator.changeMap(map_path) + time.sleep(2) # Get the costmap for start/goal validation costmap_msg = navigator.getGlobalCostmap() costmap = np.asarray(costmap_msg.data) costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x) - planners = ['NavFn', 'Smac2d', 'SmacLattice', 'SmacHybrid'] + planners = ['Navfn', 'ThetaStar', 'SmacHybrid', 'Smac2d', 'SmacLattice'] max_cost = 210 side_buffer = 100 time_stamp = navigator.get_clock().now().to_msg() @@ -125,7 +119,8 @@ def main(): random_pairs = 100 res = costmap_msg.metadata.resolution - for i in range(random_pairs): + i = 0 + while len(results) != random_pairs: print("Cycle: ", i, "out of: ", random_pairs) start = getRandomStart(costmap, max_cost, side_buffer, time_stamp, res) goal = getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res) @@ -134,22 +129,20 @@ def main(): result = getPlannerResults(navigator, start, goal, planners) if len(result) == len(planners): results.append(result) + i = i + 1 else: print("One of the planners was invalid") print("Write Results...") - nav2_planner_metrics_dir = get_package_share_directory('nav2_planner_metrics') - with open(os.path.join(nav2_planner_metrics_dir, 'results.pickle'), 'wb') as f: + with open(os.getcwd() + '/results.pickle', 'wb+') as f: pickle.dump(results, f, pickle.HIGHEST_PROTOCOL) - with open(os.path.join(nav2_planner_metrics_dir, 'costmap.pickle'), 'wb') as f: + with open(os.getcwd() + '/costmap.pickle', 'wb+') as f: pickle.dump(costmap_msg, f, pickle.HIGHEST_PROTOCOL) - with open(os.path.join(nav2_planner_metrics_dir, 'planners.pickle'), 'wb') as f: + with open(os.getcwd() + '/planners.pickle', 'wb+') as f: pickle.dump(planners, f, pickle.HIGHEST_PROTOCOL) print("Write Complete") - - navigator.lifecycleShutdown() exit(0) diff --git a/tools/planner_benchmarking/planning_benchmark_bringup.py b/tools/planner_benchmarking/planning_benchmark_bringup.py new file mode 100644 index 00000000000..e2a251405d3 --- /dev/null +++ b/tools/planner_benchmarking/planning_benchmark_bringup.py @@ -0,0 +1,73 @@ +# Copyright (c) 2022 Samsung Research America +# +# 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. + +import os + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + config = os.path.join(get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml') + map_file = os.path.join(nav2_bringup_dir, 'maps', 'map.yaml') + lifecycle_nodes = ['map_server', 'planner_server'] + + return LaunchDescription([ + Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[{'use_sim_time': True}, + {'yaml_filename': map_file}, + {'topic_name': "map"}]), + + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + parameters=[config]), + + Node( + package = 'tf2_ros', + executable = 'static_transform_publisher', + output = 'screen', + arguments = ["0", "0", "0", "0", "0", "0", "base_link", "map"]), + + Node( + package = 'tf2_ros', + executable = 'static_transform_publisher', + output = 'screen', + arguments = ["0", "0", "0", "0", "0", "0", "base_link", "odom"]), + + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + parameters=[{'use_sim_time': True}, + {'autostart': True}, + {'node_names': lifecycle_nodes}]), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + ]) diff --git a/tools/planner_benchmarking/process_data.py b/tools/planner_benchmarking/process_data.py index 443da00266f..50be77cc488 100644 --- a/tools/planner_benchmarking/process_data.py +++ b/tools/planner_benchmarking/process_data.py @@ -126,15 +126,14 @@ def maxPathCost(paths, costmap, num_of_planners): def main(): - nav2_planner_metrics_dir = get_package_share_directory('nav2_planner_metrics') print("Read data") - with open(os.path.join(nav2_planner_metrics_dir, 'results.pickle'), 'rb') as f: + with open(os.getcwd() + '/results.pickle', 'rb') as f: results = pickle.load(f) - with open(os.path.join(nav2_planner_metrics_dir, 'planners.pickle'), 'rb') as f: + with open(os.getcwd() + '/planners.pickle', 'rb') as f: planners = pickle.load(f) - with open(os.path.join(nav2_planner_metrics_dir, 'costmap.pickle'), 'rb') as f: + with open(os.getcwd() + '/costmap.pickle', 'rb') as f: costmap = pickle.load(f) paths = getPaths(results) diff --git a/tools/smoother_benchmarking/README.md b/tools/smoother_benchmarking/README.md new file mode 100644 index 00000000000..65ea8a4c9da --- /dev/null +++ b/tools/smoother_benchmarking/README.md @@ -0,0 +1,98 @@ +# Planners Smoothing Benchmark + +This experiment runs a set with randomly generated goals for objective benchmarking. + +Bechmarking scripts require the following python packages to be installed: + +``` +pip install transforms3d +pip install seaborn +pip install tabulate +``` + +To use the suite, modify the Nav2 bringup parameters `nav2_params.yaml` to include selected path planner: + +``` +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: ["SmacHybrid"] + SmacHybrid: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + tolerance: 0.5 + motion_model_for_search: "DUBIN" # default, non-reverse motion + smooth_path: false # should be disabled for experiment + analytic_expansion_max_length: 0.3 # decreased to avoid robot jerking +``` + +... and path smoothers for benchmark: + +``` + smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother", "constrained_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + constrained_smoother: + plugin: "nav2_constrained_smoother/ConstrainedSmoother" + w_smooth: 100000.0 # tuned +``` + +Set global costmap, path planner and smoothers parameters to those desired in `nav2_params.yaml`. +Inside of `metrics.py`, you can change reference path planner / path smoothers to use. + +For the benchmarking purposes, the clarification of execution time may be made for planner and smoother servers, to reduce impacts caused by other system actions outside of the planning / smoothing algorithm (optional): + +``` +diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp +index c7a90bcb..6f93edbf 100644 +--- a/nav2_planner/src/planner_server.cpp ++++ b/nav2_planner/src/planner_server.cpp +@@ -381,7 +381,10 @@ void PlannerServer::computePlanThroughPoses() + } + + // Get plan from start -> goal ++ auto planning_start = steady_clock_.now(); + nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); ++ auto planning_duration = steady_clock_.now() - planning_start; ++ result->planning_time = planning_duration; + + if (!validatePath(curr_goal, curr_path, goal->planner_id)) { + throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); +@@ -398,7 +401,7 @@ void PlannerServer::computePlanThroughPoses() + publishPlan(result->path); + + auto cycle_duration = steady_clock_.now() - start_time; +- result->planning_time = cycle_duration; ++ // result->planning_time = cycle_duration; + + if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { + RCLCPP_WARN( +diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp +index ada1f664..610e9512 100644 +--- a/nav2_smoother/src/nav2_smoother.cpp ++++ b/nav2_smoother/src/nav2_smoother.cpp +@@ -253,8 +253,6 @@ bool SmootherServer::findSmootherId( + + void SmootherServer::smoothPlan() + { +- auto start_time = steady_clock_.now(); +- + RCLCPP_INFO(get_logger(), "Received a path to smooth."); + + auto result = std::make_shared(); +@@ -271,6 +269,8 @@ void SmootherServer::smoothPlan() + // Perform smoothing + auto goal = action_server_->get_current_goal(); + result->path = goal->path; ++ ++ auto start_time = steady_clock_.now(); + result->was_completed = smoothers_[current_smoother_]->smooth( + result->path, goal->max_smoothing_duration); + result->smoothing_duration = steady_clock_.now() - start_time; +``` + +Then execute the benchmarking: + +- `ros2 launch ./smoother_benchmark_bringup.py` to launch the nav2 stack and path smoothers benchmarking +- `python3 ./process_data.py` to take the metric files and process them into key results (and plots) diff --git a/tools/smoother_benchmarking/maps/smoothers_world.pgm b/tools/smoother_benchmarking/maps/smoothers_world.pgm new file mode 100644 index 00000000000..a79c5cd79e3 Binary files /dev/null and b/tools/smoother_benchmarking/maps/smoothers_world.pgm differ diff --git a/tools/smoother_benchmarking/maps/smoothers_world.yaml b/tools/smoother_benchmarking/maps/smoothers_world.yaml new file mode 100644 index 00000000000..c0931662030 --- /dev/null +++ b/tools/smoother_benchmarking/maps/smoothers_world.yaml @@ -0,0 +1,6 @@ +image: smoothers_world.pgm +resolution: 0.050000 +origin: [0.0, 0.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/tools/smoother_benchmarking/metrics.py b/tools/smoother_benchmarking/metrics.py new file mode 100644 index 00000000000..6b576b08a9c --- /dev/null +++ b/tools/smoother_benchmarking/metrics.py @@ -0,0 +1,157 @@ +#! /usr/bin/env python3 +# Copyright (c) 2022 Samsung R&D Institute Russia +# Copyright (c) 2022 Joshua Wallace +# +# 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. + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator +import rclpy + +import math +import os +import pickle +import numpy as np + +from random import seed +from random import randint +from random import uniform + +from transforms3d.euler import euler2quat + + +# Note: Map origin is assumed to be (0,0) + +def getPlannerResults(navigator, initial_pose, goal_pose, planner): + return navigator._getPathImpl(initial_pose, goal_pose, planner, use_start=True) + +def getSmootherResults(navigator, path, smoothers): + smoothed_results = [] + for smoother in smoothers: + smoothed_result = navigator._smoothPathImpl(path, smoother) + if smoothed_result is not None: + smoothed_results.append(smoothed_result) + else: + print(smoother, " failed to smooth the path") + return None + return smoothed_results + +def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res): + start = PoseStamped() + start.header.frame_id = 'map' + start.header.stamp = time_stamp + while True: + row = randint(side_buffer, costmap.shape[0]-side_buffer) + col = randint(side_buffer, costmap.shape[1]-side_buffer) + + if costmap[row, col] < max_cost: + start.pose.position.x = col*res + start.pose.position.y = row*res + + yaw = uniform(0, 1) * 2*math.pi + quad = euler2quat(0.0, 0.0, yaw) + start.pose.orientation.w = quad[0] + start.pose.orientation.x = quad[1] + start.pose.orientation.y = quad[2] + start.pose.orientation.z = quad[3] + break + return start + +def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res): + goal = PoseStamped() + goal.header.frame_id = 'map' + goal.header.stamp = time_stamp + while True: + row = randint(side_buffer, costmap.shape[0]-side_buffer) + col = randint(side_buffer, costmap.shape[1]-side_buffer) + + start_x = start.pose.position.x + start_y = start.pose.position.y + goal_x = col*res + goal_y = row*res + x_diff = goal_x - start_x + y_diff = goal_y - start_y + dist = math.sqrt(x_diff ** 2 + y_diff ** 2) + + if costmap[row, col] < max_cost and dist > 3.0: + goal.pose.position.x = goal_x + goal.pose.position.y = goal_y + + yaw = uniform(0, 1) * 2*math.pi + quad = euler2quat(0.0, 0.0, yaw) + goal.pose.orientation.w = quad[0] + goal.pose.orientation.x = quad[1] + goal.pose.orientation.y = quad[2] + goal.pose.orientation.z = quad[3] + break + return goal + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Wait for planner and smoother to fully activate + print("Waiting for planner and smoother servers to activate") + navigator.waitUntilNav2Active('smoother_server', 'planner_server') + + # Get the costmap for start/goal validation + costmap_msg = navigator.getGlobalCostmap() + costmap = np.asarray(costmap_msg.data) + costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x) + + planner = 'SmacHybrid' + smoothers = ['simple_smoother', 'constrained_smoother'] + max_cost = 210 + side_buffer = 10 + time_stamp = navigator.get_clock().now().to_msg() + results = [] + seed(33) + + random_pairs = 100 + i = 0 + res = costmap_msg.metadata.resolution + while i < random_pairs: + print("Cycle: ", i, "out of: ", random_pairs) + start = getRandomStart(costmap, max_cost, side_buffer, time_stamp, res) + goal = getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res) + print("Start", start) + print("Goal", goal) + result = getPlannerResults(navigator, start, goal, planner) + if result is not None: + smoothed_results = getSmootherResults(navigator, result.path, smoothers) + if smoothed_results is not None: + results.append(result) + results.append(smoothed_results) + i += 1 + else: + print(planner, " planner failed to produce the path") + + print("Write Results...") + benchmark_dir = os.getcwd() + with open(os.path.join(benchmark_dir, 'results.pickle'), 'wb') as f: + pickle.dump(results, f, pickle.HIGHEST_PROTOCOL) + + with open(os.path.join(benchmark_dir, 'costmap.pickle'), 'wb') as f: + pickle.dump(costmap_msg, f, pickle.HIGHEST_PROTOCOL) + + smoothers.insert(0, planner) + with open(os.path.join(benchmark_dir, 'methods.pickle'), 'wb') as f: + pickle.dump(smoothers, f, pickle.HIGHEST_PROTOCOL) + print("Write Complete") + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/tools/smoother_benchmarking/process_data.py b/tools/smoother_benchmarking/process_data.py new file mode 100644 index 00000000000..edc0dad8111 --- /dev/null +++ b/tools/smoother_benchmarking/process_data.py @@ -0,0 +1,302 @@ +#! /usr/bin/env python3 +# Copyright (c) 2022 Samsung R&D Institute Russia +# Copyright (c) 2022 Joshua Wallace +# Copyright (c) 2021 RoboTech Vision +# +# 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. + +import numpy as np +import math + +import os +import pickle + +import seaborn as sns +import matplotlib.pylab as plt +from tabulate import tabulate + + +def getPaths(results): + paths = [] + for i in range(len(results)): + if (i % 2) == 0: + # Append non-smoothed path + paths.append(results[i].path) + else: + # Append smoothed paths array + for result in results[i]: + paths.append(result.path) + return paths + + +def getTimes(results): + times = [] + for i in range(len(results)): + if (i % 2) == 0: + # Append non-smoothed time + times.append(results[i].planning_time.nanosec/1e09 + results[i].planning_time.sec) + else: + # Append smoothed times array + for result in results[i]: + times.append(result.smoothing_duration.nanosec/1e09 + result.smoothing_duration.sec) + return times + + +def getMapCoordsFromPaths(paths, resolution): + coords = [] + for path in paths: + x = [] + y = [] + for pose in path.poses: + x.append(pose.pose.position.x/resolution) + y.append(pose.pose.position.y/resolution) + coords.append(x) + coords.append(y) + return coords + + +def getPathLength(path): + path_length = 0 + x_prev = path.poses[0].pose.position.x + y_prev = path.poses[0].pose.position.y + for i in range(1, len(path.poses)): + x_curr = path.poses[i].pose.position.x + y_curr = path.poses[i].pose.position.y + path_length = path_length + math.sqrt((x_curr-x_prev)**2 + (y_curr-y_prev)**2) + x_prev = x_curr + y_prev = y_curr + return path_length + +# Path smoothness calculations +def getSmoothness(pt_prev, pt, pt_next): + d1 = pt - pt_prev + d2 = pt_next - pt + delta = d2 - d1 + return np.dot(delta, delta) + +def getPathSmoothnesses(paths): + smoothnesses = [] + pm0 = np.array([0.0, 0.0]) + pm1 = np.array([0.0, 0.0]) + pm2 = np.array([0.0, 0.0]) + for path in paths: + smoothness = 0.0 + for i in range(2, len(path.poses)): + pm0[0] = path.poses[i].pose.position.x + pm0[1] = path.poses[i].pose.position.y + pm1[0] = path.poses[i-1].pose.position.x + pm1[1] = path.poses[i-1].pose.position.y + pm2[0] = path.poses[i-2].pose.position.x + pm2[1] = path.poses[i-2].pose.position.y + smoothness += getSmoothness(pm2, pm1, pm0) + smoothnesses.append(smoothness) + return smoothnesses + +# Curvature calculations +def arcCenter(pt_prev, pt, pt_next): + cusp_thresh = -0.7 + + d1 = pt - pt_prev + d2 = pt_next - pt + + d1_norm = d1 / np.linalg.norm(d1) + d2_norm = d2 / np.linalg.norm(d2) + cos_angle = np.dot(d1_norm, d2_norm) + + if cos_angle < cusp_thresh: + # cusp case + d2 = -d2 + pt_next = pt + d2 + + det = d1[0] * d2[1] - d1[1] * d2[0] + if abs(det) < 1e-4: # straight line + return (float('inf'), float('inf')) + + # circle center is at the intersection of mirror axes of the segments: + # http://paulbourke.net/geometry/circlesphere/ + # line intersection: + # https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Intersection%20of%20two%20lines + mid1 = (pt_prev + pt) / 2 + mid2 = (pt + pt_next) / 2 + n1 = (-d1[1], d1[0]) + n2 = (-d2[1], d2[0]) + det1 = (mid1[0] + n1[0]) * mid1[1] - (mid1[1] + n1[1]) * mid1[0] + det2 = (mid2[0] + n2[0]) * mid2[1] - (mid2[1] + n2[1]) * mid2[0] + center = np.array([(det1 * n2[0] - det2 * n1[0]) / det, (det1 * n2[1] - det2 * n1[1]) / det]) + return center + +def getPathCurvatures(paths): + curvatures = [] + pm0 = np.array([0.0, 0.0]) + pm1 = np.array([0.0, 0.0]) + pm2 = np.array([0.0, 0.0]) + for path in paths: + radiuses = [] + for i in range(2, len(path.poses)): + pm0[0] = path.poses[i].pose.position.x + pm0[1] = path.poses[i].pose.position.y + pm1[0] = path.poses[i-1].pose.position.x + pm1[1] = path.poses[i-1].pose.position.y + pm2[0] = path.poses[i-2].pose.position.x + pm2[1] = path.poses[i-2].pose.position.y + center = arcCenter(pm2, pm1, pm0) + if center[0] != float('inf'): + turning_rad = np.linalg.norm(pm1 - center); + radiuses.append(turning_rad) + curvatures.append(np.average(radiuses)) + return curvatures + +def plotResults(costmap, paths): + coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution) + data = np.asarray(costmap.data) + data.resize(costmap.metadata.size_y, costmap.metadata.size_x) + data = np.where(data <= 253, 0, data) + + plt.figure(3) + ax = sns.heatmap(data, cmap='Greys', cbar=False) + for i in range(0, len(coords), 2): + ax.plot(coords[i], coords[i+1], linewidth=0.7) + plt.axis('off') + ax.set_aspect('equal', 'box') + plt.show() + + +def averagePathCost(paths, costmap, num_of_planners): + coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution) + data = np.asarray(costmap.data) + data.resize(costmap.metadata.size_y, costmap.metadata.size_x) + + average_path_costs = [] + for i in range(num_of_planners): + average_path_costs.append([]) + + k = 0 + for i in range(0, len(coords), 2): + costs = [] + for j in range(len(coords[i])): + costs.append(data[math.floor(coords[i+1][j])][math.floor(coords[i][j])]) + average_path_costs[k % num_of_planners].append(sum(costs)/len(costs)) + k += 1 + + return average_path_costs + + +def maxPathCost(paths, costmap, num_of_planners): + coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution) + data = np.asarray(costmap.data) + data.resize(costmap.metadata.size_y, costmap.metadata.size_x) + + max_path_costs = [] + for i in range(num_of_planners): + max_path_costs.append([]) + + k = 0 + for i in range(0, len(coords), 2): + max_cost = 0 + for j in range(len(coords[i])): + cost = data[math.floor(coords[i+1][j])][math.floor(coords[i][j])] + if max_cost < cost: + max_cost = cost + max_path_costs[k % num_of_planners].append(max_cost) + k += 1 + + return max_path_costs + + +def main(): + # Read the data + benchmark_dir = os.getcwd() + print("Read data") + with open(os.path.join(benchmark_dir, 'results.pickle'), 'rb') as f: + results = pickle.load(f) + + with open(os.path.join(benchmark_dir, 'methods.pickle'), 'rb') as f: + smoothers = pickle.load(f) + planner = smoothers[0] + del smoothers[0] + methods_num = len(smoothers) + 1 + + with open(os.path.join(benchmark_dir, 'costmap.pickle'), 'rb') as f: + costmap = pickle.load(f) + + # Paths (planner and smoothers) + paths = getPaths(results) + path_lengths = [] + + for path in paths: + path_lengths.append(getPathLength(path)) + path_lengths = np.asarray(path_lengths) + total_paths = len(paths) + + # [planner, smoothers] path lenghth in a row + path_lengths.resize((int(total_paths/methods_num), methods_num)) + # [planner, smoothers] path length in a column + path_lengths = path_lengths.transpose() + + # Times + times = getTimes(results) + times = np.asarray(times) + times.resize((int(total_paths/methods_num), methods_num)) + times = np.transpose(times) + + # Costs + average_path_costs = np.asarray(averagePathCost(paths, costmap, methods_num)) + max_path_costs = np.asarray(maxPathCost(paths, costmap, methods_num)) + + # Smoothness + smoothnesses = getPathSmoothnesses(paths) + smoothnesses = np.asarray(smoothnesses) + smoothnesses.resize((int(total_paths/methods_num), methods_num)) + smoothnesses = np.transpose(smoothnesses) + + # Curvatures + curvatures = getPathCurvatures(paths) + curvatures = np.asarray(curvatures) + curvatures.resize((int(total_paths/methods_num), methods_num)) + curvatures = np.transpose(curvatures) + + # Generate table + planner_table = [['Planner', + 'Time (s)', + 'Path length (m)', + 'Average cost', + 'Max cost', + 'Path smoothness (x100)', + 'Average turning rad (m)']] + # for path planner + planner_table.append([planner, + np.average(times[0]), + np.average(path_lengths[0]), + np.average(average_path_costs[0]), + np.average(max_path_costs[0]), + np.average(smoothnesses[0]) * 100, + np.average(curvatures[0])]) + # for path smoothers + for i in range(1, methods_num): + planner_table.append([smoothers[i-1], + np.average(times[i]), + np.average(path_lengths[i]), + np.average(average_path_costs[i]), + np.average(max_path_costs[i]), + np.average(smoothnesses[i]) * 100, + np.average(curvatures[i])]) + + # Visualize results + print(tabulate(planner_table)) + plotResults(costmap, paths) + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/tools/smoother_benchmarking/smoother_benchmark_bringup.py b/tools/smoother_benchmarking/smoother_benchmark_bringup.py new file mode 100644 index 00000000000..9b0ed744ded --- /dev/null +++ b/tools/smoother_benchmarking/smoother_benchmark_bringup.py @@ -0,0 +1,94 @@ +# Copyright (c) 2022 Samsung Research America +# +# 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. + +import os + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + benchmark_dir = os.getcwd() + metrics_py = os.path.join(benchmark_dir, 'metrics.py') + config = os.path.join(get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml') + map_file = os.path.join(benchmark_dir, 'maps', 'smoothers_world.yaml') + lifecycle_nodes = ['map_server', 'planner_server', 'smoother_server'] + + static_transform_one = Node( + package = 'tf2_ros', + executable = 'static_transform_publisher', + output = 'screen', + arguments = ["0", "0", "0", "0", "0", "0", "base_link", "map"]) + + static_transform_two = Node( + package = 'tf2_ros', + executable = 'static_transform_publisher', + output = 'screen', + arguments = ["0", "0", "0", "0", "0", "0", "base_link", "odom"]) + + start_map_server_cmd = Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[{'use_sim_time': True}, + {'yaml_filename': map_file}, + {'topic_name': "map"}]) + + start_planner_server_cmd = Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + parameters=[config]) + + start_smoother_server_cmd = Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_server', + output='screen', + parameters=[config]) + + start_lifecycle_manager_cmd = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + parameters=[{'use_sim_time': True}, + {'autostart': True}, + {'node_names': lifecycle_nodes}]) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + metrics_cmd = ExecuteProcess( + cmd=['python3', '-u', metrics_py], + cwd=[benchmark_dir], output='screen') + + ld = LaunchDescription() + ld.add_action(static_transform_one) + ld.add_action(static_transform_two) + ld.add_action(start_map_server_cmd) + ld.add_action(start_planner_server_cmd) + ld.add_action(start_smoother_server_cmd) + ld.add_action(start_lifecycle_manager_cmd) + ld.add_action(rviz_cmd) + ld.add_action(metrics_cmd) + return ld