diff --git a/.circleci/config.yml b/.circleci/config.yml index 7e6fed842ab..ef14bf0d667 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -75,6 +75,9 @@ references: rosdep install -q -y \ --from-paths src \ --ignore-src \ + --skip-keys " \ + slam_toolbox \ + " \ --verbose | \ awk '$1 ~ /^resolution\:/' | \ awk -F'[][]' '{print $2}' | \ diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE.md index d2d3108e77b..884227ea104 100644 --- a/.github/ISSUE_TEMPLATE.md +++ b/.github/ISSUE_TEMPLATE.md @@ -11,6 +11,8 @@ For Bug report or feature requests, please fill out the relevant category below - Operating System: - +- ROS2 Version: + - - Version or commit hash: - diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 5c678f58e42..604c3b5c288 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 0.3.4 + 0.4.7 TODO Michael Jeronimo Carlos Orduno @@ -17,10 +17,10 @@ rclcpp rclcpp_action rclcpp_lifecycle - std_msgs behaviortree_cpp_v3 builtin_interfaces geometry_msgs + sensor_msgs nav2_msgs nav_msgs tf2 @@ -39,6 +39,7 @@ behaviortree_cpp_v3 builtin_interfaces geometry_msgs + sensor_msgs nav2_msgs nav_msgs tf2 diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index 8482e464c34..aa269bdbcc3 100644 --- a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp +++ b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp @@ -12,34 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BEHAVIOR_TREE__CLEAR_COSTMAP_SERVICE_HPP_ -#define NAV2_BEHAVIOR_TREE__CLEAR_COSTMAP_SERVICE_HPP_ - #include #include -#include -#include "nav2_behavior_tree/bt_service_node.hpp" -#include "nav2_msgs/srv/clear_entire_costmap.hpp" +#include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp" namespace nav2_behavior_tree { -class ClearEntireCostmapService : public BtServiceNode +ClearEntireCostmapService::ClearEntireCostmapService( + const std::string & service_node_name, + const BT::NodeConfiguration & conf) +: BtServiceNode(service_node_name, conf) { -public: - ClearEntireCostmapService( - const std::string & service_node_name, - const BT::NodeConfiguration & conf) - : BtServiceNode(service_node_name, conf) - { - } +} - void on_tick() override - { - increment_recovery_count(); - } -}; +void ClearEntireCostmapService::on_tick() +{ + increment_recovery_count(); +} } // namespace nav2_behavior_tree @@ -48,5 +39,3 @@ BT_REGISTER_NODES(factory) { factory.registerNodeType("ClearEntireCostmap"); } - -#endif // NAV2_BEHAVIOR_TREE__CLEAR_COSTMAP_SERVICE_HPP_ 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 be53df37fe5..f4f551eb588 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 @@ -12,61 +12,39 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BEHAVIOR_TREE__COMPUTE_PATH_TO_POSE_ACTION_HPP_ -#define NAV2_BEHAVIOR_TREE__COMPUTE_PATH_TO_POSE_ACTION_HPP_ - #include #include -#include "nav2_msgs/action/compute_path_to_pose.hpp" -#include "nav_msgs/msg/path.h" -#include "nav2_behavior_tree/bt_action_node.hpp" +#include "nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp" namespace nav2_behavior_tree { -class ComputePathToPoseAction : public BtActionNode +ComputePathToPoseAction::ComputePathToPoseAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) { -public: - ComputePathToPoseAction( - const std::string & xml_tag_name, - const std::string & action_name, - const BT::NodeConfiguration & conf) - : BtActionNode(xml_tag_name, action_name, conf) - { - } - - void on_tick() override - { - getInput("goal", goal_.pose); - getInput("planner_id", goal_.planner_id); - } +} - BT::NodeStatus on_success() override - { - setOutput("path", result_.result->path); +void ComputePathToPoseAction::on_tick() +{ + getInput("goal", goal_.pose); + getInput("planner_id", goal_.planner_id); +} - if (first_time_) { - first_time_ = false; - } else { - config().blackboard->set("path_updated", true); - } - return BT::NodeStatus::SUCCESS; - } +BT::NodeStatus ComputePathToPoseAction::on_success() +{ + setOutput("path", result_.result->path); - static BT::PortsList providedPorts() - { - return providedBasicPorts( - { - BT::OutputPort("path", "Path created by ComputePathToPose node"), - BT::InputPort("goal", "Destination to plan to"), - BT::InputPort("planner_id", ""), - }); + if (first_time_) { + first_time_ = false; + } else { + config().blackboard->set("path_updated", true); } - -private: - bool first_time_{true}; -}; + return BT::NodeStatus::SUCCESS; +} } // namespace nav2_behavior_tree @@ -83,5 +61,3 @@ BT_REGISTER_NODES(factory) factory.registerBuilder( "ComputePathToPose", builder); } - -#endif // NAV2_BEHAVIOR_TREE__COMPUTE_PATH_TO_POSE_ACTION_HPP_ diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 820362912e3..62823f40328 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -12,59 +12,42 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BEHAVIOR_TREE__FOLLOW_PATH_ACTION_HPP_ -#define NAV2_BEHAVIOR_TREE__FOLLOW_PATH_ACTION_HPP_ - #include #include -#include "nav2_msgs/action/follow_path.hpp" -#include "nav2_behavior_tree/bt_action_node.hpp" +#include "nav2_behavior_tree/plugins/action/follow_path_action.hpp" namespace nav2_behavior_tree { -class FollowPathAction : public BtActionNode +FollowPathAction::FollowPathAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) { -public: - FollowPathAction( - const std::string & xml_tag_name, - const std::string & action_name, - const BT::NodeConfiguration & conf) - : BtActionNode(xml_tag_name, action_name, conf) - { - config().blackboard->set("path_updated", false); - } - - void on_tick() override - { - getInput("path", goal_.path); - getInput("controller_id", goal_.controller_id); - } + config().blackboard->set("path_updated", false); +} - void on_wait_for_result() override - { - // Check if the goal has been updated - if (config().blackboard->get("path_updated")) { - // Reset the flag in the blackboard - config().blackboard->set("path_updated", false); +void FollowPathAction::on_tick() +{ + getInput("path", goal_.path); + getInput("controller_id", goal_.controller_id); +} - // Grab the new goal and set the flag so that we send the new goal to - // the action server on the next loop iteration - getInput("path", goal_.path); - goal_updated_ = true; - } - } +void FollowPathAction::on_wait_for_result() +{ + // Check if the goal has been updated + if (config().blackboard->get("path_updated")) { + // Reset the flag in the blackboard + config().blackboard->set("path_updated", false); - static BT::PortsList providedPorts() - { - return providedBasicPorts( - { - BT::InputPort("path", "Path to follow"), - BT::InputPort("controller_id", ""), - }); + // Grab the new goal and set the flag so that we send the new goal to + // the action server on the next loop iteration + getInput("path", goal_.path); + goal_updated_ = true; } -}; +} } // namespace nav2_behavior_tree @@ -81,5 +64,3 @@ BT_REGISTER_NODES(factory) factory.registerBuilder( "FollowPath", builder); } - -#endif // NAV2_BEHAVIOR_TREE__FOLLOW_PATH_ACTION_HPP_ diff --git a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp index a65e6433f6f..f779a682ade 100644 --- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp @@ -12,58 +12,38 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BEHAVIOR_TREE__NAVIGATE_TO_POSE_ACTION_HPP_ -#define NAV2_BEHAVIOR_TREE__NAVIGATE_TO_POSE_ACTION_HPP_ - #include #include -#include "geometry_msgs/msg/point.hpp" -#include "geometry_msgs/msg/quaternion.hpp" -#include "nav2_msgs/action/navigate_to_pose.hpp" -#include "nav2_behavior_tree/bt_action_node.hpp" +#include "nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp" namespace nav2_behavior_tree { -class NavigateToPoseAction : public BtActionNode +NavigateToPoseAction::NavigateToPoseAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) { -public: - NavigateToPoseAction( - const std::string & xml_tag_name, - const std::string & action_name, - const BT::NodeConfiguration & conf) - : BtActionNode(xml_tag_name, action_name, conf) - { - } - - void on_tick() override - { - // Use the position and orientation fields from the XML attributes to initialize the goal - geometry_msgs::msg::Point position; - geometry_msgs::msg::Quaternion orientation; - - if (!getInput("position", position) || !getInput("orientation", orientation)) { - RCLCPP_ERROR( - node_->get_logger(), - "NavigateToPoseAction: position or orientation not provided"); - return; - } +} - goal_.pose.pose.position = position; - goal_.pose.pose.orientation = orientation; +void NavigateToPoseAction::on_tick() +{ + // Use the position and orientation fields from the XML attributes to initialize the goal + geometry_msgs::msg::Point position; + geometry_msgs::msg::Quaternion orientation; + + if (!getInput("position", position) || !getInput("orientation", orientation)) { + RCLCPP_ERROR( + node_->get_logger(), + "NavigateToPoseAction: position or orientation not provided"); + return; } - // Any BT node that accepts parameters must provide a requiredNodeParameters method - static BT::PortsList providedPorts() - { - return providedBasicPorts( - { - BT::InputPort("position", "Position"), - BT::InputPort("orientation", "Orientation") - }); - } -}; + goal_.pose.pose.position = position; + goal_.pose.pose.orientation = orientation; +} } // namespace nav2_behavior_tree @@ -80,5 +60,3 @@ BT_REGISTER_NODES(factory) factory.registerBuilder( "NavigateToPose", builder); } - -#endif // NAV2_BEHAVIOR_TREE__NAVIGATE_TO_POSE_ACTION_HPP_ diff --git a/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp b/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp index edbde1af24a..388f16ccb25 100644 --- a/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp +++ b/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp @@ -12,29 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BEHAVIOR_TREE__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_ -#define NAV2_BEHAVIOR_TREE__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_ - #include -#include -#include - -#include "nav2_behavior_tree/bt_service_node.hpp" -#include "std_srvs/srv/empty.hpp" +#include "nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp" namespace nav2_behavior_tree { -class ReinitializeGlobalLocalizationService : public BtServiceNode -{ -public: - ReinitializeGlobalLocalizationService( - const std::string & service_node_name, - const BT::NodeConfiguration & conf) - : BtServiceNode(service_node_name, conf) - { - } -}; +ReinitializeGlobalLocalizationService::ReinitializeGlobalLocalizationService( + const std::string & service_node_name, + const BT::NodeConfiguration & conf) +: BtServiceNode(service_node_name, conf) +{} } // namespace nav2_behavior_tree @@ -44,5 +32,3 @@ BT_REGISTER_NODES(factory) factory.registerNodeType( "ReinitializeGlobalLocalization"); } - -#endif // NAV2_BEHAVIOR_TREE__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_ diff --git a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp new file mode 100644 index 00000000000..7f919a30f3a --- /dev/null +++ b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp @@ -0,0 +1,89 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Francisco Martin Rico +// +// 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 "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "behaviortree_cpp_v3/decorator_node.h" + +#include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp" + +namespace nav2_behavior_tree +{ + +TruncatePath::TruncatePath( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(name, conf), + distance_(1.0) +{ + getInput("distance", distance_); +} + +inline BT::NodeStatus TruncatePath::tick() +{ + setStatus(BT::NodeStatus::RUNNING); + + nav_msgs::msg::Path input_path; + + getInput("input_path", input_path); + + if (input_path.poses.empty()) { + setOutput("output_path", input_path); + return BT::NodeStatus::SUCCESS; + } + + geometry_msgs::msg::PoseStamped final_pose = input_path.poses.back(); + + double distance_to_goal = nav2_util::geometry_utils::euclidean_distance( + input_path.poses.back(), final_pose); + + while (distance_to_goal < distance_ && input_path.poses.size() > 2) { + input_path.poses.pop_back(); + distance_to_goal = nav2_util::geometry_utils::euclidean_distance( + input_path.poses.back(), final_pose); + } + + double dx = final_pose.pose.position.x - input_path.poses.back().pose.position.x; + double dy = final_pose.pose.position.y - input_path.poses.back().pose.position.y; + + double final_angle = atan2(dy, dx); + + if (std::isnan(final_angle) || std::isinf(final_angle)) { + RCLCPP_WARN( + config().blackboard->get("node")->get_logger(), + "Final angle is not valid while truncating path. Setting to 0.0"); + final_angle = 0.0; + } + + input_path.poses.back().pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + final_angle); + + setOutput("output_path", input_path); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("TruncatePath"); +} diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp new file mode 100644 index 00000000000..4aad6d16769 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -0,0 +1,67 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2019 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. + +#include + +#include "nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp" + +namespace nav2_behavior_tree +{ + +IsBatteryLowCondition::IsBatteryLowCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + battery_topic_("/battery_status"), + min_battery_(0.0), + is_voltage_(false), + is_battery_low_(false) +{ + getInput("min_battery", min_battery_); + getInput("battery_topic", battery_topic_); + getInput("is_voltage", is_voltage_); + node_ = config().blackboard->get("node"); + battery_sub_ = node_->create_subscription( + battery_topic_, + rclcpp::SystemDefaultsQoS(), + std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1)); +} + +BT::NodeStatus IsBatteryLowCondition::tick() +{ + std::lock_guard lock(mutex_); + if (is_battery_low_) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + +void IsBatteryLowCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg) +{ + std::lock_guard lock(mutex_); + if (is_voltage_) { + is_battery_low_ = msg->voltage <= min_battery_; + } else { + is_battery_low_ = msg->percentage <= min_battery_; + } +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("IsBatteryLow"); +} diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp new file mode 100644 index 00000000000..5279613498b --- /dev/null +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -0,0 +1,71 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Francisco Martin Rico +// +// 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 "geometry_msgs/msg/pose_stamped.hpp" +#include "behaviortree_cpp_v3/decorator_node.h" + +#include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +using std::placeholders::_1; + +GoalUpdater::GoalUpdater( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::DecoratorNode(name, conf) +{ + auto node = config().blackboard->get("node"); + + std::string goal_updater_topic; + node->get_parameter_or("goal_updater_topic", goal_updater_topic, "goal_update"); + + goal_sub_ = node->create_subscription( + goal_updater_topic, 10, std::bind(&GoalUpdater::callback_updated_goal, this, _1)); +} + +inline BT::NodeStatus GoalUpdater::tick() +{ + geometry_msgs::msg::PoseStamped goal; + + getInput("input_goal", goal); + + if (rclcpp::Time(last_goal_received_.header.stamp) > rclcpp::Time(goal.header.stamp)) { + goal = last_goal_received_; + } + + setOutput("output_goal", goal); + return child_node_->executeTick(); +} + +void +GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg) +{ + last_goal_received_ = *msg; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("GoalUpdater"); +} diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 36b852e61c9..93841615d9e 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Florian Gramss // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,8 +22,6 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp_v3/utils/shared_library.h" -using namespace std::chrono_literals; - namespace nav2_behavior_tree { @@ -62,13 +61,54 @@ BehaviorTreeEngine::run( } BT::Tree -BehaviorTreeEngine::buildTreeFromText( +BehaviorTreeEngine::createTreeFromText( const std::string & xml_string, BT::Blackboard::Ptr blackboard) { - BT::XMLParser p(factory_); - p.loadFromText(xml_string); - return p.instantiateTree(blackboard); + return factory_.createTreeFromText(xml_string, blackboard); +} + +BT::Tree +BehaviorTreeEngine::createTreeFromFile( + const std::string & file_path, + BT::Blackboard::Ptr blackboard) +{ + return factory_.createTreeFromFile(file_path, blackboard); +} + +void +BehaviorTreeEngine::addGrootMonitoring( + BT::Tree * tree, + uint16_t publisher_port, + uint16_t server_port, + uint16_t max_msg_per_second) +{ + // This logger publish status changes using ZeroMQ. Used by Groot + groot_monitor_ = std::make_unique( + *tree, max_msg_per_second, publisher_port, + server_port); +} + +void +BehaviorTreeEngine::resetGrootMonitor() +{ + groot_monitor_.reset(); +} + +// In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state +void +BehaviorTreeEngine::haltAllActions(BT::TreeNode * root_node) +{ + // this halt signal should propagate through the entire tree. + root_node->halt(); + + // but, just in case... + auto visitor = [](BT::TreeNode * node) { + if (node->status() == BT::NodeStatus::RUNNING) { + node->halt(); + } + }; + BT::applyRecursiveVisitor(root_node, visitor); } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/test/CMakeLists.txt b/nav2_behavior_tree/test/CMakeLists.txt index 93d1356d40a..1486afac1db 100644 --- a/nav2_behavior_tree/test/CMakeLists.txt +++ b/nav2_behavior_tree/test/CMakeLists.txt @@ -4,3 +4,4 @@ ament_target_dependencies(test_bt_conversions ${dependencies}) add_subdirectory(plugins/condition) add_subdirectory(plugins/decorator) add_subdirectory(plugins/control) +add_subdirectory(plugins/action) diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt new file mode 100644 index 00000000000..3f05ae112d5 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -0,0 +1,25 @@ +ament_add_gtest(test_action_clear_costmap_service test_clear_costmap_service.cpp) +target_link_libraries(test_action_clear_costmap_service nav2_clear_costmap_service_bt_node) +ament_target_dependencies(test_action_clear_costmap_service ${dependencies}) + +ament_add_gtest( + test_action_reinitialize_global_localization_service test_reinitialize_global_localization_service.cpp) +target_link_libraries( + test_action_reinitialize_global_localization_service nav2_reinitialize_global_localization_service_bt_node) +ament_target_dependencies(test_action_reinitialize_global_localization_service ${dependencies}) + +ament_add_gtest(test_action_compute_path_to_pose_action test_compute_path_to_pose_action.cpp) +target_link_libraries(test_action_compute_path_to_pose_action nav2_compute_path_to_pose_action_bt_node) +ament_target_dependencies(test_action_compute_path_to_pose_action ${dependencies}) + +ament_add_gtest(test_action_follow_path_action test_follow_path_action.cpp) +target_link_libraries(test_action_follow_path_action nav2_follow_path_action_bt_node) +ament_target_dependencies(test_action_follow_path_action ${dependencies}) + +ament_add_gtest(test_action_navigate_to_pose_action test_navigate_to_pose_action.cpp) +target_link_libraries(test_action_navigate_to_pose_action nav2_navigate_to_pose_action_bt_node) +ament_target_dependencies(test_action_navigate_to_pose_action ${dependencies}) + +ament_add_gtest(test_truncate_path_action test_truncate_path_action.cpp) +target_link_libraries(test_truncate_path_action nav2_truncate_path_action_bt_node) +ament_target_dependencies(test_truncate_path_action ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp new file mode 100644 index 00000000000..8906d03d7fc --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp @@ -0,0 +1,173 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// 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/back_up_action.hpp" + +class BackUpActionServer : public TestActionServer +{ +public: + BackUpActionServer() + : TestActionServer("backup") + {} + +protected: + void execute( + const typename std::shared_ptr>) + override + {} +}; + +class BackUpActionTestFixture : 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(10)); + config_->blackboard->set("path_updated", false); + 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, "backup", config); + }; + + factory_->registerBuilder("BackUp", 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 BackUpActionTestFixture::node_ = nullptr; +std::shared_ptr BackUpActionTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * BackUpActionTestFixture::config_ = nullptr; +std::shared_ptr BackUpActionTestFixture::factory_ = nullptr; +std::shared_ptr BackUpActionTestFixture::tree_ = nullptr; + +TEST_F(BackUpActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->getInput("backup_dist"), 0.15); + EXPECT_EQ(tree_->rootNode()->getInput("backup_speed"), 0.025); + + xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->getInput("backup_dist"), 2.0); + EXPECT_EQ(tree_->rootNode()->getInput("backup_speed"), 0.26); +} + +TEST_F(BackUpActionTestFixture, 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); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); + + auto goal = action_server_->getCurrentGoal(); + EXPECT_EQ(goal->target.x, 2.0); + EXPECT_EQ(goal->speed, 0.26f); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + BackUpActionTestFixture::action_server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(BackUpActionTestFixture::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_clear_costmap_service.cpp b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp new file mode 100644 index 00000000000..c5a65c52a1f --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp @@ -0,0 +1,130 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// 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_service.hpp" +#include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp" + +class ClearEntireCostmapService : public TestService +{ +public: + ClearEntireCostmapService() + : TestService("clear_entire_costmap") + {} +}; + +class ClearEntireCostmapServiceTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("clear_entire_costmap_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(10)); + config_->blackboard->set("path_updated", false); + config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("number_recoveries", 0); + + factory_->registerNodeType("ClearEntireCostmap"); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + server_.reset(); + factory_.reset(); + } + + void SetUp() override + { + config_->blackboard->set("number_recoveries", 0); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr server_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr ClearEntireCostmapServiceTestFixture::node_ = nullptr; +std::shared_ptr ClearEntireCostmapServiceTestFixture::server_ = nullptr; +BT::NodeConfiguration * ClearEntireCostmapServiceTestFixture::config_ = nullptr; +std::shared_ptr ClearEntireCostmapServiceTestFixture::factory_ = nullptr; +std::shared_ptr ClearEntireCostmapServiceTestFixture::tree_ = nullptr; + +TEST_F(ClearEntireCostmapServiceTestFixture, 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); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize service and spin on new thread + ClearEntireCostmapServiceTestFixture::server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(ClearEntireCostmapServiceTestFixture::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_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp new file mode 100644 index 00000000000..dd059315379 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp @@ -0,0 +1,198 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// 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 "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp" + +class ComputePathToPoseActionServer : public TestActionServer +{ +public: + ComputePathToPoseActionServer() + : TestActionServer("compute_path_to_pose") + {} + +protected: + void execute( + const typename std::shared_ptr< + rclcpp_action::ServerGoalHandle> goal_handle) + override + { + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + result->path.poses.resize(1); + result->path.poses[0].pose.position.x = goal->pose.pose.position.x; + goal_handle->succeed(result); + } +}; + +class ComputePathToPoseActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("compute_path_to_pose_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(10)); + config_->blackboard->set("path_updated", false); + config_->blackboard->set("initial_pose_received", false); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "compute_path_to_pose", config); + }; + + factory_->registerBuilder( + "ComputePathToPose", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + factory_.reset(); + } + + 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 ComputePathToPoseActionTestFixture::node_ = nullptr; +std::shared_ptr +ComputePathToPoseActionTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * ComputePathToPoseActionTestFixture::config_ = nullptr; +std::shared_ptr ComputePathToPoseActionTestFixture::factory_ = nullptr; +std::shared_ptr ComputePathToPoseActionTestFixture::tree_ = nullptr; + +TEST_F(ComputePathToPoseActionTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new goal and set it on blackboard + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.pose.position.x = 1.0; + config_->blackboard->set("goal", goal); + + // first tick should send the goal to our server + EXPECT_EQ(config_->blackboard->get("path_updated"), false); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); + EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // not set to true after the first goal + EXPECT_EQ(config_->blackboard->get("path_updated"), false); + + // check if returned path is correct + nav_msgs::msg::Path path; + config_->blackboard->get("path", path); + EXPECT_EQ(path.poses.size(), 1u); + EXPECT_EQ(path.poses[0].pose.position.x, 1.0); + + // halt node so another goal can be sent + tree_->rootNode()->halt(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); + + // set new goal + goal.pose.position.x = -2.5; + config_->blackboard->set("goal", goal); + + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, -2.5); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // path is updated on new goal + EXPECT_EQ(config_->blackboard->get("path_updated"), true); + + config_->blackboard->get("path", path); + EXPECT_EQ(path.poses.size(), 1u); + EXPECT_EQ(path.poses[0].pose.position.x, -2.5); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + ComputePathToPoseActionTestFixture::action_server_ = + std::make_shared(); + + std::thread server_thread([]() { + rclcpp::spin(ComputePathToPoseActionTestFixture::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_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp new file mode 100644 index 00000000000..2bd07f42323 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp @@ -0,0 +1,188 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// 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 "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/follow_path_action.hpp" + +class FollowPathActionServer : public TestActionServer +{ +public: + FollowPathActionServer() + : TestActionServer("follow_path") + {} + +protected: + void execute( + const typename std::shared_ptr< + rclcpp_action::ServerGoalHandle> goal_handle) + override + { + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + goal_handle->succeed(result); + } +}; + +class FollowPathActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("follow_path_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(10)); + config_->blackboard->set("path_updated", false); + config_->blackboard->set("initial_pose_received", false); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "follow_path", config); + }; + + factory_->registerBuilder( + "FollowPath", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + factory_.reset(); + } + + 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 FollowPathActionTestFixture::node_ = nullptr; +std::shared_ptr +FollowPathActionTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * FollowPathActionTestFixture::config_ = nullptr; +std::shared_ptr FollowPathActionTestFixture::factory_ = nullptr; +std::shared_ptr FollowPathActionTestFixture::tree_ = nullptr; + +TEST_F(FollowPathActionTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + config_->blackboard->set("path_updated", true); + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(config_->blackboard->get("path_updated"), false); + + // set new path on blackboard + nav_msgs::msg::Path path; + path.poses.resize(1); + path.poses[0].pose.position.x = 1.0; + config_->blackboard->set("path", path); + + // first tick should send the goal to our server + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(tree_->rootNode()->getInput("controller_id"), std::string("FollowPath")); + EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u); + EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->controller_id, std::string("FollowPath")); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // halt node so another goal can be sent + tree_->rootNode()->halt(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); + + // set new goal + path.poses[0].pose.position.x = -2.5; + config_->blackboard->set("path", path); + + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u); + EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, -2.5); + config_->blackboard->set("path_updated", true); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // path is updated on new goal + EXPECT_EQ(config_->blackboard->get("path_updated"), false); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + FollowPathActionTestFixture::action_server_ = + std::make_shared(); + + std::thread server_thread([]() { + rclcpp::spin(FollowPathActionTestFixture::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_navigate_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp new file mode 100644 index 00000000000..c57ca23bce4 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp @@ -0,0 +1,179 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// 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 "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/quaternion.hpp" + +#include "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp" + +class NavigateToPoseActionServer : public TestActionServer +{ +public: + NavigateToPoseActionServer() + : TestActionServer("navigate_to_pose") + {} + +protected: + void execute( + const typename std::shared_ptr< + rclcpp_action::ServerGoalHandle> goal_handle) + override + { + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + goal_handle->succeed(result); + } +}; + +class NavigateToPoseActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("navigate_to_pose_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(10)); + config_->blackboard->set("path_updated", false); + config_->blackboard->set("initial_pose_received", false); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "navigate_to_pose", config); + }; + + factory_->registerBuilder( + "NavigateToPose", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + factory_.reset(); + } + + 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 NavigateToPoseActionTestFixture::node_ = nullptr; +std::shared_ptr +NavigateToPoseActionTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * NavigateToPoseActionTestFixture::config_ = nullptr; +std::shared_ptr NavigateToPoseActionTestFixture::factory_ = nullptr; +std::shared_ptr NavigateToPoseActionTestFixture::tree_ = nullptr; + +TEST_F(NavigateToPoseActionTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + geometry_msgs::msg::PoseStamped pose; + + // first tick should send the goal to our server + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // halt node so another goal can be sent + tree_->rootNode()->halt(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); + + // set new goal + pose.pose.position.x = -2.5; + pose.pose.orientation.x = 1.0; + config_->blackboard->set("position", pose.pose.position); + config_->blackboard->set("orientation", pose.pose.orientation); + + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + NavigateToPoseActionTestFixture::action_server_ = + std::make_shared(); + + std::thread server_thread([]() { + rclcpp::spin(NavigateToPoseActionTestFixture::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_reinitialize_global_localization_service.cpp b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp new file mode 100644 index 00000000000..03078d384d0 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp @@ -0,0 +1,126 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// 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_service.hpp" +#include "nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp" + +class ReinitializeGlobalLocalizationService : public TestService +{ +public: + ReinitializeGlobalLocalizationService() + : TestService("reinitialize_global_localization") + {} +}; + +class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("reinitialize_global_localization_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(10)); + config_->blackboard->set("path_updated", false); + config_->blackboard->set("initial_pose_received", false); + + factory_->registerNodeType( + "ReinitializeGlobalLocalization"); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + server_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr server_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr ReinitializeGlobalLocalizationServiceTestFixture::node_ = nullptr; +std::shared_ptr +ReinitializeGlobalLocalizationServiceTestFixture::server_ = nullptr; +BT::NodeConfiguration * ReinitializeGlobalLocalizationServiceTestFixture::config_ = nullptr; +std::shared_ptr +ReinitializeGlobalLocalizationServiceTestFixture::factory_ = nullptr; +std::shared_ptr ReinitializeGlobalLocalizationServiceTestFixture::tree_ = nullptr; + +TEST_F(ReinitializeGlobalLocalizationServiceTestFixture, test_tick) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize service and spin on new thread + ReinitializeGlobalLocalizationServiceTestFixture::server_ = + std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(ReinitializeGlobalLocalizationServiceTestFixture::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_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp new file mode 100644 index 00000000000..e24b3b251e1 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -0,0 +1,168 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// 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/spin_action.hpp" + +class SpinActionServer : public TestActionServer +{ +public: + SpinActionServer() + : TestActionServer("spin") + {} + +protected: + void execute( + const typename std::shared_ptr>) + override + {} +}; + +class SpinActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("spin_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(10)); + config_->blackboard->set("path_updated", false); + 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, "spin", config); + }; + + factory_->registerBuilder("Spin", 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 SpinActionTestFixture::node_ = nullptr; +std::shared_ptr SpinActionTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * SpinActionTestFixture::config_ = nullptr; +std::shared_ptr SpinActionTestFixture::factory_ = nullptr; +std::shared_ptr SpinActionTestFixture::tree_ = nullptr; + +TEST_F(SpinActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->getInput("spin_dist"), 1.57); + + xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->getInput("spin_dist"), 3.14); +} + +TEST_F(SpinActionTestFixture, 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); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); + EXPECT_EQ(action_server_->getCurrentGoal()->target_yaw, 3.14f); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + SpinActionTestFixture::action_server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(SpinActionTestFixture::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_truncate_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp new file mode 100644 index 00000000000..11c56ef082d --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp @@ -0,0 +1,156 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Francisco Martin Rico +// +// 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 "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" + +#include "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp" + + +class TruncatePathTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("change_goal_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "TruncatePath", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr TruncatePathTestFixture::node_ = nullptr; + +BT::NodeConfiguration * TruncatePathTestFixture::config_ = nullptr; +std::shared_ptr TruncatePathTestFixture::factory_ = nullptr; +std::shared_ptr TruncatePathTestFixture::tree_ = nullptr; + +TEST_F(TruncatePathTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new goal and set it on blackboard + nav_msgs::msg::Path path; + path.header.stamp = node_->now(); + + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 0; + pose.pose.position.y = 0; + pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.0); + path.poses.push_back(pose); + + pose.pose.position.x = 0.5; + pose.pose.position.y = 0.0; + path.poses.push_back(pose); + + pose.pose.position.x = 0.9; + pose.pose.position.y = 0.0; + path.poses.push_back(pose); + + pose.pose.position.x = 1.5; + pose.pose.position.y = 0.5; + path.poses.push_back(pose); + + EXPECT_EQ(path.poses.size(), 4u); + + config_->blackboard->set("path", path); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + nav_msgs::msg::Path truncated_path; + config_->blackboard->get("truncated_path", truncated_path); + + EXPECT_NE(path, truncated_path); + EXPECT_EQ(truncated_path.poses.size(), 2u); + + double r, p, y; + tf2::Quaternion q; + tf2::fromMsg(truncated_path.poses.back().pose.orientation, q); + tf2::Matrix3x3(q).getRPY(r, p, y); + + EXPECT_NEAR(y, 0.463, 0.001); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp new file mode 100644 index 00000000000..b1023c47233 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -0,0 +1,169 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// 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/wait_action.hpp" + +class WaitActionServer : public TestActionServer +{ +public: + WaitActionServer() + : TestActionServer("wait") + {} + +protected: + void execute( + const typename std::shared_ptr>) + override + {} +}; + +class WaitActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("wait_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(10)); + config_->blackboard->set("path_updated", false); + 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, "wait", config); + }; + + factory_->registerBuilder("Wait", 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 WaitActionTestFixture::node_ = nullptr; +std::shared_ptr WaitActionTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * WaitActionTestFixture::config_ = nullptr; +std::shared_ptr WaitActionTestFixture::factory_ = nullptr; +std::shared_ptr WaitActionTestFixture::tree_ = nullptr; + +TEST_F(WaitActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->getInput("wait_duration"), 1); + + xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->getInput("wait_duration"), 10); +} + +TEST_F(WaitActionTestFixture, 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); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); + EXPECT_EQ(rclcpp::Duration(action_server_->getCurrentGoal()->time).seconds(), 5.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 + WaitActionTestFixture::action_server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(WaitActionTestFixture::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/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index e8d9d8d3d51..b1350f74537 100644 --- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt @@ -25,3 +25,7 @@ ament_target_dependencies(test_condition_transform_available ${dependencies}) ament_add_gtest(test_condition_is_stuck test_is_stuck.cpp) target_link_libraries(test_condition_is_stuck nav2_is_stuck_condition_bt_node) ament_target_dependencies(test_condition_is_stuck ${dependencies}) + +ament_add_gtest(test_condition_is_battery_low test_is_battery_low.cpp) +target_link_libraries(test_condition_is_battery_low nav2_is_battery_low_condition_bt_node) +ament_target_dependencies(test_condition_is_battery_low ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp new file mode 100644 index 00000000000..992c50297de --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp @@ -0,0 +1,162 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2019 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. + +#include +#include +#include +#include +#include + +#include "sensor_msgs/msg/battery_state.hpp" + +#include "../../test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp" + +class IsBatteryLowConditionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("test_is_battery_low"); + 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_); + + factory_->registerNodeType("IsBatteryLow"); + + battery_pub_ = node_->create_publisher( + "/battery_status", + rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + battery_pub_.reset(); + node_.reset(); + factory_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static rclcpp::Publisher::SharedPtr battery_pub_; +}; + +rclcpp::Node::SharedPtr IsBatteryLowConditionTestFixture::node_ = nullptr; +BT::NodeConfiguration * IsBatteryLowConditionTestFixture::config_ = nullptr; +std::shared_ptr IsBatteryLowConditionTestFixture::factory_ = nullptr; +rclcpp::Publisher::SharedPtr +IsBatteryLowConditionTestFixture::battery_pub_ = nullptr; + +TEST_F(IsBatteryLowConditionTestFixture, test_behavior_percentage) +{ + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + sensor_msgs::msg::BatteryState battery_msg; + battery_msg.percentage = 1.0; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + + battery_msg.percentage = 0.49; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + + battery_msg.percentage = 0.51; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + + battery_msg.percentage = 0.0; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); +} + +TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage) +{ + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + sensor_msgs::msg::BatteryState battery_msg; + battery_msg.voltage = 10.0; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + + battery_msg.voltage = 4.9; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + + battery_msg.voltage = 5.1; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + + battery_msg.voltage = 0.0; + battery_pub_->publish(battery_msg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); +} + +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_behavior_tree/test/plugins/decorator/CMakeLists.txt b/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt index a1b568a8a54..ad53c5d1d1a 100644 --- a/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt @@ -9,3 +9,7 @@ ament_target_dependencies(test_decorator_speed_controller ${dependencies}) ament_add_gtest(test_decorator_rate_controller test_rate_controller.cpp) target_link_libraries(test_decorator_rate_controller nav2_rate_controller_bt_node) ament_target_dependencies(test_decorator_rate_controller ${dependencies}) + +ament_add_gtest(test_goal_updater_node test_goal_updater_node.cpp) +target_link_libraries(test_goal_updater_node nav2_goal_updater_node_bt_node) +ament_target_dependencies(test_goal_updater_node ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp new file mode 100644 index 00000000000..341192757fb --- /dev/null +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -0,0 +1,149 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Francisco Martin Rico +// +// 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 "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp" + + +class GoalUpdaterTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("goal_updater_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "GoalUpdater", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr GoalUpdaterTestFixture::node_ = nullptr; + +BT::NodeConfiguration * GoalUpdaterTestFixture::config_ = nullptr; +std::shared_ptr GoalUpdaterTestFixture::factory_ = nullptr; +std::shared_ptr GoalUpdaterTestFixture::tree_ = nullptr; + +TEST_F(GoalUpdaterTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new goal and set it on blackboard + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.pose.position.x = 1.0; + config_->blackboard->set("goal", goal); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + geometry_msgs::msg::PoseStamped updated_goal; + config_->blackboard->get("updated_goal", updated_goal); + + EXPECT_EQ(updated_goal, goal); + + geometry_msgs::msg::PoseStamped goal_to_update; + goal_to_update.header.stamp = node_->now(); + goal_to_update.pose.position.x = 2.0; + + auto goal_updater_pub = + node_->create_publisher("goal_update", 10); + + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + goal_updater_pub->publish(goal_to_update); + + rclcpp::spin_some(node_); + } + + config_->blackboard->get("updated_goal", updated_goal); + + EXPECT_NE(updated_goal, goal); + EXPECT_EQ(updated_goal, goal_to_update); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/test_action_server.hpp b/nav2_behavior_tree/test/test_action_server.hpp new file mode 100644 index 00000000000..2c7891edb6b --- /dev/null +++ b/nav2_behavior_tree/test/test_action_server.hpp @@ -0,0 +1,82 @@ +// Copyright (c) 2020 Sarthak Mittal +// +// 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 TEST_ACTION_SERVER_HPP_ +#define TEST_ACTION_SERVER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +template +class TestActionServer : public rclcpp::Node +{ +public: + explicit TestActionServer( + std::string action_name, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("test_action_server", options) + { + using namespace std::placeholders; // NOLINT + + this->action_server_ = rclcpp_action::create_server( + this->get_node_base_interface(), + this->get_node_clock_interface(), + this->get_node_logging_interface(), + this->get_node_waitables_interface(), + action_name, + std::bind(&TestActionServer::handle_goal, this, _1, _2), + std::bind(&TestActionServer::handle_cancel, this, _1), + std::bind(&TestActionServer::handle_accepted, this, _1)); + } + + std::shared_ptr getCurrentGoal() const + { + return current_goal_; + } + +protected: + virtual rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID &, + std::shared_ptr goal) + { + current_goal_ = goal; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + virtual rclcpp_action::CancelResponse handle_cancel( + const typename std::shared_ptr>) + { + return rclcpp_action::CancelResponse::ACCEPT; + } + + virtual void execute( + const typename std::shared_ptr> goal_handle) = 0; + + void handle_accepted( + const std::shared_ptr> goal_handle) + { + using namespace std::placeholders; // NOLINT + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&TestActionServer::execute, this, _1), goal_handle}.detach(); + } + +private: + typename rclcpp_action::Server::SharedPtr action_server_; + std::shared_ptr current_goal_; +}; + +#endif // TEST_ACTION_SERVER_HPP_ diff --git a/nav2_behavior_tree/test/test_service.hpp b/nav2_behavior_tree/test/test_service.hpp new file mode 100644 index 00000000000..15447837e2f --- /dev/null +++ b/nav2_behavior_tree/test/test_service.hpp @@ -0,0 +1,60 @@ +// Copyright (c) 2020 Sarthak Mittal +// +// 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 TEST_SERVICE_HPP_ +#define TEST_SERVICE_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +template +class TestService : public rclcpp::Node +{ +public: + explicit TestService( + std::string service_name, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("test_service", options) + { + using namespace std::placeholders; // NOLINT + + server_ = create_service( + service_name, + std::bind(&TestService::handle_service, this, _1, _2, _3)); + } + + std::shared_ptr getCurrentRequest() const + { + return current_request_; + } + +protected: + virtual void handle_service( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + (void)request_header; + (void)response; + current_request_ = request; + } + +private: + typename rclcpp::Service::SharedPtr server_; + std::shared_ptr current_request_; +}; + +#endif // TEST_SERVICE_HPP_ diff --git a/nav2_bringup/bringup/launch/bringup_launch.py b/nav2_bringup/bringup/launch/bringup_launch.py index 61a048d0da9..4e200fcde25 100644 --- a/nav2_bringup/bringup/launch/bringup_launch.py +++ b/nav2_bringup/bringup/launch/bringup_launch.py @@ -41,7 +41,7 @@ def generate_launch_description(): autostart = LaunchConfiguration('autostart') stdout_linebuf_envvar = SetEnvironmentVariable( - 'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1') + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') declare_namespace_cmd = DeclareLaunchArgument( 'namespace', diff --git a/nav2_bringup/bringup/launch/localization_launch.py b/nav2_bringup/bringup/launch/localization_launch.py index 2791e0068cd..e058bba8c10 100644 --- a/nav2_bringup/bringup/launch/localization_launch.py +++ b/nav2_bringup/bringup/launch/localization_launch.py @@ -56,7 +56,7 @@ def generate_launch_description(): return LaunchDescription([ # Set env var to print messages to stdout immediately - SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), DeclareLaunchArgument( 'namespace', default_value='', diff --git a/nav2_bringup/bringup/launch/navigation_launch.py b/nav2_bringup/bringup/launch/navigation_launch.py index b837ac4d4eb..c421d709731 100644 --- a/nav2_bringup/bringup/launch/navigation_launch.py +++ b/nav2_bringup/bringup/launch/navigation_launch.py @@ -64,7 +64,7 @@ def generate_launch_description(): return LaunchDescription([ # Set env var to print messages to stdout immediately - SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), DeclareLaunchArgument( 'namespace', default_value='', diff --git a/nav2_bringup/bringup/package.xml b/nav2_bringup/bringup/package.xml index 2d764f7b723..d34bd54d0af 100644 --- a/nav2_bringup/bringup/package.xml +++ b/nav2_bringup/bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 0.3.4 + 0.4.7 Bringup scripts and configurations for the navigation2 stack Michael Jeronimo Steve Macenski @@ -18,6 +18,7 @@ launch_ros navigation2 nav2_common + slam_toolbox ament_lint_common ament_lint_auto diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index 0c9c0ffa7af..4d1e9643815 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -37,6 +37,7 @@ amcl: z_max: 0.05 z_rand: 0.5 z_short: 0.05 + scan_topic: scan amcl_map_client: ros__parameters: @@ -85,8 +86,21 @@ controller_server: min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" controller_plugins: ["FollowPath"] + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" @@ -145,6 +159,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 + inflation_radius: 0.55 obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True @@ -211,7 +226,7 @@ recoveries_server: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "back_up", "wait"] + recovery_plugins: ["spin", "backup", "wait"] spin: plugin: "nav2_recoveries/Spin" backup: diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index 6f3cb4e31ca..39d1ff0e452 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -37,6 +37,7 @@ amcl: z_max: 0.05 z_rand: 0.5 z_short: 0.05 + scan_topic: scan amcl_map_client: ros__parameters: @@ -85,8 +86,21 @@ controller_server: min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" controller_plugins: ["FollowPath"] + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" @@ -145,6 +159,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 + inflation_radius: 0.55 obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True @@ -211,7 +226,7 @@ recoveries_server: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "back_up", "wait"] + recovery_plugins: ["spin", "backup", "wait"] spin: plugin: "nav2_recoveries/Spin" backup: diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 137cec171c7..bae3aa36c03 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -37,6 +37,7 @@ amcl: z_max: 0.05 z_rand: 0.5 z_short: 0.05 + scan_topic: scan amcl_map_client: ros__parameters: @@ -52,6 +53,9 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -68,6 +72,8 @@ bt_navigator: - nav2_rate_controller_bt_node - nav2_distance_controller_bt_node - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_round_robin_node_bt_node @@ -86,8 +92,21 @@ controller_server: min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" controller_plugins: ["FollowPath"] + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" @@ -148,20 +167,11 @@ local_costmap: height: 3 resolution: 0.05 robot_radius: 0.22 - plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"] + plugins: ["voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" + inflation_radius: 0.55 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -171,13 +181,13 @@ local_costmap: z_voxels: 16 max_obstacle_height: 2.0 mark_threshold: 0 - observation_sources: pointcloud - pointcloud: - topic: /intel_realsense_r200_depth/points + observation_sources: scan + scan: + topic: /scan max_obstacle_height: 2.0 clearing: True marking: True - data_type: "PointCloud2" + data_type: "LaserScan" static_layer: map_subscribe_transient_local: True always_send_full_costmap: True @@ -198,7 +208,8 @@ global_costmap: use_sim_time: True robot_radius: 0.22 resolution: 0.05 - plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True @@ -209,27 +220,13 @@ global_costmap: clearing: True marking: True data_type: "LaserScan" - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: pointcloud - pointcloud: - topic: /intel_realsense_r200_depth/points - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "PointCloud2" static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 always_send_full_costmap: True global_costmap_client: ros__parameters: @@ -249,6 +246,7 @@ map_saver: save_map_timeout: 5000 free_thresh_default: 0.25 occupied_thresh_default: 0.65 + map_subscribe_transient_local: False planner_server: ros__parameters: @@ -270,10 +268,10 @@ recoveries_server: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] + recovery_plugins: ["spin", "back_up", "wait"] spin: plugin: "nav2_recoveries/Spin" - backup: + back_up: plugin: "nav2_recoveries/BackUp" wait: plugin: "nav2_recoveries/Wait" diff --git a/nav2_bringup/bringup/rviz/nav2_default_view.rviz b/nav2_bringup/bringup/rviz/nav2_default_view.rviz index 04e989eb170..c07e3e0cba6 100644 --- a/nav2_bringup/bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/bringup/rviz/nav2_default_view.rviz @@ -249,6 +249,20 @@ Visualization Manager: Value: /global_costmap/costmap Use Timestamp: false Value: true + - Alpha: 0.3 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Downsampled Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap + Use Timestamp: false + Value: true - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path diff --git a/nav2_bringup/bringup/worlds/waffle.model b/nav2_bringup/bringup/worlds/waffle.model index 74af1c7670e..bc984be6c45 100644 --- a/nav2_bringup/bringup/worlds/waffle.model +++ b/nav2_bringup/bringup/worlds/waffle.model @@ -266,7 +266,7 @@ 0.0 0.144 0.023 0 0 0 - model://turtlebot3_waffle/meshes/left_tire.dae + model://turtlebot3_waffle/meshes/tire.dae 0.001 0.001 0.001 @@ -324,7 +324,7 @@ 0.0 -0.144 0.023 0 0 0 - model://turtlebot3_waffle/meshes/right_tire.dae + model://turtlebot3_waffle/meshes/tire.dae 0.001 0.001 0.001 diff --git a/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py b/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py index 5f4de1d913f..05cc3c6dbc2 100644 --- a/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py +++ b/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py @@ -36,6 +36,8 @@ def main(): help='the y component of the initial position [meters]') parser.add_argument('-z', type=float, default=0, help='the z component of the initial position [meters]') + parser.add_argument('-k', '--timeout', type=float, default=10.0, + help="Seconds to wait. Block until the future is complete if negative. Don't wait if 0.") group = parser.add_mutually_exclusive_group(required=True) group.add_argument('-t', '--turtlebot_type', type=str, @@ -97,7 +99,7 @@ def main(): node.get_logger().info('Sending service request to `/spawn_entity`') future = client.call_async(request) - rclpy.spin_until_future_complete(node, future) + rclpy.spin_until_future_complete(node, future, args.timeout) if future.result() is not None: print('response: %r' % future.result()) else: diff --git a/nav2_bringup/nav2_gazebo_spawner/package.xml b/nav2_bringup/nav2_gazebo_spawner/package.xml index d80f199ec2f..c69941fb2a9 100644 --- a/nav2_bringup/nav2_gazebo_spawner/package.xml +++ b/nav2_bringup/nav2_gazebo_spawner/package.xml @@ -2,7 +2,7 @@ nav2_gazebo_spawner - 0.3.4 + 0.4.7 Package for spawning a robot model into Gazebo for navigation2 lkumarbe lkumarbe diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index da362c91068..22d8f4884cc 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -82,5 +82,6 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) - +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_bt_navigator/README.md b/nav2_bt_navigator/README.md index c0aa7ea66ca..f0864df97ad 100644 --- a/nav2_bt_navigator/README.md +++ b/nav2_bt_navigator/README.md @@ -24,8 +24,9 @@ A Behavior Tree consists of control flow nodes, such as fallback, sequence, para ## Navigation Behavior Trees -The BT Navigator package has three sample XML-based descriptions of BTs. -These trees are [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml), [navigate_w_replanning_distance.xml](behavior_trees/navigate_w_replanning_distance.xml) and [navigate_w_replanning_and_recovery.xml](behavior_trees/navigate_w_replanning_and_recovery.xml). +The BT Navigator package has four sample XML-based descriptions of BTs. +These trees are [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml), [navigate_w_replanning_distance.xml](behavior_trees/navigate_w_replanning_distance.xml), [navigate_w_replanning_and_recovery.xml](behavior_trees/navigate_w_replanning_and_recovery.xml) and +[followpoint.xml](behavior_trees/followpoint.xml). The user may use any of these sample trees or develop a more complex tree which could better suit the user's needs. ### Navigate with Replanning (time-based) @@ -66,6 +67,9 @@ returns FAILURE, all nodes are halted and this node returns FAILURE. * SpeedController: A custom control flow node, which controls the tick rate based on the current speed. This decorator offers the most flexibility as the user can set the minimum/maximum tick rate which is adjusted according to the current speed. +* GoalUpdater: A custom control node, which updates the goal pose. It subscribes to a topic in which it can receive an updated goal pose to use instead of the one commanded in action. It is useful for dynamic object following tasks. + + #### Condition Nodes * GoalReached: Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, which in that case the `ComputePathToPose` action node will not get ticked. @@ -81,6 +85,9 @@ The graphical version of this Behavior Tree: The navigate with replanning BT first ticks the `RateController` node which specifies how frequently the `GoalReached` and `ComputePathToPose` should be invoked. Then the `GoalReached` nodes check the distance to the goal to determine if the `ComputePathToPose` should be ticked or not. The `ComputePathToPose` gets the incoming goal pose from the blackboard, computes the path and puts the result back on the blackboard, where `FollowPath` picks it up. Each time a new path is computed, the blackboard gets updated and then `FollowPath` picks up the new goal to compute a control effort for. `controller_id` will specify the type of control effort you'd like to compute such as `FollowPath` `FollowPathSlow` `FollowPathExactly`, etc. + +* TruncatePath: A custom control node, which modifies a path making it shorter. It removes parts of the path closer than a distance to the goal pose. The resulting last pose of the path orientates the robot to the original goal pose. + ### Navigate with replanning and simple recovery actions With the recovery node, simple recoverable navigation with replanning can be implemented by utilizing the [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml) and a sequence of recovery actions. Our custom behavior actions for recovery are: `clearEntirelyCostmapServiceRequest` for both global and local costmaps, `spin` and `wait`. A graphical version of this simple recoverable Behavior Tree is depicted in the figure below. @@ -118,6 +125,13 @@ In the navigation stack, multi-scope recovery actions are implemented for each m This tree currently is not our default tree in the stack. The xml file is located here: [navigate_w_replanning_and_round_robin_recovery.xml](behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml). +### Navigate following a dynamic point to a certain distance +This tree is an example of how behavior trees can be used to make the robot do more than navigate the current position to the desired pose. In this case, the robot will follow a point that changes dynamically during the execution of the action. The robot will stop its advance and will be oriented towards the target position when it reaches a distance to the target established in the tree. The UpdateGoal decorator node will be used to update the target position. The TruncatePath node will modify the generated path by removing the end part of this path, to maintain a distance from the target, and changes the orientation of the last position. This tree never returns that the action has finished successfully, but must be canceled when you want to stop following the target position. + +![Navigate following a dynamic point to a certain distance](./doc/follow_point.png) + +This tree currently is not our default tree in the stack. The xml file is located here: [follow_point.xml](behavior_trees/follow_point.xml). +
## Open Issues diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml new file mode 100644 index 00000000000..ffae8d9b2ab --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/follow_point.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + diff --git a/nav2_bt_navigator/doc/follow_point.png b/nav2_bt_navigator/doc/follow_point.png new file mode 100644 index 00000000000..825bc4e16c8 Binary files /dev/null and b/nav2_bt_navigator/doc/follow_point.png differ diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 76e63c7d5da..5c87558a2ed 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -82,11 +82,6 @@ class BtNavigator : public nav2_util::LifecycleNode * @return SUCCESS or FAILURE */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when in error state - * @param state Reference to LifeCycle node state - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; using Action = nav2_msgs::action::NavigateToPose; diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 1639ca7edec..35280452472 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 0.3.4 + 0.4.7 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 f0c063c79a6..e44e64887e7 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -51,12 +52,18 @@ BtNavigator::BtNavigator() "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_speed_controller_bt_node", + "nav2_truncate_path_action_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node", "nav2_time_expired_condition_bt_node", - "nav2_distance_traveled_condition_bt_node" + "nav2_distance_traveled_condition_bt_node", + "nav2_rotate_action_bt_node", + "nav2_translate_action_bt_node", + "nav2_is_battery_low_condition_bt_node", + "nav2_goal_updater_node_bt_node", + "nav2_navigate_to_pose_action_bt_node", }; // Declare this node's parameters @@ -66,6 +73,9 @@ BtNavigator::BtNavigator() declare_parameter("global_frame", std::string("map")); declare_parameter("robot_base_frame", std::string("base_link")); declare_parameter("odom_topic", std::string("odom")); + declare_parameter("enable_groot_monitoring", true); + declare_parameter("groot_zmq_publisher_port", 1666); + declare_parameter("groot_zmq_server_port", 1667); } BtNavigator::~BtNavigator() @@ -144,9 +154,13 @@ BtNavigator::loadBehaviorTree(const std::string & bt_xml_filename) { // Use previous BT if it is the existing one if (current_bt_xml_filename_ == bt_xml_filename) { + RCLCPP_DEBUG(get_logger(), "BT will not be reloaded as the given xml is already loaded"); return true; } + // if a new tree is created, than the ZMQ Publisher must be destroyed + bt_->resetGrootMonitor(); + // Read the input BT XML from the specified file into a string std::ifstream xml_file(bt_xml_filename); @@ -159,13 +173,21 @@ BtNavigator::loadBehaviorTree(const std::string & bt_xml_filename) std::istreambuf_iterator(xml_file), std::istreambuf_iterator()); - RCLCPP_DEBUG(get_logger(), "Behavior Tree file: '%s'", bt_xml_filename.c_str()); - RCLCPP_DEBUG(get_logger(), "Behavior Tree XML: %s", xml_string.c_str()); - // Create the Behavior Tree from the XML input - tree_ = bt_->buildTreeFromText(xml_string, blackboard_); + tree_ = bt_->createTreeFromText(xml_string, blackboard_); current_bt_xml_filename_ = bt_xml_filename; + // get parameter for monitoring with Groot via ZMQ Publisher + if (get_parameter("enable_groot_monitoring").as_bool()) { + uint16_t zmq_publisher_port = get_parameter("groot_zmq_publisher_port").as_int(); + uint16_t zmq_server_port = get_parameter("groot_zmq_server_port").as_int(); + // optionally add max_msg_per_second = 25 (default) here + try { + bt_->addGrootMonitoring(&tree_, zmq_publisher_port, zmq_server_port); + } catch (const std::logic_error & e) { + RCLCPP_ERROR(get_logger(), "ZMQ already enabled, Error: %s", e.what()); + } + } return true; } @@ -206,21 +228,16 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) action_server_.reset(); plugin_lib_names_.clear(); + current_bt_xml_filename_.clear(); blackboard_.reset(); bt_->haltAllActions(tree_.rootNode()); + bt_->resetGrootMonitor(); bt_.reset(); RCLCPP_INFO(get_logger(), "Completed Cleaning up"); return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -BtNavigator::on_error(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn BtNavigator::on_shutdown(const rclcpp_lifecycle::State & /*state*/) { @@ -247,6 +264,19 @@ BtNavigator::navigateToPose() return action_server_->is_cancel_requested(); }; + std::string bt_xml_filename = action_server_->get_current_goal()->behavior_tree; + + // Empty id in request is default for backward compatibility + bt_xml_filename = bt_xml_filename == "" ? default_bt_xml_filename_ : bt_xml_filename; + + if (!loadBehaviorTree(bt_xml_filename)) { + RCLCPP_ERROR( + get_logger(), "BT file not found: %s. Navigation canceled.", + bt_xml_filename.c_str()); + action_server_->terminate_current(); + return; + } + RosTopicLogger topic_logger(client_node_, tree_); std::shared_ptr feedback_msg = std::make_shared(); @@ -276,19 +306,6 @@ BtNavigator::navigateToPose() action_server_->publish_feedback(feedback_msg); }; - auto bt_xml_filename = action_server_->get_current_goal()->behavior_tree; - - // Empty id in request is default for backward compatibility - bt_xml_filename = bt_xml_filename == "" ? default_bt_xml_filename_ : bt_xml_filename; - - if (!loadBehaviorTree(bt_xml_filename)) { - RCLCPP_ERROR( - get_logger(), "BT file not found: %s. Navigation canceled", - bt_xml_filename.c_str(), current_bt_xml_filename_.c_str()); - action_server_->terminate_current(); - return; - } - // Execute the BT that was previously created in the configure step nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling); // Make sure that the Bt is not in a running state from a previous execution @@ -310,9 +327,6 @@ BtNavigator::navigateToPose() RCLCPP_INFO(get_logger(), "Navigation canceled"); action_server_->terminate_all(); break; - - default: - throw std::logic_error("Invalid status return from BT"); } } diff --git a/nav2_bt_navigator/src/ros_topic_logger.cpp b/nav2_bt_navigator/src/ros_topic_logger.cpp index 472b30d323b..5b248014e81 100644 --- a/nav2_bt_navigator/src/ros_topic_logger.cpp +++ b/nav2_bt_navigator/src/ros_topic_logger.cpp @@ -39,12 +39,7 @@ void RosTopicLogger::callback( // BT timestamps are a duration since the epoch. Need to convert to a time_point // before converting to a msg. -#ifndef _WIN32 - event.timestamp = - tf2_ros::toMsg(std::chrono::time_point(timestamp)); -#else - event.timestamp = tf2_ros::toMsg(timestamp); -#endif + event.timestamp = tf2_ros::toMsg(tf2::TimePoint(timestamp)); event.node_name = node.name(); event.previous_status = toStr(prev_status, false); event.current_status = toStr(status, false); diff --git a/nav2_common/nav2_common/launch/__init__.py b/nav2_common/nav2_common/launch/__init__.py index 1f0638f81a0..c25fadb220c 100644 --- a/nav2_common/nav2_common/launch/__init__.py +++ b/nav2_common/nav2_common/launch/__init__.py @@ -12,5 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. +from .has_node_params import HasNodeParams from .rewritten_yaml import RewrittenYaml from .replace_string import ReplaceString diff --git a/nav2_common/nav2_common/launch/has_node_params.py b/nav2_common/nav2_common/launch/has_node_params.py new file mode 100644 index 00000000000..21b8a32f9e2 --- /dev/null +++ b/nav2_common/nav2_common/launch/has_node_params.py @@ -0,0 +1,61 @@ +# Copyright (c) 2021 PAL Robotics S.L. +# +# 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 typing import List +from typing import Text + +import yaml +import launch + + +class HasNodeParams(launch.Substitution): + """ + Substitution that checks if a param file contains parameters for a node. + + Used in launch system + """ + + def __init__(self, + source_file: launch.SomeSubstitutionsType, + node_name: Text) -> None: + super().__init__() + """ + Construct the substitution + + :param: source_file the parameter YAML file + :param: node_name the name of the node to check + """ + + # import here to avoid loop + from launch.utilities import normalize_to_list_of_substitutions + self.__source_file = normalize_to_list_of_substitutions(source_file) + self.__node_name = node_name + + @property + def name(self) -> List[launch.Substitution]: + """Getter for name.""" + return self.__source_file + + def describe(self) -> Text: + """Return a description of this substitution as a string.""" + return '' + + def perform(self, context: launch.LaunchContext) -> Text: + yaml_filename = launch.utilities.perform_substitutions( + context, self.name) + data = yaml.safe_load(open(yaml_filename, 'r')) + + if self.__node_name in data.keys(): + return "True" + return "False" diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 4d9b86373f4..40332a64275 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -21,131 +21,166 @@ import tempfile import launch + class DictItemReference: - def __init__(self, dictionary, key): - self.dictionary = dictionary - self.dictKey = key + def __init__(self, dictionary, key): + self.dictionary = dictionary + self.dictKey = key + + def key(self): + return self.dictKey - def key(self): - return self.dictKey + def setValue(self, value): + self.dictionary[self.dictKey] = value - def setValue(self, value): - self.dictionary[self.dictKey] = value class RewrittenYaml(launch.Substitution): - """ - Substitution that modifies the given YAML file. - - Used in launch system - """ - - def __init__(self, - source_file: launch.SomeSubstitutionsType, - param_rewrites: Dict, - root_key: Optional[launch.SomeSubstitutionsType] = None, - key_rewrites: Optional[Dict] = None, - convert_types = False) -> None: - super().__init__() """ - Construct the substitution + Substitution that modifies the given YAML file. - :param: source_file the original YAML file to modify - :param: param_rewrites mappings to replace - :param: root_key if provided, the contents are placed under this key - :param: key_rewrites keys of mappings to replace - :param: convert_types whether to attempt converting the string to a number or boolean + Used in launch system """ - from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop - self.__source_file = normalize_to_list_of_substitutions(source_file) - self.__param_rewrites = {} - self.__key_rewrites = {} - self.__convert_types = convert_types - self.__root_key = None - for key in param_rewrites: - self.__param_rewrites[key] = normalize_to_list_of_substitutions(param_rewrites[key]) - if key_rewrites is not None: - for key in key_rewrites: - self.__key_rewrites[key] = normalize_to_list_of_substitutions(key_rewrites[key]) - if root_key is not None: - self.__root_key = normalize_to_list_of_substitutions(root_key) - - @property - def name(self) -> List[launch.Substitution]: - """Getter for name.""" - return self.__source_file - - def describe(self) -> Text: - """Return a description of this substitution as a string.""" - return '' - - def perform(self, context: launch.LaunchContext) -> Text: - yaml_filename = launch.utilities.perform_substitutions(context, self.name) - rewritten_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False) - param_rewrites, keys_rewrites = self.resolve_rewrites(context) - data = yaml.safe_load(open(yaml_filename, 'r')) - self.substitute_params(data, param_rewrites) - self.substitute_keys(data, keys_rewrites) - if self.__root_key is not None: - root_key = launch.utilities.perform_substitutions(context, self.__root_key) - if root_key: - data = {root_key: data} - yaml.dump(data, rewritten_yaml) - rewritten_yaml.close() - return rewritten_yaml.name - - def resolve_rewrites(self, context): - resolved_params = {} - for key in self.__param_rewrites: - resolved_params[key] = launch.utilities.perform_substitutions(context, self.__param_rewrites[key]) - resolved_keys = {} - for key in self.__key_rewrites: - resolved_keys[key] = launch.utilities.perform_substitutions(context, self.__key_rewrites[key]) - return resolved_params, resolved_keys - - def substitute_params(self, yaml, param_rewrites): - for key in self.getYamlLeafKeys(yaml): - if key.key() in param_rewrites: - raw_value = param_rewrites[key.key()] - key.setValue(self.convert(raw_value)) - - def substitute_keys(self, yaml, key_rewrites): - if len(key_rewrites) != 0: - for key, val in yaml.items(): - if isinstance(val, dict) and key in key_rewrites: - new_key = key_rewrites[key] - yaml[new_key] = yaml[key] - del yaml[key] - self.substitute_keys(val, key_rewrites) - - def getYamlLeafKeys(self, yamlData): - try: - for key in yamlData.keys(): - for k in self.getYamlLeafKeys(yamlData[key]): - yield k - yield DictItemReference(yamlData, key) - except AttributeError: - return - - def convert(self, text_value): - if self.__convert_types: - # try converting to float - try: - return float(text_value) - except ValueError: - pass - - # try converting to int - try: - return int(text_value) - except ValueError: - pass - - # try converting to bool - if text_value.lower() == "true": - return True - if text_value.lower() == "false": - return False - - #nothing else worked so fall through and return text - return text_value + def __init__(self, + source_file: launch.SomeSubstitutionsType, + param_rewrites: Dict, + root_key: Optional[launch.SomeSubstitutionsType] = None, + key_rewrites: Optional[Dict] = None, + convert_types = False) -> None: + super().__init__() + """ + Construct the substitution + + :param: source_file the original YAML file to modify + :param: param_rewrites mappings to replace + :param: root_key if provided, the contents are placed under this key + :param: key_rewrites keys of mappings to replace + :param: convert_types whether to attempt converting the string to a number or boolean + """ + + from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop + self.__source_file = normalize_to_list_of_substitutions(source_file) + self.__param_rewrites = {} + self.__key_rewrites = {} + self.__convert_types = convert_types + self.__root_key = None + for key in param_rewrites: + self.__param_rewrites[key] = normalize_to_list_of_substitutions(param_rewrites[key]) + if key_rewrites is not None: + for key in key_rewrites: + self.__key_rewrites[key] = normalize_to_list_of_substitutions(key_rewrites[key]) + if root_key is not None: + self.__root_key = normalize_to_list_of_substitutions(root_key) + + @property + def name(self) -> List[launch.Substitution]: + """Getter for name.""" + return self.__source_file + + def describe(self) -> Text: + """Return a description of this substitution as a string.""" + return '' + + def perform(self, context: launch.LaunchContext) -> Text: + yaml_filename = launch.utilities.perform_substitutions(context, self.name) + rewritten_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False) + param_rewrites, keys_rewrites = self.resolve_rewrites(context) + data = yaml.safe_load(open(yaml_filename, 'r')) + self.substitute_params(data, param_rewrites) + self.substitute_keys(data, keys_rewrites) + if self.__root_key is not None: + root_key = launch.utilities.perform_substitutions(context, self.__root_key) + if root_key: + data = {root_key: data} + yaml.dump(data, rewritten_yaml) + rewritten_yaml.close() + return rewritten_yaml.name + + def resolve_rewrites(self, context): + resolved_params = {} + for key in self.__param_rewrites: + resolved_params[key] = launch.utilities.perform_substitutions(context, self.__param_rewrites[key]) + resolved_keys = {} + for key in self.__key_rewrites: + resolved_keys[key] = launch.utilities.perform_substitutions(context, self.__key_rewrites[key]) + return resolved_params, resolved_keys + + def substitute_params(self, yaml, param_rewrites): + # substitute leaf-only parameters + for key in self.getYamlLeafKeys(yaml): + if key.key() in param_rewrites: + raw_value = param_rewrites[key.key()] + key.setValue(self.convert(raw_value)) + + # substitute total path parameters + yaml_paths = self.pathify(yaml) + for path in yaml_paths: + if path in param_rewrites: + # this is an absolute path (ex. 'key.keyA.keyB.val') + rewrite_val = param_rewrites[path] + yaml_keys = path.split('.') + yaml = self.updateYamlPathVals(yaml, yaml_keys, rewrite_val) + + + def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val): + for key in yaml_key_list: + if key == yaml_key_list[-1]: + yaml[key] = rewrite_val + break + key = yaml_key_list.pop(0) + yaml[key] = self.updateYamlPathVals(yaml.get(key, {}), yaml_key_list, rewrite_val) + + return yaml + + def substitute_keys(self, yaml, key_rewrites): + if len(key_rewrites) != 0: + for key, val in yaml.items(): + if isinstance(val, dict) and key in key_rewrites: + new_key = key_rewrites[key] + yaml[new_key] = yaml[key] + del yaml[key] + self.substitute_keys(val, key_rewrites) + + def getYamlLeafKeys(self, yamlData): + try: + for key in yamlData.keys(): + for k in self.getYamlLeafKeys(yamlData[key]): + yield k + yield DictItemReference(yamlData, key) + except AttributeError: + return + + def pathify(self, d, p=None, paths=None, joinchar='.'): + if p is None: + paths = {} + self.pathify(d, "", paths, joinchar=joinchar) + return paths + pn = p + if p != "": + pn += '.' + if isinstance(d, dict): + for k in d: + v = d[k] + self.pathify(v, pn + k, paths, joinchar=joinchar) + elif isinstance(d, list): + for idx, e in enumerate(d): + self.pathify(e, pn + str(idx), paths, joinchar=joinchar) + else: + paths[p] = d + + def convert(self, text_value): + if self.__convert_types: + # try converting to int or float + try: + return float(text_value) if '.' in text_value else int(text_value) + except ValueError: + pass + + # try converting to bool + if text_value.lower() == "true": + return True + if text_value.lower() == "false": + return False + + # nothing else worked so fall through and return text + return text_value diff --git a/nav2_common/package.xml b/nav2_common/package.xml index da741cb44a5..753275f3bda 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 0.3.4 + 0.4.7 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index b4cb1b50d03..5ca5a22605c 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -4,6 +4,7 @@ project(nav2_controller) find_package(ament_cmake REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_common REQUIRED) +find_package(angles REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(std_msgs REQUIRED) @@ -29,12 +30,12 @@ set(library_name ${executable_name}_core) add_library(${library_name} src/nav2_controller.cpp - src/progress_checker.cpp ) target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") set(dependencies + angles rclcpp rclcpp_action std_msgs @@ -46,6 +47,22 @@ set(dependencies pluginlib ) +add_library(simple_progress_checker SHARED plugins/simple_progress_checker.cpp) +ament_target_dependencies(simple_progress_checker ${dependencies}) +# prevent pluginlib from using boost +target_compile_definitions(simple_progress_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +add_library(simple_goal_checker SHARED plugins/simple_goal_checker.cpp) +ament_target_dependencies(simple_goal_checker ${dependencies}) +# prevent pluginlib from using boost +target_compile_definitions(simple_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +add_library(stopped_goal_checker SHARED plugins/stopped_goal_checker.cpp) +target_link_libraries(stopped_goal_checker simple_goal_checker) +ament_target_dependencies(stopped_goal_checker ${dependencies}) +# prevent pluginlib from using boost +target_compile_definitions(stopped_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + ament_target_dependencies(${library_name} ${dependencies} ) @@ -53,13 +70,23 @@ ament_target_dependencies(${library_name} # prevent pluginlib from using boost target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(plugins/test) +endif() + ament_target_dependencies(${executable_name} ${dependencies} ) target_link_libraries(${executable_name} ${library_name}) -install(TARGETS ${library_name} +install(TARGETS simple_progress_checker simple_goal_checker stopped_goal_checker ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -79,6 +106,10 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(${library_name}) +ament_export_libraries(simple_progress_checker + simple_goal_checker + stopped_goal_checker + ${library_name}) +pluginlib_export_plugin_description_file(nav2_core plugins.xml) ament_package() diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp index b0b28b462a1..f59b65b9999 100644 --- a/nav2_controller/include/nav2_controller/nav2_controller.hpp +++ b/nav2_controller/include/nav2_controller/nav2_controller.hpp @@ -22,6 +22,8 @@ #include #include "nav2_core/controller.hpp" +#include "nav2_core/progress_checker.hpp" +#include "nav2_core/goal_checker.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "tf2_ros/transform_listener.h" #include "nav2_msgs/action/follow_path.hpp" @@ -100,12 +102,6 @@ class ControllerServer : public nav2_util::LifecycleNode * @return Success or Failure */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when in Error state - * @param state LifeCycle Node's state - * @return Success or Failure - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; using Action = nav2_msgs::action::FollowPath; using ActionServer = nav2_util::SimpleActionServer; @@ -201,6 +197,22 @@ class ControllerServer : public nav2_util::LifecycleNode std::unique_ptr odom_sub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_publisher_; + // Progress Checker Plugin + pluginlib::ClassLoader progress_checker_loader_; + nav2_core::ProgressChecker::Ptr progress_checker_; + std::string default_progress_checker_id_; + std::string default_progress_checker_type_; + std::string progress_checker_id_; + std::string progress_checker_type_; + + // Goal Checker Plugin + pluginlib::ClassLoader goal_checker_loader_; + nav2_core::GoalChecker::Ptr goal_checker_; + std::string default_goal_checker_id_; + std::string default_goal_checker_type_; + std::string goal_checker_id_; + std::string goal_checker_type_; + // Controller Plugins pluginlib::ClassLoader lp_loader_; ControllerMap controllers_; @@ -210,15 +222,12 @@ class ControllerServer : public nav2_util::LifecycleNode std::vector controller_types_; std::string controller_ids_concat_, current_controller_; - std::unique_ptr progress_checker_; - double controller_frequency_; double min_x_velocity_threshold_; double min_y_velocity_threshold_; double min_theta_velocity_threshold_; // Whether we've published the single controller warning yet - bool single_controller_warning_given_{false}; geometry_msgs::msg::Pose end_pose_; }; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp similarity index 92% rename from nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp rename to nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index 697a0dc10e4..b7e483c1611 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -32,8 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DWB_PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ -#define DWB_PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ +#ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ #include #include @@ -42,7 +42,7 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "nav2_core/goal_checker.hpp" -namespace dwb_plugins +namespace nav2_controller { /** @@ -72,6 +72,6 @@ class SimpleGoalChecker : public nav2_core::GoalChecker double xy_goal_tolerance_sq_; }; -} // namespace dwb_plugins +} // namespace nav2_controller -#endif // DWB_PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ +#endif // NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ diff --git a/nav2_controller/include/nav2_controller/progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp similarity index 63% rename from nav2_controller/include/nav2_controller/progress_checker.hpp rename to nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp index 3d754ad7aa2..92a5374a5e0 100644 --- a/nav2_controller/include/nav2_controller/progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp @@ -12,40 +12,32 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_ -#define NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_ +#ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_ +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_core/progress_checker.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" namespace nav2_controller { /** - * @class nav2_controller::ProgressChecker - * @brief This class is used to check the position of the robot to make sure - * that it is actually progressing towards a goal. - */ -class ProgressChecker +* @class SimpleProgressChecker +* @brief This plugin is used to check the position of the robot to make sure +* that it is actually progressing towards a goal. +*/ + +class SimpleProgressChecker : public nav2_core::ProgressChecker { public: - /** - * @brief Constructor of ProgressChecker - * @param node Node pointer - */ - explicit ProgressChecker(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node); - /** - * @brief Checks if the robot has moved compare to previous - * pose - * @param current_pose Current pose of the robot - * @throw nav2_core::PlannerException when failed to make progress - */ - void check(geometry_msgs::msg::PoseStamped & current_pose); - /** - * @brief Reset class state upon calling - */ - void reset() {baseline_pose_set_ = false;} + void initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_name) override; + bool check(geometry_msgs::msg::PoseStamped & current_pose) override; + void reset() override; protected: /** @@ -72,4 +64,4 @@ class ProgressChecker }; } // namespace nav2_controller -#endif // NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_ +#endif // NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_ diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp similarity index 88% rename from nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp rename to nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp index 23f727d9456..869edb6330b 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp @@ -32,17 +32,17 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DWB_PLUGINS__STOPPED_GOAL_CHECKER_HPP_ -#define DWB_PLUGINS__STOPPED_GOAL_CHECKER_HPP_ +#ifndef NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_ #include #include #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "dwb_plugins/simple_goal_checker.hpp" +#include "nav2_controller/plugins/simple_goal_checker.hpp" -namespace dwb_plugins +namespace nav2_controller { /** @@ -65,6 +65,6 @@ class StoppedGoalChecker : public SimpleGoalChecker double rot_stopped_velocity_, trans_stopped_velocity_; }; -} // namespace dwb_plugins +} // namespace nav2_controller -#endif // DWB_PLUGINS__STOPPED_GOAL_CHECKER_HPP_ +#endif // NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_ diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 5bbe0ee342e..0e38b5994da 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,13 +2,14 @@ nav2_controller - 0.3.4 + 0.4.7 Controller action interface Carl Delsey Apache-2.0 ament_cmake nav2_common + angles rclcpp rclcpp_action std_msgs @@ -24,5 +25,6 @@ ament_cmake + diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml new file mode 100644 index 00000000000..ad27127c03d --- /dev/null +++ b/nav2_controller/plugins.xml @@ -0,0 +1,17 @@ + + + + Checks if distance between current and previous pose is above a threshold + + + + + Checks if current pose is within goal window for x,y and yaw + + + + + Checks linear and angular velocity after stopping + + + diff --git a/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp similarity index 94% rename from nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp rename to nav2_controller/plugins/simple_goal_checker.cpp index b108b551989..f1ec7dba6ca 100644 --- a/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -34,7 +34,7 @@ #include #include -#include "dwb_plugins/simple_goal_checker.hpp" +#include "nav2_controller/plugins/simple_goal_checker.hpp" #include "pluginlib/class_list_macros.hpp" #include "angles/angles.h" #include "nav2_util/node_utils.hpp" @@ -43,7 +43,7 @@ #include "tf2/utils.h" #pragma GCC diagnostic pop -namespace dwb_plugins +namespace nav2_controller { SimpleGoalChecker::SimpleGoalChecker() @@ -103,6 +103,6 @@ bool SimpleGoalChecker::isGoalReached( return fabs(dyaw) < yaw_goal_tolerance_; } -} // namespace dwb_plugins +} // namespace nav2_controller -PLUGINLIB_EXPORT_CLASS(dwb_plugins::SimpleGoalChecker, nav2_core::GoalChecker) +PLUGINLIB_EXPORT_CLASS(nav2_controller::SimpleGoalChecker, nav2_core::GoalChecker) diff --git a/nav2_controller/src/progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp similarity index 65% rename from nav2_controller/src/progress_checker.cpp rename to nav2_controller/plugins/simple_progress_checker.cpp index 68fe96de50c..498d13256cb 100644 --- a/nav2_controller/src/progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -12,33 +12,37 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nav2_controller/progress_checker.hpp" +#include "nav2_controller/plugins/simple_progress_checker.hpp" #include +#include #include "nav2_core/exceptions.hpp" #include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" #include "nav2_util/node_utils.hpp" +#include "pluginlib/class_list_macros.hpp" namespace nav2_controller { static double pose_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &); -ProgressChecker::ProgressChecker(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) -: nh_(node) +void SimpleProgressChecker::initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_name) { + nh_ = node; nav2_util::declare_parameter_if_not_declared( - nh_, "required_movement_radius", rclcpp::ParameterValue(0.5)); + nh_, plugin_name + ".required_movement_radius", rclcpp::ParameterValue(0.5)); nav2_util::declare_parameter_if_not_declared( - nh_, "movement_time_allowance", rclcpp::ParameterValue(10.0)); + nh_, plugin_name + ".movement_time_allowance", rclcpp::ParameterValue(10.0)); // Scale is set to 0 by default, so if it was not set otherwise, set to 0 - nh_->get_parameter_or("required_movement_radius", radius_, 0.5); + nh_->get_parameter_or(plugin_name + ".required_movement_radius", radius_, 0.5); double time_allowance_param = 0.0; - nh_->get_parameter_or("movement_time_allowance", time_allowance_param, 10.0); + nh_->get_parameter_or(plugin_name + ".movement_time_allowance", time_allowance_param, 10.0); time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param); } -void ProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) +bool SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) { // relies on short circuit evaluation to not call is_robot_moved_enough if // baseline_pose is not set. @@ -47,21 +51,27 @@ void ProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) if ((!baseline_pose_set_) || (is_robot_moved_enough(current_pose2d))) { reset_baseline_pose(current_pose2d); - return; + return true; } if ((nh_->now() - baseline_time_) > time_allowance_) { - throw nav2_core::PlannerException("Failed to make progress"); + return false; } + return true; +} + +void SimpleProgressChecker::reset() +{ + baseline_pose_set_ = false; } -void ProgressChecker::reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose) +void SimpleProgressChecker::reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose) { baseline_pose_ = pose; baseline_time_ = nh_->now(); baseline_pose_set_ = true; } -bool ProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose) +bool SimpleProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose) { if (pose_distance(pose, baseline_pose_) > radius_) { return true; @@ -81,3 +91,5 @@ static double pose_distance( } } // namespace nav2_controller + +PLUGINLIB_EXPORT_CLASS(nav2_controller::SimpleProgressChecker, nav2_core::ProgressChecker) diff --git a/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp similarity index 93% rename from nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp rename to nav2_controller/plugins/stopped_goal_checker.cpp index c94948e8967..2e24ad181aa 100644 --- a/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp +++ b/nav2_controller/plugins/stopped_goal_checker.cpp @@ -35,14 +35,14 @@ #include #include #include -#include "dwb_plugins/stopped_goal_checker.hpp" +#include "nav2_controller/plugins/stopped_goal_checker.hpp" #include "pluginlib/class_list_macros.hpp" #include "nav2_util/node_utils.hpp" using std::hypot; using std::fabs; -namespace dwb_plugins +namespace nav2_controller { StoppedGoalChecker::StoppedGoalChecker() @@ -80,6 +80,6 @@ bool StoppedGoalChecker::isGoalReached( hypot(velocity.linear.x, velocity.linear.y) <= trans_stopped_velocity_; } -} // namespace dwb_plugins +} // namespace nav2_controller -PLUGINLIB_EXPORT_CLASS(dwb_plugins::StoppedGoalChecker, nav2_core::GoalChecker) +PLUGINLIB_EXPORT_CLASS(nav2_controller::StoppedGoalChecker, nav2_core::GoalChecker) diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt new file mode 100644 index 00000000000..d4f34d3918f --- /dev/null +++ b/nav2_controller/plugins/test/CMakeLists.txt @@ -0,0 +1,4 @@ +ament_add_gtest(pctest progress_checker.cpp) +target_link_libraries(pctest simple_progress_checker) +ament_add_gtest(gctest goal_checker.cpp) +target_link_libraries(gctest simple_goal_checker stopped_goal_checker) diff --git a/nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp similarity index 94% rename from nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp rename to nav2_controller/plugins/test/goal_checker.cpp index 3e5b5e0357b..fe175db4450 100644 --- a/nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -36,13 +36,13 @@ #include #include "gtest/gtest.h" -#include "dwb_plugins/simple_goal_checker.hpp" -#include "dwb_plugins/stopped_goal_checker.hpp" +#include "nav2_controller/plugins/simple_goal_checker.hpp" +#include "nav2_controller/plugins/stopped_goal_checker.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav2_util/lifecycle_node.hpp" -using dwb_plugins::SimpleGoalChecker; -using dwb_plugins::StoppedGoalChecker; +using nav2_controller::SimpleGoalChecker; +using nav2_controller::StoppedGoalChecker; void checkMacro( nav2_core::GoalChecker & gc, @@ -151,8 +151,8 @@ TEST(VelocityIterator, two_checks) SimpleGoalChecker gc; StoppedGoalChecker sgc; - gc.initialize(x, "dwb"); - sgc.initialize(x, "dwb"); + gc.initialize(x, "nav2_controller"); + sgc.initialize(x, "nav2_controller"); sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true); sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, false); sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, false); diff --git a/nav2_controller/plugins/test/progress_checker.cpp b/nav2_controller/plugins/test/progress_checker.cpp new file mode 100644 index 00000000000..418ff1032ae --- /dev/null +++ b/nav2_controller/plugins/test/progress_checker.cpp @@ -0,0 +1,135 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 copyright holder 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 HOLDER 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. + */ + +#include +#include + +#include "gtest/gtest.h" +#include "nav2_controller/plugins/simple_progress_checker.hpp" +#include "nav_2d_utils/conversions.hpp" +#include "nav2_util/lifecycle_node.hpp" + +using nav2_controller::SimpleProgressChecker; + +class TestLifecycleNode : public nav2_util::LifecycleNode +{ +public: + explicit TestLifecycleNode(const std::string & name) + : nav2_util::LifecycleNode(name) + { + } + + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } +}; + +void checkMacro( + nav2_core::ProgressChecker & pc, + double x0, double y0, + double x1, double y1, + int delay, + bool expected_result) +{ + pc.reset(); + geometry_msgs::msg::PoseStamped pose0, pose1; + pose0.pose.position.x = x0; + pose0.pose.position.y = y0; + pose1.pose.position.x = x1; + pose1.pose.position.y = y1; + EXPECT_TRUE(pc.check(pose0)); + rclcpp::sleep_for(std::chrono::seconds(delay)); + if (expected_result) { + EXPECT_TRUE(pc.check(pose1)); + } else { + EXPECT_FALSE(pc.check(pose1)); + } +} + +TEST(SimpleProgressChecker, progress_checker_reset) +{ + auto x = std::make_shared("progress_checker"); + + nav2_core::ProgressChecker * pc = new SimpleProgressChecker; + pc->reset(); + delete pc; + EXPECT_TRUE(true); +} + +TEST(SimpleProgressChecker, unit_tests) +{ + auto x = std::make_shared("progress_checker"); + + SimpleProgressChecker pc; + pc.initialize(x, "nav2_controller"); + checkMacro(pc, 0, 0, 0, 0, 1, true); + checkMacro(pc, 0, 0, 1, 0, 1, true); + checkMacro(pc, 0, 0, 0, 1, 1, true); + checkMacro(pc, 0, 0, 1, 0, 11, true); + checkMacro(pc, 0, 0, 0, 1, 11, true); + checkMacro(pc, 0, 0, 0, 0, 11, false); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index fc7c36e2737..dd938c4dbce 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -20,9 +20,9 @@ #include "nav2_core/exceptions.hpp" #include "nav_2d_utils/conversions.hpp" +#include "nav_2d_utils/tf_help.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" -#include "nav2_controller/progress_checker.hpp" #include "nav2_controller/nav2_controller.hpp" using namespace std::chrono_literals; @@ -32,6 +32,12 @@ namespace nav2_controller ControllerServer::ControllerServer() : LifecycleNode("controller_server", "", true), + progress_checker_loader_("nav2_core", "nav2_core::ProgressChecker"), + default_progress_checker_id_{"progress_checker"}, + default_progress_checker_type_{"nav2_controller::SimpleProgressChecker"}, + goal_checker_loader_("nav2_core", "nav2_core::GoalChecker"), + default_goal_checker_id_{"goal_checker"}, + default_goal_checker_type_{"nav2_controller::SimpleGoalChecker"}, lp_loader_("nav2_core", "nav2_core::Controller"), default_ids_{"FollowPath"}, default_types_{"dwb_core::DWBLocalPlanner"} @@ -40,6 +46,8 @@ ControllerServer::ControllerServer() declare_parameter("controller_frequency", 20.0); + declare_parameter("progress_checker_plugin", default_progress_checker_id_); + declare_parameter("goal_checker_plugin", default_goal_checker_id_); declare_parameter("controller_plugins", default_ids_); declare_parameter("min_x_velocity_threshold", rclcpp::ParameterValue(0.0001)); declare_parameter("min_y_velocity_threshold", rclcpp::ParameterValue(0.0001)); @@ -61,12 +69,29 @@ ControllerServer::~ControllerServer() nav2_util::CallbackReturn ControllerServer::on_configure(const rclcpp_lifecycle::State & state) { + auto node = shared_from_this(); + RCLCPP_INFO(get_logger(), "Configuring controller interface"); + get_parameter("progress_checker_plugin", progress_checker_id_); + if (progress_checker_id_ == default_progress_checker_id_) { + nav2_util::declare_parameter_if_not_declared( + node, default_progress_checker_id_ + ".plugin", + rclcpp::ParameterValue(default_progress_checker_type_)); + } + get_parameter("goal_checker_plugin", goal_checker_id_); + if (goal_checker_id_ == default_goal_checker_id_) { + nav2_util::declare_parameter_if_not_declared( + node, default_goal_checker_id_ + ".plugin", + rclcpp::ParameterValue(default_goal_checker_type_)); + } + get_parameter("controller_plugins", controller_ids_); if (controller_ids_ == default_ids_) { for (size_t i = 0; i < default_ids_.size(); ++i) { - declare_parameter(default_ids_[i] + ".plugin", default_types_[i]); + nav2_util::declare_parameter_if_not_declared( + node, default_ids_[i] + ".plugin", + rclcpp::ParameterValue(default_types_[i])); } } controller_types_.resize(controller_ids_.size()); @@ -79,9 +104,32 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) costmap_ros_->on_configure(state); - auto node = shared_from_this(); - - progress_checker_ = std::make_unique(node); + try { + progress_checker_type_ = nav2_util::get_plugin_type_param(node, progress_checker_id_); + progress_checker_ = progress_checker_loader_.createUniqueInstance(progress_checker_type_); + RCLCPP_INFO( + get_logger(), "Created progress_checker : %s of type %s", + progress_checker_id_.c_str(), progress_checker_type_.c_str()); + progress_checker_->initialize(node, progress_checker_id_); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + get_logger(), + "Failed to create progress_checker. Exception: %s", ex.what()); + return nav2_util::CallbackReturn::FAILURE; + } + try { + goal_checker_type_ = nav2_util::get_plugin_type_param(node, goal_checker_id_); + goal_checker_ = goal_checker_loader_.createUniqueInstance(goal_checker_type_); + RCLCPP_INFO( + get_logger(), "Created goal_checker : %s of type %s", + goal_checker_id_.c_str(), goal_checker_type_.c_str()); + goal_checker_->initialize(node, goal_checker_id_); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + get_logger(), + "Failed to create goal_checker. Exception: %s", ex.what()); + return nav2_util::CallbackReturn::FAILURE; + } for (size_t i = 0; i != controller_ids_.size(); i++) { try { @@ -96,8 +144,10 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) costmap_ros_->getTfBuffer(), costmap_ros_); controllers_.insert({controller_ids_[i], controller}); } catch (const pluginlib::PluginlibException & ex) { - RCLCPP_FATAL(get_logger(), "Failed to create controller. Exception: %s", ex.what()); - exit(-1); + RCLCPP_FATAL( + get_logger(), + "Failed to create controller. Exception: %s", ex.what()); + return nav2_util::CallbackReturn::FAILURE; } } @@ -105,6 +155,10 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) controller_ids_concat_ += controller_ids_[i] + std::string(" "); } + RCLCPP_INFO( + get_logger(), + "Controller Server has %s controllers available.", controller_ids_concat_.c_str()); + odom_sub_ = std::make_unique(node); vel_publisher_ = create_publisher("cmd_vel", 1); @@ -165,24 +219,14 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state) // Release any allocated resources action_server_.reset(); - for (it = controllers_.begin(); it != controllers_.end(); ++it) { - it->second.reset(); - } odom_sub_.reset(); - vel_publisher_.reset(); action_server_.reset(); + goal_checker_->reset(); return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -ControllerServer::on_error(const rclcpp_lifecycle::State &) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn ControllerServer::on_shutdown(const rclcpp_lifecycle::State &) { @@ -196,13 +240,10 @@ bool ControllerServer::findControllerId( { if (controllers_.find(c_name) == controllers_.end()) { if (controllers_.size() == 1 && c_name.empty()) { - if (!single_controller_warning_given_) { - RCLCPP_WARN( - get_logger(), "No controller was specified in action call." - " Server will use only plugin loaded %s. " - "This warning will appear once.", controller_ids_concat_.c_str()); - single_controller_warning_given_ = true; - } + RCLCPP_WARN_ONCE( + get_logger(), "No controller was specified in action call." + " Server will use only plugin loaded %s. " + "This warning will appear once.", controller_ids_concat_.c_str()); current_controller = controllers_.begin()->first; } else { RCLCPP_ERROR( @@ -212,6 +253,7 @@ bool ControllerServer::findControllerId( return false; } } else { + RCLCPP_DEBUG(get_logger(), "Selected controller: %s.", c_name.c_str()); current_controller = c_name; } @@ -235,15 +277,10 @@ void ControllerServer::computeControl() setPlannerPath(action_server_->get_current_goal()->path); progress_checker_->reset(); - rclcpp::Rate loop_rate(controller_frequency_); + rclcpp::WallRate loop_rate(controller_frequency_); while (rclcpp::ok()) { - if (action_server_ == nullptr) { - RCLCPP_DEBUG(get_logger(), "Action server unavailable. Stopping."); - return; - } - - if (!action_server_->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "Action server is inactive. Stopping."); + if (action_server_ == nullptr || !action_server_->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); return; } @@ -276,7 +313,7 @@ void ControllerServer::computeControl() return; } - RCLCPP_DEBUG(get_logger(), "DWB succeeded, setting result"); + RCLCPP_DEBUG(get_logger(), "Controller succeeded, setting result"); publishZeroVelocity(); @@ -294,7 +331,13 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) } controllers_[current_controller_]->setPlan(path); - auto end_pose = *(path.poses.end() - 1); + auto end_pose = path.poses.back(); + end_pose.header.frame_id = path.header.frame_id; + rclcpp::Duration tolerance(costmap_ros_->getTransformTolerance() * 1e9); + nav_2d_utils::transformPose( + costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(), + end_pose, end_pose, tolerance); + goal_checker_->reset(); RCLCPP_DEBUG( get_logger(), "Path end point is (%.2f, %.2f)", @@ -310,7 +353,9 @@ void ControllerServer::computeAndPublishVelocity() throw nav2_core::PlannerException("Failed to obtain robot pose"); } - progress_checker_->check(pose); + if (!progress_checker_->check(pose)) { + throw nav2_core::PlannerException("Failed to make progress"); + } nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); @@ -350,7 +395,12 @@ void ControllerServer::updateGlobalPath() void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity) { auto cmd_vel = std::make_unique(velocity.twist); - vel_publisher_->publish(std::move(cmd_vel)); + if ( + vel_publisher_->is_activated() && + this->count_subscribers(vel_publisher_->get_topic_name()) > 0) + { + vel_publisher_->publish(std::move(cmd_vel)); + } } void ControllerServer::publishZeroVelocity() @@ -362,6 +412,8 @@ void ControllerServer::publishZeroVelocity() velocity.twist.linear.x = 0; velocity.twist.linear.y = 0; velocity.twist.linear.z = 0; + velocity.header.frame_id = costmap_ros_->getBaseFrameID(); + velocity.header.stamp = now(); publishVelocity(velocity); } @@ -375,14 +427,13 @@ bool ControllerServer::isGoalReached() nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(twist); - return controllers_[current_controller_]->isGoalReached(pose, velocity); + return goal_checker_->isGoalReached(pose.pose, end_pose_, velocity); } bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose) { geometry_msgs::msg::PoseStamped current_pose; if (!costmap_ros_->getRobotPose(current_pose)) { - RCLCPP_ERROR(this->get_logger(), "Could not get robot pose"); return false; } pose = current_pose; diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 1e6e98052c3..69aa10f25b0 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -112,20 +112,6 @@ class Controller virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity) = 0; - - /** - * @brief Controller isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity. - * - * The pose that it checks against is the last pose in the current global plan. - * The calculation is delegated to the goal_checker plugin. - * - * @param pose Current pose - * @param velocity Current velocity - * @return True if the robot should be considered as having reached the goal. - */ - virtual bool isGoalReached( - const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & velocity) = 0; }; } // namespace nav2_core diff --git a/nav2_core/include/nav2_core/progress_checker.hpp b/nav2_core/include/nav2_core/progress_checker.hpp new file mode 100644 index 00000000000..2dd9c5bef89 --- /dev/null +++ b/nav2_core/include/nav2_core/progress_checker.hpp @@ -0,0 +1,62 @@ +// Copyright (c) 2019 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. + +#ifndef NAV2_CORE__PROGRESS_CHECKER_HPP_ +#define NAV2_CORE__PROGRESS_CHECKER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" + +namespace nav2_core +{ +/** + * @class nav2_core::ProgressChecker + * @brief This class defines the plugin interface used to check the + * position of the robot to make sure that it is actually progressing + * towards a goal. + */ +class ProgressChecker +{ +public: + typedef std::shared_ptr Ptr; + + virtual ~ProgressChecker() {} + + /** + * @brief Initialize parameters for ProgressChecker + * @param node Node pointer + */ + virtual void initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_name) = 0; + /** + * @brief Checks if the robot has moved compare to previous + * pose + * @param current_pose Current pose of the robot + * @return True if progress is made + */ + virtual bool check(geometry_msgs::msg::PoseStamped & current_pose) = 0; + /** + * @brief Reset class state upon calling + */ + virtual void reset() = 0; +}; +} // namespace nav2_core + +#endif // NAV2_CORE__PROGRESS_CHECKER_HPP_ diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 5750d62b3ae..abe8c478702 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 0.3.4 + 0.4.7 A set of headers for plugins core to the navigation2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index d7508415edf..4c9a2a93666 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_sensor_msgs REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(angles REQUIRED) remove_definitions(-DDISABLE_LIBUSB-1.0) find_package(Eigen3 REQUIRED) @@ -72,6 +73,7 @@ set(dependencies tf2_ros tf2_sensor_msgs visualization_msgs + angles ) ament_target_dependencies(nav2_costmap_2d_core @@ -84,6 +86,7 @@ add_library(layers SHARED plugins/obstacle_layer.cpp src/observation_buffer.cpp plugins/voxel_layer.cpp + plugins/range_sensor_layer.cpp ) ament_target_dependencies(layers ${dependencies} diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index 62bfac732b9..a0725392332 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -12,6 +12,9 @@ Similar to obstacle costmap, but uses 3D voxel grid to store data. + + A range-sensor (sonar, IR) based obstacle layer for costmap_2d + diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index 8f9fd438677..d74895b40a2 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -125,6 +125,14 @@ class Costmap2D */ unsigned char getCost(unsigned int mx, unsigned int my) const; + /** + * @brief Get the cost of a cell in the costmap + * @param mx The x coordinate of the cell + * @param my The y coordinate of the cell + * @return The cost of the cell + */ + unsigned char getCost(unsigned int index) const; + /** * @brief Set the cost of a cell in the costmap * @param mx The x coordinate of the cell diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 5524f5737d3..124d1666a51 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -97,7 +97,6 @@ class Costmap2DROS : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; /** * @brief Subscribes to sensor topics if necessary and starts costmap @@ -249,6 +248,14 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::shared_ptr getTfBuffer() {return tf_buffer_;} + /** + * @brief Get the costmap's use_radius_ parameter, corresponding to + * whether the footprint for the robot is a circle with radius robot_radius_ + * or an arbitrarily defined footprint in footprint_. + * @return use_radius_ + */ + bool getUseRadius() {return use_radius_;} + protected: rclcpp::Node::SharedPtr client_node_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp index 636a907f076..9f54728e635 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp @@ -68,7 +68,7 @@ class CostmapTopicCollisionChecker CostmapSubscriber & costmap_sub_; FootprintSubscriber & footprint_sub_; double transform_tolerance_; - FootprintCollisionChecker collision_checker_; + FootprintCollisionChecker> collision_checker_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp index 24f35aa1359..39c51328b8d 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp @@ -32,20 +32,21 @@ namespace nav2_costmap_2d { typedef std::vector Footprint; +template class FootprintCollisionChecker { public: FootprintCollisionChecker(); - explicit FootprintCollisionChecker(std::shared_ptr costmap); + explicit FootprintCollisionChecker(CostmapT costmap); double footprintCost(const Footprint footprint); double footprintCostAtPose(double x, double y, double theta, const Footprint footprint); double lineCost(int x0, int x1, int y0, int y1) const; bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my); double pointCost(int x, int y) const; - void setCostmap(std::shared_ptr costmap); + void setCostmap(CostmapT costmap); -private: - std::shared_ptr costmap_; +protected: + CostmapT costmap_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index 08fed09145a..4acf05bd558 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -40,6 +40,7 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/layer.hpp" @@ -77,7 +78,7 @@ class InflationLayer : public Layer public: InflationLayer(); - ~InflationLayer() override = default; + ~InflationLayer(); void onInitialize() override; void updateBounds( @@ -115,6 +116,13 @@ class InflationLayer : public Layer return cost; } + // Provide a typedef to ease future code maintenance + typedef std::recursive_mutex mutex_t; + mutex_t * getMutex() + { + return access_; + } + protected: void onFootprintChanged() override; @@ -184,6 +192,7 @@ class InflationLayer : public Layer // Indicates that the entire costmap should be reinflated next time around. bool need_reinflation_; + mutex_t * access_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp index dbbfb2caa93..e3195020780 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp @@ -59,6 +59,11 @@ class Observation delete cloud_; } + /** + * @brief Explicitly define copy assignment operator for Observation as it has a user-declared destructor + */ + Observation & operator=(const Observation &) = default; + /** * @brief Creates an observation from an origin point and a point cloud * @param origin The origin point of the observation diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp new file mode 100644 index 00000000000..ce2861599d7 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp @@ -0,0 +1,138 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 David V. Lu!! + * Copyright (c) 2020, Bytes Robotics + * 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 copyright holder 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 HOLDER 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. + */ + +#ifndef NAV2_COSTMAP_2D__RANGE_SENSOR_LAYER_HPP_ +#define NAV2_COSTMAP_2D__RANGE_SENSOR_LAYER_HPP_ + +#include +#include +#include +#include + +#include "map_msgs/msg/occupancy_grid_update.hpp" +#include "message_filters/subscriber.h" +#include "nav2_costmap_2d/costmap_layer.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "sensor_msgs/msg/range.hpp" + +namespace nav2_costmap_2d +{ + +class RangeSensorLayer : public CostmapLayer +{ +public: + enum class InputSensorType + { + VARIABLE, + FIXED, + ALL + }; + + RangeSensorLayer(); + + virtual void onInitialize(); + virtual void updateBounds( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, double * max_x, double * max_y); + virtual void updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, int min_i, + int min_j, int max_i, int max_j); + virtual void reset(); + virtual void deactivate(); + virtual void activate(); + + void bufferIncomingRangeMsg(const sensor_msgs::msg::Range::SharedPtr range_message); + +private: + void updateCostmap(); + void updateCostmap(sensor_msgs::msg::Range & range_message, bool clear_sensor_cone); + + void processRangeMsg(sensor_msgs::msg::Range & range_message); + void processFixedRangeMsg(sensor_msgs::msg::Range & range_message); + void processVariableRangeMsg(sensor_msgs::msg::Range & range_message); + + void resetRange(); + + inline double gamma(double theta); + inline double delta(double phi); + inline double sensor_model(double r, double phi, double theta); + + inline void get_deltas(double angle, double * dx, double * dy); + inline void update_cell( + double ox, double oy, double ot, + double r, double nx, double ny, bool clear); + + inline double to_prob(unsigned char c) + { + return static_cast(c) / nav2_costmap_2d::LETHAL_OBSTACLE; + } + inline unsigned char to_cost(double p) + { + return static_cast(p * nav2_costmap_2d::LETHAL_OBSTACLE); + } + + std::function processRangeMessageFunc_; + std::mutex range_message_mutex_; + std::list range_msgs_buffer_; + + double max_angle_, phi_v_; + double inflate_cone_; + std::string global_frame_; + + double clear_threshold_, mark_threshold_; + bool clear_on_max_reading_; + + tf2::Duration transform_tolerance_; + double no_readings_timeout_; + rclcpp::Time last_reading_time_; + unsigned int buffered_readings_; + std::vector::SharedPtr> range_subs_; + double min_x_, min_y_, max_x_, max_y_; + + float area(int x1, int y1, int x2, int y2, int x3, int y3) + { + return fabs((x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)) / 2.0); + } + + int orient2d(int Ax, int Ay, int Bx, int By, int Cx, int Cy) + { + return (Bx - Ax) * (Cy - Ay) - (By - Ay) * (Cx - Ax); + } +}; +} // namespace nav2_costmap_2d +#endif // NAV2_COSTMAP_2D__RANGE_SENSOR_LAYER_HPP_ 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 f6f01e8c563..bbd5daa757e 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -111,6 +111,8 @@ class StaticLayer : public CostmapLayer bool trinary_costmap_; bool map_received_{false}; tf2::Duration transform_tolerance_; + std::atomic update_in_progress_; + nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index f208be38207..56881189234 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 0.3.4 + 0.4.7 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 @@ -19,6 +19,7 @@ ament_cmake nav2_common + angles geometry_msgs laser_geometry map_msgs diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 86561d334b3..13feac3cbc2 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -40,6 +40,8 @@ #include #include #include +#include +#include #include "nav2_costmap_2d/costmap_math.hpp" #include "nav2_costmap_2d/footprint.hpp" @@ -63,12 +65,19 @@ InflationLayer::InflationLayer() inflate_around_unknown_(false), cell_inflation_radius_(0), cached_cell_inflation_radius_(0), + resolution_(0), cache_length_(0), last_min_x_(std::numeric_limits::lowest()), last_min_y_(std::numeric_limits::lowest()), last_max_x_(std::numeric_limits::max()), last_max_y_(std::numeric_limits::max()) { + access_ = new mutex_t(); +} + +InflationLayer::~InflationLayer() +{ + delete access_; } void @@ -158,12 +167,13 @@ InflationLayer::updateCosts( int max_i, int max_j) { + std::lock_guard guard(*getMutex()); if (!enabled_ || (cell_inflation_radius_ == 0)) { return; } // make sure the inflation list is empty at the beginning of the cycle (should always be true) - for (auto & dist:inflation_cells_) { + for (auto & dist : inflation_cells_) { RCLCPP_FATAL_EXPRESSION( rclcpp::get_logger("nav2_costmap_2d"), !dist.empty(), "The inflation list must be empty at the beginning of inflation"); @@ -215,9 +225,10 @@ InflationLayer::updateCosts( // Process cells by increasing distance; new cells are appended to the // corresponding distance bin, so they // can overtake previously inserted but farther away cells - for (const auto & dist_bin: inflation_cells_) { + for (const auto & dist_bin : inflation_cells_) { for (std::size_t i = 0; i < dist_bin.size(); ++i) { - // Do not use iterator or for-range based loops to iterate though dist_bin, since it's size might + // Do not use iterator or for-range based loops to + // iterate though dist_bin, since it's size might // change when a new cell is enqueued, invalidating all iterators unsigned int index = dist_bin[i].index_; @@ -260,7 +271,7 @@ InflationLayer::updateCosts( } } - for (auto & dist:inflation_cells_) { + for (auto & dist : inflation_cells_) { dist.clear(); dist.reserve(200); } @@ -302,6 +313,7 @@ InflationLayer::enqueue( void InflationLayer::computeCaches() { + std::lock_guard guard(*getMutex()); if (cell_inflation_radius_ == 0) { return; } diff --git a/nav2_costmap_2d/plugins/range_sensor_layer.cpp b/nav2_costmap_2d/plugins/range_sensor_layer.cpp new file mode 100644 index 00000000000..c0cb1111555 --- /dev/null +++ b/nav2_costmap_2d/plugins/range_sensor_layer.cpp @@ -0,0 +1,524 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 David V. Lu!! + * Copyright (c) 2020, Bytes Robotics + * 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 copyright holder 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 HOLDER 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. + */ + +#include +#include +#include +#include +#include +#include + +#include "pluginlib/class_list_macros.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "nav2_costmap_2d/range_sensor_layer.hpp" + +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::RangeSensorLayer, nav2_costmap_2d::Layer) + +using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; +using nav2_costmap_2d::NO_INFORMATION; + +using namespace std::literals::chrono_literals; + +namespace nav2_costmap_2d +{ + +RangeSensorLayer::RangeSensorLayer() {} + +void RangeSensorLayer::onInitialize() +{ + current_ = true; + buffered_readings_ = 0; + last_reading_time_ = node_->now(); + default_value_ = to_cost(0.5); + + matchSize(); + resetRange(); + + declareParameter("enabled", rclcpp::ParameterValue(true)); + node_->get_parameter(name_ + "." + "enabled", enabled_); + declareParameter("phi", rclcpp::ParameterValue(1.2)); + node_->get_parameter(name_ + "." + "phi", phi_v_); + declareParameter("inflate_cone", rclcpp::ParameterValue(1.0)); + node_->get_parameter(name_ + "." + "inflate_cone", inflate_cone_); + declareParameter("no_readings_timeout", rclcpp::ParameterValue(0.0)); + node_->get_parameter(name_ + "." + "no_readings_timeout", no_readings_timeout_); + declareParameter("clear_threshold", rclcpp::ParameterValue(0.2)); + node_->get_parameter(name_ + "." + "clear_threshold", clear_threshold_); + declareParameter("mark_threshold", rclcpp::ParameterValue(0.8)); + node_->get_parameter(name_ + "." + "mark_threshold", mark_threshold_); + declareParameter("clear_on_max_reading", rclcpp::ParameterValue(false)); + node_->get_parameter(name_ + "." + "clear_on_max_reading", clear_on_max_reading_); + + double temp_tf_tol = 0.0; + node_->get_parameter("transform_tolerance", temp_tf_tol); + transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); + + std::vector topic_names{}; + declareParameter("topics", rclcpp::ParameterValue(topic_names)); + node_->get_parameter(name_ + "." + "topics", topic_names); + + InputSensorType input_sensor_type = InputSensorType::ALL; + std::string sensor_type_name; + declareParameter("input_sensor_type", rclcpp::ParameterValue("ALL")); + node_->get_parameter(name_ + "." + "input_sensor_type", sensor_type_name); + + std::transform( + sensor_type_name.begin(), sensor_type_name.end(), + sensor_type_name.begin(), ::toupper); + RCLCPP_INFO( + node_->get_logger(), "%s: %s as input_sensor_type given", + name_.c_str(), sensor_type_name.c_str()); + + if (sensor_type_name == "VARIABLE") { + input_sensor_type = InputSensorType::VARIABLE; + } else if (sensor_type_name == "FIXED") { + input_sensor_type = InputSensorType::FIXED; + } else if (sensor_type_name == "ALL") { + input_sensor_type = InputSensorType::ALL; + } else { + RCLCPP_ERROR( + node_->get_logger(), "%s: Invalid input sensor type: %s. Defaulting to ALL.", + name_.c_str(), sensor_type_name.c_str()); + } + + // Validate topic names list: it must be a (normally non-empty) list of strings + if (topic_names.empty()) { + RCLCPP_FATAL( + node_->get_logger(), "Invalid topic names list: it must" + "be a non-empty list of strings"); + return; + } + + // Traverse the topic names list subscribing to all of them with the same callback method + for (auto & topic_name : topic_names) { + if (input_sensor_type == InputSensorType::VARIABLE) { + processRangeMessageFunc_ = std::bind( + &RangeSensorLayer::processVariableRangeMsg, this, + std::placeholders::_1); + } else if (input_sensor_type == InputSensorType::FIXED) { + processRangeMessageFunc_ = std::bind( + &RangeSensorLayer::processFixedRangeMsg, this, + std::placeholders::_1); + } else if (input_sensor_type == InputSensorType::ALL) { + processRangeMessageFunc_ = std::bind( + &RangeSensorLayer::processRangeMsg, this, + std::placeholders::_1); + } else { + RCLCPP_ERROR( + node_->get_logger(), + "%s: Invalid input sensor type: %s. Did you make a new type" + "and forgot to choose the subscriber for it?", + name_.c_str(), sensor_type_name.c_str()); + } + range_subs_.push_back( + node_->create_subscription( + topic_name, rclcpp::SensorDataQoS(), std::bind( + &RangeSensorLayer::bufferIncomingRangeMsg, this, + std::placeholders::_1))); + + RCLCPP_INFO( + node_->get_logger(), "RangeSensorLayer: subscribed to " + "topic %s", range_subs_.back()->get_topic_name()); + } + global_frame_ = layered_costmap_->getGlobalFrameID(); +} + + +double RangeSensorLayer::gamma(double theta) +{ + if (fabs(theta) > max_angle_) { + return 0.0; + } else { + return 1 - pow(theta / max_angle_, 2); + } +} + +double RangeSensorLayer::delta(double phi) +{ + return 1 - (1 + tanh(2 * (phi - phi_v_))) / 2; +} + +void RangeSensorLayer::get_deltas(double angle, double * dx, double * dy) +{ + double ta = tan(angle); + if (ta == 0) { + *dx = 0; + } else { + *dx = resolution_ / ta; + } + + *dx = copysign(*dx, cos(angle)); + *dy = copysign(resolution_, sin(angle)); +} + +double RangeSensorLayer::sensor_model(double r, double phi, double theta) +{ + double lbda = delta(phi) * gamma(theta); + + double delta = resolution_; + + if (phi >= 0.0 && phi < r - 2 * delta * r) { + return (1 - lbda) * (0.5); + } else if (phi < r - delta * r) { + return lbda * 0.5 * pow((phi - (r - 2 * delta * r)) / (delta * r), 2) + + (1 - lbda) * .5; + } else if (phi < r + delta * r) { + double J = (r - phi) / (delta * r); + return lbda * ((1 - (0.5) * pow(J, 2)) - 0.5) + 0.5; + } else { + return 0.5; + } +} + +void RangeSensorLayer::bufferIncomingRangeMsg( + const sensor_msgs::msg::Range::SharedPtr range_message) +{ + range_message_mutex_.lock(); + range_msgs_buffer_.push_back(*range_message); + range_message_mutex_.unlock(); +} + +void RangeSensorLayer::updateCostmap() +{ + std::list range_msgs_buffer_copy; + + range_message_mutex_.lock(); + range_msgs_buffer_copy = std::list(range_msgs_buffer_); + range_msgs_buffer_.clear(); + range_message_mutex_.unlock(); + + for (auto & range_msgs_it : range_msgs_buffer_copy) { + processRangeMessageFunc_(range_msgs_it); + } +} + +void RangeSensorLayer::processRangeMsg(sensor_msgs::msg::Range & range_message) +{ + if (range_message.min_range == range_message.max_range) { + processFixedRangeMsg(range_message); + } else { + processVariableRangeMsg(range_message); + } +} + +void RangeSensorLayer::processFixedRangeMsg(sensor_msgs::msg::Range & range_message) +{ + if (!std::isinf(range_message.range)) { + RCLCPP_ERROR( + node_->get_logger(), + "Fixed distance ranger (min_range == max_range) in frame %s sent invalid value. " + "Only -Inf (== object detected) and Inf (== no object detected) are valid.", + range_message.header.frame_id.c_str()); + return; + } + + bool clear_sensor_cone = false; + + if (range_message.range > 0) { // +inf + if (!clear_on_max_reading_) { + return; // no clearing at all + } + clear_sensor_cone = true; + } + + range_message.range = range_message.min_range; + + updateCostmap(range_message, clear_sensor_cone); +} + +void RangeSensorLayer::processVariableRangeMsg(sensor_msgs::msg::Range & range_message) +{ + if (range_message.range < range_message.min_range || range_message.range > + range_message.max_range) + { + return; + } + + bool clear_sensor_cone = false; + + if (range_message.range >= range_message.max_range && clear_on_max_reading_) { + clear_sensor_cone = true; + } + + updateCostmap(range_message, clear_sensor_cone); +} + +void RangeSensorLayer::updateCostmap( + sensor_msgs::msg::Range & range_message, + bool clear_sensor_cone) +{ + max_angle_ = range_message.field_of_view / 2; + + geometry_msgs::msg::PointStamped in, out; + in.header.stamp = range_message.header.stamp; + in.header.frame_id = range_message.header.frame_id; + + if (!tf_->canTransform( + in.header.frame_id, global_frame_, tf2_ros::fromMsg(in.header.stamp))) + { + RCLCPP_INFO( + node_->get_logger(), "Range sensor layer can't transform from %s to %s", + global_frame_.c_str(), in.header.frame_id.c_str()); + return; + } + + tf_->transform(in, out, global_frame_, transform_tolerance_); + + double ox = out.point.x, oy = out.point.y; + + in.point.x = range_message.range; + + tf_->transform(in, out, global_frame_, transform_tolerance_); + + double tx = out.point.x, ty = out.point.y; + + // calculate target props + double dx = tx - ox, dy = ty - oy, theta = atan2(dy, dx), d = sqrt(dx * dx + dy * dy); + + // Integer Bounds of Update + int bx0, by0, bx1, by1; + + // Triangle that will be really updated; the other cells within bounds are ignored + // This triangle is formed by the origin and left and right sides of sonar cone + int Ox, Oy, Ax, Ay, Bx, By; + + // Bounds includes the origin + worldToMapNoBounds(ox, oy, Ox, Oy); + bx1 = bx0 = Ox; + by1 = by0 = Oy; + touch(ox, oy, &min_x_, &min_y_, &max_x_, &max_y_); + + // Update Map with Target Point + unsigned int aa, ab; + if (worldToMap(tx, ty, aa, ab)) { + setCost(aa, ab, 233); + touch(tx, ty, &min_x_, &min_y_, &max_x_, &max_y_); + } + + double mx, my; + + // Update left side of sonar cone + mx = ox + cos(theta - max_angle_) * d * 1.2; + my = oy + sin(theta - max_angle_) * d * 1.2; + worldToMapNoBounds(mx, my, Ax, Ay); + bx0 = std::min(bx0, Ax); + bx1 = std::max(bx1, Ax); + by0 = std::min(by0, Ay); + by1 = std::max(by1, Ay); + touch(mx, my, &min_x_, &min_y_, &max_x_, &max_y_); + + // Update right side of sonar cone + mx = ox + cos(theta + max_angle_) * d * 1.2; + my = oy + sin(theta + max_angle_) * d * 1.2; + + worldToMapNoBounds(mx, my, Bx, By); + bx0 = std::min(bx0, Bx); + bx1 = std::max(bx1, Bx); + by0 = std::min(by0, By); + by1 = std::max(by1, By); + touch(mx, my, &min_x_, &min_y_, &max_x_, &max_y_); + + // Limit Bounds to Grid + bx0 = std::max(0, bx0); + by0 = std::max(0, by0); + bx1 = std::min(static_cast(size_x_), bx1); + by1 = std::min(static_cast(size_y_), by1); + + for (unsigned int x = bx0; x <= (unsigned int)bx1; x++) { + for (unsigned int y = by0; y <= (unsigned int)by1; y++) { + bool update_xy_cell = true; + + // Unless inflate_cone_ is set to 100 %, we update cells only within the + // (partially inflated) sensor cone, projected on the costmap as a triangle. + // 0 % corresponds to just the triangle, but if your sensor fov is very + // narrow, the covered area can become zero due to cell discretization. + // See wiki description for more details + if (inflate_cone_ < 1.0) { + // Determine barycentric coordinates + int w0 = orient2d(Ax, Ay, Bx, By, x, y); + int w1 = orient2d(Bx, By, Ox, Oy, x, y); + int w2 = orient2d(Ox, Oy, Ax, Ay, x, y); + + // Barycentric coordinates inside area threshold; this is not mathematically + // sound at all, but it works! + float bcciath = -static_cast(inflate_cone_) * area(Ax, Ay, Bx, By, Ox, Oy); + update_xy_cell = w0 >= bcciath && w1 >= bcciath && w2 >= bcciath; + } + + if (update_xy_cell) { + double wx, wy; + mapToWorld(x, y, wx, wy); + update_cell(ox, oy, theta, range_message.range, wx, wy, clear_sensor_cone); + } + } + } + + buffered_readings_++; + last_reading_time_ = node_->now(); +} + +void RangeSensorLayer::update_cell( + double ox, double oy, double ot, double r, + double nx, double ny, bool clear) +{ + unsigned int x, y; + if (worldToMap(nx, ny, x, y)) { + double dx = nx - ox, dy = ny - oy; + double theta = atan2(dy, dx) - ot; + theta = angles::normalize_angle(theta); + double phi = sqrt(dx * dx + dy * dy); + double sensor = 0.0; + if (!clear) { + sensor = sensor_model(r, phi, theta); + } + double prior = to_prob(getCost(x, y)); + double prob_occ = sensor * prior; + double prob_not = (1 - sensor) * (1 - prior); + double new_prob = prob_occ / (prob_occ + prob_not); + + RCLCPP_DEBUG(node_->get_logger(), "%f %f | %f %f = %f", dx, dy, theta, phi, sensor); + RCLCPP_DEBUG(node_->get_logger(), "%f | %f %f | %f", prior, prob_occ, prob_not, new_prob); + unsigned char c = to_cost(new_prob); + setCost(x, y, c); + } +} + +void RangeSensorLayer::resetRange() +{ + min_x_ = min_y_ = std::numeric_limits::max(); + max_x_ = max_y_ = -std::numeric_limits::max(); +} + +void RangeSensorLayer::updateBounds( + double robot_x, double robot_y, + double robot_yaw, double * min_x, double * min_y, + double * max_x, double * max_y) +{ + robot_yaw = 0 + robot_yaw; // Avoid error if variable not in use + if (layered_costmap_->isRolling()) { + updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); + } + + updateCostmap(); + + *min_x = std::min(*min_x, min_x_); + *min_y = std::min(*min_y, min_y_); + *max_x = std::max(*max_x, max_x_); + *max_y = std::max(*max_y, max_y_); + + resetRange(); + + if (!enabled_) { + current_ = true; + return; + } + + if (buffered_readings_ == 0) { + if (no_readings_timeout_ > 0.0 && + (node_->now() - last_reading_time_).seconds() > no_readings_timeout_) + { + RCLCPP_WARN( + node_->get_logger(), "No range readings received for %.2f seconds, " + "while expected at least every %.2f seconds.", + (node_->now() - last_reading_time_).seconds(), + no_readings_timeout_); + current_ = false; + } + } +} + +void RangeSensorLayer::updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j) +{ + if (!enabled_) { + return; + } + + unsigned char * master_array = master_grid.getCharMap(); + unsigned int span = master_grid.getSizeInCellsX(); + unsigned char clear = to_cost(clear_threshold_), mark = to_cost(mark_threshold_); + + for (int j = min_j; j < max_j; j++) { + unsigned int it = j * span + min_i; + for (int i = min_i; i < max_i; i++) { + unsigned char prob = costmap_[it]; + unsigned char current; + if (prob == nav2_costmap_2d::NO_INFORMATION) { + it++; + continue; + } else if (prob > mark) { + current = nav2_costmap_2d::LETHAL_OBSTACLE; + } else if (prob < clear) { + current = nav2_costmap_2d::FREE_SPACE; + } else { + it++; + continue; + } + + unsigned char old_cost = master_array[it]; + + if (old_cost == NO_INFORMATION || old_cost < current) { + master_array[it] = current; + } + it++; + } + } + + buffered_readings_ = 0; + current_ = true; +} + +void RangeSensorLayer::reset() +{ + RCLCPP_DEBUG(node_->get_logger(), "Reseting range sensor layer..."); + deactivate(); + resetMaps(); + current_ = true; + activate(); +} + +void RangeSensorLayer::deactivate() +{ + range_msgs_buffer_.clear(); +} + +void RangeSensorLayer::activate() +{ + range_msgs_buffer_.clear(); +} + +} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 8180e3f6ba2..cb3a01f1be4 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -58,6 +58,7 @@ namespace nav2_costmap_2d { StaticLayer::StaticLayer() +: map_buffer_(nullptr) { } @@ -123,10 +124,18 @@ StaticLayer::getParameters() declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false)); declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true)); declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0)); + declareParameter("map_topic", rclcpp::ParameterValue("")); node_->get_parameter(name_ + "." + "enabled", enabled_); node_->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); - node_->get_parameter("map_topic", map_topic_); + std::string private_map_topic, global_map_topic; + node_->get_parameter(name_ + "." + "map_topic", private_map_topic); + node_->get_parameter("map_topic", global_map_topic); + if (!private_map_topic.empty()) { + map_topic_ = private_map_topic; + } else { + map_topic_ = global_map_topic; + } node_->get_parameter( name_ + "." + "map_subscribe_transient_local", map_subscribe_transient_local_); @@ -140,6 +149,7 @@ StaticLayer::getParameters() // Enforce bounds lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); map_received_ = false; + update_in_progress_.store(false); transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); } @@ -193,6 +203,9 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) unsigned int index = 0; + // we have a new map, update full size of map + std::lock_guard guard(*getMutex()); + // initialize the costmap with static data for (unsigned int i = 0; i < size_y; ++i) { for (unsigned int j = 0; j < size_x; ++j) { @@ -204,8 +217,6 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) map_frame_ = new_map.header.frame_id; - // we have a new map, update full size of map - std::lock_guard guard(*getMutex()); x_ = y_ = 0; width_ = size_x_; height_ = size_y_; @@ -249,9 +260,15 @@ void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map) { std::lock_guard guard(*getMutex()); - processMap(*new_map); 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; } } @@ -311,6 +328,14 @@ StaticLayer::updateBounds( } std::lock_guard guard(*getMutex()); + update_in_progress_.store(true); + + // If there is a new available map, load it. + if (map_buffer_) { + processMap(*map_buffer_); + map_buffer_ = nullptr; + } + if (!layered_costmap_->isRolling() ) { if (!(has_updated_data_ || has_extra_bounds_)) { return; @@ -338,6 +363,7 @@ StaticLayer::updateCosts( int min_i, int min_j, int max_i, int max_j) { if (!enabled_) { + update_in_progress_.store(false); return; } if (!map_received_) { @@ -347,6 +373,7 @@ StaticLayer::updateCosts( RCLCPP_WARN(node_->get_logger(), "Can't update static costmap layer, no map received"); count = 0; } + update_in_progress_.store(false); return; } @@ -369,6 +396,7 @@ StaticLayer::updateCosts( transform_tolerance_); } catch (tf2::TransformException & ex) { RCLCPP_ERROR(node_->get_logger(), "StaticLayer: %s", ex.what()); + update_in_progress_.store(false); return; } // Copy map data given proper transformations @@ -393,6 +421,7 @@ StaticLayer::updateCosts( } } } + update_in_progress_.store(false); } } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 5235e236757..9a960dc689d 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -88,10 +88,9 @@ void VoxelLayer::onInitialize() if (publish_voxel_) { voxel_pub_ = node_->create_publisher( "voxel_grid", custom_qos); + voxel_pub_->on_activate(); } - voxel_pub_->on_activate(); - clearing_endpoints_pub_ = node_->create_publisher( "clearing_endpoints", custom_qos); diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index e5fc9472920..21a96bcf8b5 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -212,6 +212,11 @@ unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const return costmap_[getIndex(mx, my)]; } +unsigned char Costmap2D::getCost(unsigned int undex) const +{ + return costmap_[undex]; +} + void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost) { costmap_[getIndex(mx, my)] = cost; @@ -430,7 +435,7 @@ void Costmap2D::convexFillCells( MapLocation pt; // loop though cells in the column - for (unsigned int y = min_pt.y; y < max_pt.y; ++y) { + for (unsigned int y = min_pt.y; y <= max_pt.y; ++y) { pt.x = x; pt.y = y; polygon_cells.push_back(pt); diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 75c7552628b..89c3d28aca8 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -274,13 +274,6 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -Costmap2DROS::on_error(const rclcpp_lifecycle::State &) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn Costmap2DROS::on_shutdown(const rclcpp_lifecycle::State &) { @@ -312,16 +305,16 @@ Costmap2DROS::getParameters() get_parameter("width", map_width_meters_); get_parameter("plugins", plugin_names_); + auto node = shared_from_this(); + if (plugin_names_ == default_plugins_) { for (size_t i = 0; i < default_plugins_.size(); ++i) { - declare_parameter(default_plugins_[i] + ".plugin", default_types_[i]); + nav2_util::declare_parameter_if_not_declared( + node, default_plugins_[i] + ".plugin", rclcpp::ParameterValue(default_types_[i])); } } plugin_types_.resize(plugin_names_.size()); - // Semantic checks... - auto node = shared_from_this(); - // 1. All plugins must have 'plugin' param defined in their namespace to define the plugin type for (size_t i = 0; i < plugin_names_.size(); ++i) { plugin_types_[i] = nav2_util::get_plugin_type_param(node, plugin_names_[i]); @@ -394,7 +387,7 @@ Costmap2DROS::mapUpdateLoop(double frequency) RCLCPP_DEBUG(get_logger(), "Entering loop"); - rclcpp::Rate r(frequency); // 200ms by default + rclcpp::WallRate r(frequency); // 200ms by default while (rclcpp::ok() && !map_update_thread_shutdown_) { nav2_util::ExecutionTimer timer; diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index 15adcd8665b..20d60b3442a 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -31,18 +31,21 @@ using namespace std::chrono_literals; namespace nav2_costmap_2d { -FootprintCollisionChecker::FootprintCollisionChecker() +template +FootprintCollisionChecker::FootprintCollisionChecker() : costmap_(nullptr) { } -FootprintCollisionChecker::FootprintCollisionChecker( - std::shared_ptr costmap) +template +FootprintCollisionChecker::FootprintCollisionChecker( + CostmapT costmap) : costmap_(costmap) { } -double FootprintCollisionChecker::footprintCost(const Footprint footprint) +template +double FootprintCollisionChecker::footprintCost(const Footprint footprint) { // now we really have to lay down the footprint in the costmap_ grid unsigned int x0, x1, y0, y1; @@ -80,7 +83,8 @@ double FootprintCollisionChecker::footprintCost(const Footprint footprint) return footprint_cost; } -double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1) const +template +double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1) const { double line_cost = 0.0; double point_cost = -1.0; @@ -96,23 +100,27 @@ double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1) const return line_cost; } -bool FootprintCollisionChecker::worldToMap( +template +bool FootprintCollisionChecker::worldToMap( double wx, double wy, unsigned int & mx, unsigned int & my) { return costmap_->worldToMap(wx, wy, mx, my); } -double FootprintCollisionChecker::pointCost(int x, int y) const +template +double FootprintCollisionChecker::pointCost(int x, int y) const { return costmap_->getCost(x, y); } -void FootprintCollisionChecker::setCostmap(std::shared_ptr costmap) +template +void FootprintCollisionChecker::setCostmap(CostmapT costmap) { costmap_ = costmap; } -double FootprintCollisionChecker::footprintCostAtPose( +template +double FootprintCollisionChecker::footprintCostAtPose( double x, double y, double theta, const Footprint footprint) { double cos_th = cos(theta); @@ -128,4 +136,8 @@ double FootprintCollisionChecker::footprintCostAtPose( return footprintCost(oriented_footprint); } +// declare our valid template parameters +template class FootprintCollisionChecker>; +template class FootprintCollisionChecker; + } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/layered_costmap.cpp b/nav2_costmap_2d/src/layered_costmap.cpp index 48ba1ea467d..a03b79c0dcd 100644 --- a/nav2_costmap_2d/src/layered_costmap.cpp +++ b/nav2_costmap_2d/src/layered_costmap.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include "nav2_costmap_2d/footprint.hpp" diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 4170ad3875b..5ecbeba923d 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -43,6 +43,17 @@ target_link_libraries(obstacle_tests_exec layers ) +ament_add_gtest_executable(range_tests_exec + range_tests.cpp +) +ament_target_dependencies(range_tests_exec + ${dependencies} +) +target_link_libraries(range_tests_exec + nav2_costmap_2d_core + layers +) + ament_add_test(test_collision_checker GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" @@ -83,6 +94,16 @@ ament_add_test(obstacle_tests TEST_EXECUTABLE=$ ) +ament_add_test(range_tests + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) + ## TODO(bpwilcox): this test (I believe) is intended to be launched with the simple_driving_test.xml, ## which has a dependency on rosbag playback # ament_add_gtest_executable(costmap_tester diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index 40262ce9833..2e3aa19c53c 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -44,7 +44,7 @@ #include "nav2_costmap_2d/obstacle_layer.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" #include "nav2_costmap_2d/observation_buffer.hpp" -#include "nav2_costmap_2d/testing_helper.hpp" +#include "../testing_helper.hpp" #include "nav2_util/node_utils.hpp" using geometry_msgs::msg::Point; diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp index 7e1910f5f76..a635cd8e6a8 100644 --- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp +++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp @@ -41,7 +41,7 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav2_costmap_2d/observation_buffer.hpp" -#include "nav2_costmap_2d/testing_helper.hpp" +#include "../testing_helper.hpp" using std::begin; using std::end; diff --git a/nav2_costmap_2d/test/integration/range_tests.cpp b/nav2_costmap_2d/test/integration/range_tests.cpp new file mode 100644 index 00000000000..196406e7e02 --- /dev/null +++ b/nav2_costmap_2d/test/integration/range_tests.cpp @@ -0,0 +1,293 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bytes Robotics + * 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 copyright holder 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 HOLDER 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. + */ + +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "nav2_costmap_2d/observation_buffer.hpp" +#include "../testing_helper.hpp" +#include "sensor_msgs/msg/range.hpp" + +using std::begin; +using std::end; +using std::for_each; +using std::all_of; +using std::none_of; +using std::pair; +using std::string; + +class RclCppFixture +{ +public: + RclCppFixture() + { + rclcpp::init(0, nullptr); + } + + ~RclCppFixture() + { + rclcpp::shutdown(); + } +}; + +RclCppFixture g_rclcppfixture; + +class TestLifecycleNode : public nav2_util::LifecycleNode +{ +public: + explicit TestLifecycleNode(const string & name) + : nav2_util::LifecycleNode(name) + { + } + + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &) + { + return nav2_util::CallbackReturn::SUCCESS; + } +}; + +class TestNode : public ::testing::Test +{ +public: + TestNode() + : node_(std::make_shared("range_test_node")), + tf_(node_->get_clock()) + { + // Standard non-plugin specific parameters + node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); + node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); + node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false)); + node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + node_->declare_parameter( + "unknown_cost_value", + rclcpp::ParameterValue(static_cast(0xff))); + node_->declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); + node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3)); + node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string("range"))); + node_->declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map"))); + + + // Range sensor specific parameters + node_->declare_parameter( + "range.topics", + rclcpp::ParameterValue( + std::vector{"/range/topic"})); + node_->declare_parameter("range.phi", rclcpp::ParameterValue(1.2)); + node_->declare_parameter("range.clear_on_max_reading", rclcpp::ParameterValue(true)); + } + + ~TestNode() {} + +protected: + std::shared_ptr node_; + tf2_ros::Buffer tf_; +}; + +// Test clearing at max range +TEST_F(TestNode, testClearingAtMaxRange) { + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = node_->now(); + transform.header.frame_id = "frame"; + transform.child_frame_id = "base_link"; + transform.transform.translation.y = 5; + transform.transform.translation.x = 2; + tf_.setTransform(transform, "default_authority", true); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr rlayer{nullptr}; + addRangeLayer(layers, tf_, node_, rlayer); + + sensor_msgs::msg::Range msg; + msg.min_range = 1.0; + msg.max_range = 7.0; + msg.range = 2.0; + msg.header.stamp = node_->now(); + msg.header.frame_id = "base_link"; + msg.radiation_type = msg.ULTRASOUND; + msg.field_of_view = 0.174533; // 10 deg + rlayer->bufferIncomingRangeMsg(std::make_shared(msg)); + + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose +// printMap(*(layers.getCostmap())); + + ASSERT_EQ(layers.getCostmap()->getCost(4, 5), 254); + + msg.range = 7.0; + msg.header.stamp = node_->now(); + rlayer->bufferIncomingRangeMsg(std::make_shared(msg)); + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose +// printMap(*(layers.getCostmap())); + + ASSERT_EQ(layers.getCostmap()->getCost(4, 5), 0); +} + +// Testing fixed scan with robot forward motion +TEST_F(TestNode, testProbabalisticModelForward) { + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = node_->now(); + transform.header.frame_id = "frame"; + transform.child_frame_id = "base_link"; + transform.transform.translation.y = 5; + transform.transform.translation.x = 2; + tf_.setTransform(transform, "default_authority", true); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr rlayer{nullptr}; + addRangeLayer(layers, tf_, node_, rlayer); + + sensor_msgs::msg::Range msg; + msg.min_range = 1.0; + msg.max_range = 10.0; + msg.range = 3.0; + msg.header.stamp = node_->now(); + msg.header.frame_id = "base_link"; + msg.radiation_type = msg.ULTRASOUND; + msg.field_of_view = 0.174533; // 10 deg + rlayer->bufferIncomingRangeMsg(std::make_shared(msg)); + + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose +// printMap(*(layers.getCostmap())); + + rlayer->bufferIncomingRangeMsg(std::make_shared(msg)); + transform.transform.translation.y = 5; + transform.transform.translation.x = 4; + tf_.setTransform(transform, "default_authority", true); + + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose +// printMap(*(layers.getCostmap())); + + rlayer->bufferIncomingRangeMsg(std::make_shared(msg)); + + transform.transform.translation.y = 5; + transform.transform.translation.x = 6; + tf_.setTransform(transform, "default_authority", true); + + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose +// printMap(*(layers.getCostmap())); + + ASSERT_EQ(layers.getCostmap()->getCost(5, 5), 254); + ASSERT_EQ(layers.getCostmap()->getCost(6, 5), 0); + ASSERT_EQ(layers.getCostmap()->getCost(7, 5), 254); + ASSERT_EQ(layers.getCostmap()->getCost(8, 5), 0); + ASSERT_EQ(layers.getCostmap()->getCost(9, 5), 254); +} + +// Testing fixed motion with downward movement +TEST_F(TestNode, testProbabalisticModelDownward) { + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = node_->now(); + transform.header.frame_id = "frame"; + transform.child_frame_id = "base_link"; + transform.transform.translation.y = 3; + transform.transform.translation.x = 2; + tf_.setTransform(transform, "default_authority", true); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr rlayer{nullptr}; + addRangeLayer(layers, tf_, node_, rlayer); + + sensor_msgs::msg::Range msg; + msg.min_range = 1.0; + msg.max_range = 10.0; + msg.range = 1.0; + msg.header.stamp = node_->now(); + msg.header.frame_id = "base_link"; + msg.radiation_type = msg.ULTRASOUND; + msg.field_of_view = 0.174533; // 10 deg + rlayer->bufferIncomingRangeMsg(std::make_shared(msg)); + + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose +// printMap(*(layers.getCostmap())); + + rlayer->bufferIncomingRangeMsg(std::make_shared(msg)); + + transform.transform.translation.y = 5; + transform.transform.translation.x = 2; + tf_.setTransform(transform, "default_authority", true); + + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose +// printMap(*(layers.getCostmap())); + + rlayer->bufferIncomingRangeMsg(std::make_shared(msg)); + + transform.transform.translation.y = 7; + transform.transform.translation.x = 2; + tf_.setTransform(transform, "default_authority", true); + + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose +// printMap(*(layers.getCostmap())); + + ASSERT_EQ(layers.getCostmap()->getCost(3, 3), 254); + ASSERT_EQ(layers.getCostmap()->getCost(3, 4), 0); + ASSERT_EQ(layers.getCostmap()->getCost(3, 5), 254); + ASSERT_EQ(layers.getCostmap()->getCost(3, 6), 0); + ASSERT_EQ(layers.getCostmap()->getCost(3, 7), 254); +} diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index 56baa03a4b4..619613c6785 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -24,7 +24,7 @@ #include "nav2_costmap_2d/static_layer.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" #include "nav2_costmap_2d/costmap_2d_publisher.hpp" -#include "nav2_costmap_2d/testing_helper.hpp" +#include "../testing_helper.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/node_utils.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -183,7 +183,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode setPose(x, y, theta); publishFootprint(); publishCostmap(); - rclcpp::sleep_for(std::chrono::milliseconds(50)); + rclcpp::sleep_for(std::chrono::milliseconds(1000)); return collision_checker_->isCollisionFree(pose); } diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp b/nav2_costmap_2d/test/testing_helper.hpp similarity index 90% rename from nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp rename to nav2_costmap_2d/test/testing_helper.hpp index 013341adf90..82c7d53ec2b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp +++ b/nav2_costmap_2d/test/testing_helper.hpp @@ -23,6 +23,7 @@ #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/static_layer.hpp" +#include "nav2_costmap_2d/range_sensor_layer.hpp" #include "nav2_costmap_2d/obstacle_layer.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" #include "nav2_util/lifecycle_node.hpp" @@ -97,6 +98,16 @@ void addObstacleLayer( layers.addPlugin(std::shared_ptr(olayer)); } +void addRangeLayer( + nav2_costmap_2d::LayeredCostmap & layers, + tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr & rlayer) +{ + rlayer = std::make_shared(); + rlayer->initialize(&layers, "range", &tf, node, nullptr, nullptr /*TODO*/); + layers.addPlugin(std::shared_ptr(rlayer)); +} + void addObservation( std::shared_ptr olayer, double x, double y, double z = 0.0, double ox = 0.0, double oy = 0.0, double oz = MAX_Z, bool marking = true, bool clearing = true) diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp index f0e9c306233..1be0d1282d2 100644 --- a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp @@ -39,7 +39,8 @@ TEST(collision_footprint, test_basic) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); @@ -51,7 +52,8 @@ TEST(collision_footprint, test_point_cost) std::shared_ptr costmap_ = std::make_shared(100, 100, 0.1, 0, 0, 0); - nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); auto value = collision_checker.pointCost(50, 50); @@ -63,7 +65,8 @@ TEST(collision_footprint, test_world_to_map) std::shared_ptr costmap_ = std::make_shared(100, 100, 0.1, 0, 0, 0); - nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); unsigned int x, y; @@ -105,7 +108,8 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); EXPECT_NEAR(value, 0.0, 0.001); @@ -140,7 +144,8 @@ TEST(collision_footprint, test_point_and_line_cost) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker(costmap_); auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); EXPECT_NEAR(value, 0.0, 0.001); diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index b7bb3ed1933..23a2029c47d 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 0.3.4 + 0.4.7 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index 4ded71cb7eb..f4a588611a9 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -99,20 +99,6 @@ class DWBLocalPlanner : public nav2_core::Controller const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity) override; - /** - * @brief nav2_core isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity. - * - * The pose that it checks against is the last pose in the current global plan. - * The calculation is delegated to the goal_checker plugin. - * - * @param pose Current pose - * @param velocity Current velocity - * @return True if the robot should be considered as having reached the goal. - */ - bool isGoalReached( - const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & velocity) override; - /** * @brief Score a given command. Can be used for testing. * @@ -174,6 +160,11 @@ class DWBLocalPlanner : public nav2_core::Controller * 3) If prune_plan_ is true, it will remove all points that we've already passed from both the transformed plan * and the saved global_plan_. Technically, it iterates to a pose on the path that is within prune_distance_ * of the robot and erases all poses before that. + * + * Additionally, shorten_transformed_plan_ determines whether we will pass the full plan all + * the way to the nav goal on to the critics or just a subset of the plan near the robot. + * True means pass just a subset. This gives DWB less discretion to decide how it gets to the + * nav goal. Instead it is encouraged to try to get on to the path generated by the global planner. */ virtual nav_2d_msgs::msg::Path2D transformGlobalPlan( const nav_2d_msgs::msg::Pose2DStamped & pose); @@ -182,6 +173,7 @@ class DWBLocalPlanner : public nav2_core::Controller double prune_distance_; bool debug_trajectory_details_; rclcpp::Duration transform_tolerance_{0, 0}; + bool shorten_transformed_plan_; /** * @brief try to resolve a possibly shortened critic name with the default namespaces and the suffix "Critic" @@ -210,9 +202,6 @@ class DWBLocalPlanner : public nav2_core::Controller pluginlib::ClassLoader traj_gen_loader_; TrajectoryGenerator::Ptr traj_generator_; - pluginlib::ClassLoader goal_checker_loader_; - nav2_core::GoalChecker::Ptr goal_checker_; - pluginlib::ClassLoader critic_loader_; std::vector critics_; diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 234168543fa..fbb064d0705 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 0.3.4 + 0.4.7 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 011ea530422..793e9c90f7f 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -59,7 +59,6 @@ namespace dwb_core DWBLocalPlanner::DWBLocalPlanner() : traj_gen_loader_("dwb_core", "dwb_core::TrajectoryGenerator"), - goal_checker_loader_("dwb_core", "nav2_core::GoalChecker"), critic_loader_("dwb_core", "dwb_core::TrajectoryCritic") { } @@ -87,18 +86,17 @@ void DWBLocalPlanner::configure( declare_parameter_if_not_declared( node_, dwb_plugin_name_ + ".trajectory_generator_name", rclcpp::ParameterValue(std::string("dwb_plugins::StandardTrajectoryGenerator"))); - declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + ".goal_checker_name", - rclcpp::ParameterValue(std::string("dwb_plugins::SimpleGoalChecker"))); declare_parameter_if_not_declared( node_, dwb_plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node_, dwb_plugin_name_ + ".shorten_transformed_plan", + rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node_, dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", rclcpp::ParameterValue(true)); std::string traj_generator_name; - std::string goal_checker_name; double transform_tolerance; node_->get_parameter(dwb_plugin_name_ + ".transform_tolerance", transform_tolerance); @@ -109,19 +107,17 @@ void DWBLocalPlanner::configure( node_->get_parameter(dwb_plugin_name_ + ".prune_distance", prune_distance_); node_->get_parameter(dwb_plugin_name_ + ".debug_trajectory_details", debug_trajectory_details_); node_->get_parameter(dwb_plugin_name_ + ".trajectory_generator_name", traj_generator_name); - node_->get_parameter(dwb_plugin_name_ + ".goal_checker_name", goal_checker_name); node_->get_parameter( dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", short_circuit_trajectory_evaluation_); + node->get_parameter(dwb_plugin_name_ + ".shorten_transformed_plan", shorten_transformed_plan_); pub_ = std::make_unique(node_, dwb_plugin_name_); pub_->on_configure(); traj_generator_ = traj_gen_loader_.createUniqueInstance(traj_generator_name); - goal_checker_ = goal_checker_loader_.createUniqueInstance(goal_checker_name); traj_generator_->initialize(node_, dwb_plugin_name_); - goal_checker_->initialize(node_, dwb_plugin_name_); try { loadCritics(); @@ -149,7 +145,6 @@ DWBLocalPlanner::cleanup() pub_->on_cleanup(); traj_generator_.reset(); - goal_checker_.reset(); } std::string @@ -263,42 +258,6 @@ DWBLocalPlanner::loadBackwardsCompatibleParameters() /* *INDENT-ON* */ } -bool -DWBLocalPlanner::isGoalReached( - const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & velocity) -{ - if (global_plan_.poses.size() == 0) { - RCLCPP_WARN( - rclcpp::get_logger( - "DWBLocalPlanner"), "Cannot check if the goal is reached without the goal being set!"); - return false; - } - nav_2d_msgs::msg::Pose2DStamped local_start_pose2d, goal_pose2d, local_goal_pose2d; - - nav_2d_utils::transformPose( - tf_, costmap_ros_->getGlobalFrameID(), - nav_2d_utils::poseStampedToPose2D(pose), - local_start_pose2d, transform_tolerance_); - - goal_pose2d.header.frame_id = global_plan_.header.frame_id; - goal_pose2d.pose = global_plan_.poses.back(); - - nav_2d_utils::transformPose( - tf_, costmap_ros_->getGlobalFrameID(), goal_pose2d, - local_goal_pose2d, transform_tolerance_); - - geometry_msgs::msg::PoseStamped local_start_pose, local_goal_pose; - local_start_pose = nav_2d_utils::pose2DToPoseStamped(local_start_pose2d); - local_goal_pose = nav_2d_utils::pose2DToPoseStamped(local_goal_pose2d); - - bool ret = goal_checker_->isGoalReached(local_start_pose.pose, local_goal_pose.pose, velocity); - if (ret) { - RCLCPP_INFO(rclcpp::get_logger("DWBLocalPlanner"), "Goal reached!"); - } - return ret; -} - void DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) { @@ -308,7 +267,6 @@ DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) } traj_generator_->reset(); - goal_checker_->reset(); pub_->publishGlobalPlan(path2d); global_plan_ = path2d; @@ -371,6 +329,9 @@ DWBLocalPlanner::computeVelocityCommands( prepareGlobalPlan(pose, transformed_plan, goal_pose); + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + std::unique_lock lock(*(costmap->getMutex())); + for (TrajectoryCritic::Ptr critic : critics_) { if (critic->prepare(pose.pose, velocity, goal_pose.pose, transformed_plan) == false) { RCLCPP_WARN(rclcpp::get_logger("DWBLocalPlanner"), "A scoring function failed to prepare"); @@ -390,6 +351,8 @@ DWBLocalPlanner::computeVelocityCommands( critic->debrief(cmd_vel.velocity); } + lock.unlock(); + pub_->publishLocalPlan(pose.header, best.traj); pub_->publishCostGrid(costmap_ros_, critics_); @@ -401,6 +364,9 @@ DWBLocalPlanner::computeVelocityCommands( for (TrajectoryCritic::Ptr critic : critics_) { critic->debrief(empty_cmd); } + + lock.unlock(); + pub_->publishLocalPlan(pose.header, empty_traj); pub_->publishCostGrid(costmap_ros_, critics_); @@ -556,18 +522,11 @@ DWBLocalPlanner::transformGlobalPlan( sq_transform_start_threshold = sq_dist_threshold; } - // Determines whether we will pass the full plan all the way to the nav goal on - // to the critics or just a subset of the plan near the robot. True means pass - // just a subset. This gives DWB less discretion to decide how it gets to the - // nav goal. Instead it is encouraged to try to get on to the path generated - // by the global planner. - bool shorten_transformed_plan = true; - // Set the maximum distance we'll include points after the part of the part of // the plan near the robot (the end of the plan). This determines the amount // of the plan passed on to the critics double sq_transform_end_threshold; - if (shorten_transformed_plan) { + if (shorten_transformed_plan_) { sq_transform_end_threshold = std::min(sq_dist_threshold, sq_prune_dist); } else { sq_transform_end_threshold = sq_dist_threshold; diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index e8a6be0e0a9..6b121ecd507 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 0.3.4 + 0.4.7 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 cc2fe01abe6..057b2933359 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 0.3.4 + 0.4.7 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt index ea4d7868eaa..6e54ad061e3 100644 --- a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt @@ -27,19 +27,6 @@ include_directories( include ) -add_library(simple_goal_checker SHARED src/simple_goal_checker.cpp) -ament_target_dependencies(simple_goal_checker ${dependencies}) -# prevent pluginlib from using boost -target_compile_definitions(simple_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - - -add_library(stopped_goal_checker SHARED src/stopped_goal_checker.cpp) -target_link_libraries(stopped_goal_checker simple_goal_checker) -ament_target_dependencies(stopped_goal_checker ${dependencies}) -# prevent pluginlib from using boost -target_compile_definitions(stopped_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - - add_library(standard_traj_generator SHARED src/standard_traj_generator.cpp src/limited_accel_generator.cpp @@ -60,7 +47,7 @@ if(BUILD_TESTING) add_subdirectory(test) endif() -install(TARGETS simple_goal_checker stopped_goal_checker standard_traj_generator +install(TARGETS standard_traj_generator ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -73,8 +60,6 @@ install(FILES plugins.xml ) ament_export_include_directories(include) -ament_export_libraries(simple_goal_checker) -ament_export_libraries(stopped_goal_checker) ament_export_libraries(standard_traj_generator) pluginlib_export_plugin_description_file(dwb_core plugins.xml) diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index f0121c20313..f425fc26aa7 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 0.3.4 + 0.4.7 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt index 272f38cf4ca..29845a84998 100644 --- a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt @@ -1,7 +1,4 @@ ament_add_gtest(vtest velocity_iterator_test.cpp) -ament_add_gtest(goal_checker goal_checker.cpp) -target_link_libraries(goal_checker simple_goal_checker stopped_goal_checker) - ament_add_gtest(twist_gen_test twist_gen.cpp) target_link_libraries(twist_gen_test standard_traj_generator) diff --git a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp index df8c0a0235f..1e4ca4cc6c1 100644 --- a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp +++ b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp @@ -74,10 +74,14 @@ std::vector getDefaultKinematicParameters() return parameters; } -rclcpp_lifecycle::LifecycleNode::SharedPtr makeTestNode(const std::string & name) +rclcpp_lifecycle::LifecycleNode::SharedPtr makeTestNode( + const std::string & name, + const std::vector & overrides = {}) { - rclcpp::NodeOptions node_options = nav2_util::get_node_options_default(); + rclcpp::NodeOptions node_options; node_options.parameter_overrides(getDefaultKinematicParameters()); + node_options.parameter_overrides().insert( + node_options.parameter_overrides().end(), overrides.begin(), overrides.end()); auto node = rclcpp_lifecycle::LifecycleNode::make_shared(name, node_options); node->on_configure(node->get_current_state()); @@ -142,8 +146,7 @@ TEST(VelocityIterator, standard_gen) TEST(VelocityIterator, max_xy) { - auto nh = makeTestNode("max_xy"); - nh->set_parameters({rclcpp::Parameter("dwb.max_speed_xy", 1.0)}); + auto nh = makeTestNode("max_xy", {rclcpp::Parameter("dwb.max_speed_xy", 1.0)}); StandardTrajectoryGenerator gen; gen.initialize(nh, "dwb"); @@ -155,8 +158,7 @@ TEST(VelocityIterator, max_xy) TEST(VelocityIterator, min_xy) { - auto nh = makeTestNode("min_xy"); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)}); + auto nh = makeTestNode("min_xy", {rclcpp::Parameter("dwb.min_speed_xy", -1.0)}); StandardTrajectoryGenerator gen; gen.initialize(nh, "dwb"); std::vector twists = gen.getTwists(zero); @@ -167,8 +169,7 @@ TEST(VelocityIterator, min_xy) TEST(VelocityIterator, min_theta) { - auto nh = makeTestNode("min_theta"); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); + auto nh = makeTestNode("min_theta", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); StandardTrajectoryGenerator gen; gen.initialize(nh, "dwb"); std::vector twists = gen.getTwists(zero); @@ -179,10 +180,11 @@ TEST(VelocityIterator, min_theta) TEST(VelocityIterator, no_limits) { - auto nh = makeTestNode("no_limits"); - nh->set_parameters({rclcpp::Parameter("dwb.max_speed_xy", -1.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); + auto nh = makeTestNode( + "no_limits", { + rclcpp::Parameter("dwb.max_speed_xy", -1.0), + rclcpp::Parameter("dwb.min_speed_xy", -1.0), + rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); StandardTrajectoryGenerator gen; gen.initialize(nh, "dwb"); std::vector twists = gen.getTwists(zero); @@ -193,14 +195,15 @@ TEST(VelocityIterator, no_limits) TEST(VelocityIterator, no_limits_samples) { - auto nh = makeTestNode("no_limits_samples"); - nh->set_parameters({rclcpp::Parameter("dwb.max_speed_xy", -1.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); - int x_samples = 10, y_samples = 3, theta_samples = 5; - nh->set_parameters({rclcpp::Parameter("dwb.vx_samples", x_samples)}); - nh->set_parameters({rclcpp::Parameter("dwb.vy_samples", y_samples)}); - nh->set_parameters({rclcpp::Parameter("dwb.vtheta_samples", theta_samples)}); + const int x_samples = 10, y_samples = 3, theta_samples = 5; + auto nh = makeTestNode( + "no_limits_samples", { + rclcpp::Parameter("dwb.max_speed_xy", -1.0), + rclcpp::Parameter("dwb.min_speed_xy", -1.0), + rclcpp::Parameter("dwb.min_speed_theta", -1.0), + rclcpp::Parameter("dwb.vx_samples", x_samples), + rclcpp::Parameter("dwb.vy_samples", y_samples), + rclcpp::Parameter("dwb.vtheta_samples", theta_samples)}); StandardTrajectoryGenerator gen; gen.initialize(nh, "dwb"); std::vector twists = gen.getTwists(zero); @@ -210,8 +213,7 @@ TEST(VelocityIterator, no_limits_samples) TEST(VelocityIterator, dwa_gen) { - auto nh = makeTestNode("dwa_gen"); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); + auto nh = makeTestNode("dwa_gen", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); dwb_plugins::LimitedAccelGenerator gen; gen.initialize(nh, "dwb"); std::vector twists = gen.getTwists(zero); @@ -222,8 +224,7 @@ TEST(VelocityIterator, dwa_gen) TEST(VelocityIterator, nonzero) { - auto nh = makeTestNode("nonzero"); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); + auto nh = makeTestNode("nonzero", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)}); dwb_plugins::LimitedAccelGenerator gen; gen.initialize(nh, "dwb"); nav_2d_msgs::msg::Twist2D initial; @@ -273,9 +274,8 @@ const double DEFAULT_SIM_TIME = 1.7; TEST(TrajectoryGenerator, basic) { - auto nh = makeTestNode("basic"); + auto nh = makeTestNode("basic", {rclcpp::Parameter("dwb.linear_granularity", 0.5)}); StandardTrajectoryGenerator gen; - nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)}); gen.initialize(nh, "dwb"); dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward); matchTwist(res.velocity, forward); @@ -290,10 +290,11 @@ TEST(TrajectoryGenerator, basic) TEST(TrajectoryGenerator, basic_no_last_point) { - auto nh = makeTestNode("basic_no_last_point"); + auto nh = makeTestNode( + "basic_no_last_point", { + rclcpp::Parameter("dwb.include_last_point", false), + rclcpp::Parameter("dwb.linear_granularity", 0.5)}); StandardTrajectoryGenerator gen; - nh->set_parameters({rclcpp::Parameter("dwb.include_last_point", false)}); - nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)}); gen.initialize(nh, "dwb"); dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward); matchTwist(res.velocity, forward); @@ -308,9 +309,8 @@ TEST(TrajectoryGenerator, basic_no_last_point) TEST(TrajectoryGenerator, too_slow) { - auto nh = makeTestNode("too_slow"); + auto nh = makeTestNode("too_slow", {rclcpp::Parameter("dwb.linear_granularity", 0.5)}); StandardTrajectoryGenerator gen; - nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)}); gen.initialize(nh, "dwb"); nav_2d_msgs::msg::Twist2D cmd; cmd.x = 0.2; @@ -326,9 +326,8 @@ TEST(TrajectoryGenerator, too_slow) TEST(TrajectoryGenerator, holonomic) { - auto nh = makeTestNode("holonomic"); + auto nh = makeTestNode("holonomic", {rclcpp::Parameter("dwb.linear_granularity", 0.5)}); StandardTrajectoryGenerator gen; - nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)}); gen.initialize(nh, "dwb"); nav_2d_msgs::msg::Twist2D cmd; cmd.x = 0.3; @@ -346,10 +345,11 @@ TEST(TrajectoryGenerator, holonomic) TEST(TrajectoryGenerator, twisty) { - auto nh = makeTestNode("twisty"); + auto nh = makeTestNode( + "twisty", { + rclcpp::Parameter("dwb.linear_granularity", 0.5), + rclcpp::Parameter("dwb.angular_granularity", 0.025)}); StandardTrajectoryGenerator gen; - nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)}); - nh->set_parameters({rclcpp::Parameter("dwb.angular_granularity", 0.025)}); gen.initialize(nh, "dwb"); nav_2d_msgs::msg::Twist2D cmd; cmd.x = 0.3; @@ -370,10 +370,11 @@ TEST(TrajectoryGenerator, twisty) TEST(TrajectoryGenerator, sim_time) { - auto nh = makeTestNode("sim_time"); const double sim_time = 2.5; - nh->set_parameters({rclcpp::Parameter("dwb.sim_time", sim_time)}); - nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)}); + auto nh = makeTestNode( + "sim_time", { + rclcpp::Parameter("dwb.sim_time", sim_time), + rclcpp::Parameter("dwb.linear_granularity", 0.5)}); StandardTrajectoryGenerator gen; gen.initialize(nh, "dwb"); dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward); @@ -389,12 +390,13 @@ TEST(TrajectoryGenerator, sim_time) TEST(TrajectoryGenerator, accel) { - auto nh = makeTestNode("accel"); - nh->set_parameters({rclcpp::Parameter("dwb.sim_time", 5.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.discretize_by_time", true)}); - nh->set_parameters({rclcpp::Parameter("dwb.time_granularity", 1.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.acc_lim_x", 0.1)}); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)}); + auto nh = makeTestNode( + "accel", { + rclcpp::Parameter("dwb.sim_time", 5.0), + rclcpp::Parameter("dwb.discretize_by_time", true), + rclcpp::Parameter("dwb.time_granularity", 1.0), + rclcpp::Parameter("dwb.acc_lim_x", 0.1), + rclcpp::Parameter("dwb.min_speed_xy", -1.0)}); StandardTrajectoryGenerator gen; gen.initialize(nh, "dwb"); @@ -412,13 +414,14 @@ TEST(TrajectoryGenerator, accel) TEST(TrajectoryGenerator, dwa) { - auto nh = makeTestNode("dwa"); - nh->set_parameters({rclcpp::Parameter("dwb.sim_period", 1.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.sim_time", 5.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.discretize_by_time", true)}); - nh->set_parameters({rclcpp::Parameter("dwb.time_granularity", 1.0)}); - nh->set_parameters({rclcpp::Parameter("dwb.acc_lim_x", 0.1)}); - nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)}); + auto nh = makeTestNode( + "dwa", { + rclcpp::Parameter("dwb.sim_period", 1.0), + rclcpp::Parameter("dwb.sim_time", 5.0), + rclcpp::Parameter("dwb.discretize_by_time", true), + rclcpp::Parameter("dwb.time_granularity", 1.0), + rclcpp::Parameter("dwb.acc_lim_x", 0.1), + rclcpp::Parameter("dwb.min_speed_xy", -1.0)}); dwb_plugins::LimitedAccelGenerator gen; gen.initialize(nh, "dwb"); diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 655b3d9cc82..1ae96ea4028 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 - 0.3.4 + 0.4.7 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 1abcf6b31b8..3579746eaf0 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 - 0.3.4 + 0.4.7 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/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt index 06bb2a5a193..adb0123d7f9 100644 --- a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt +++ b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt @@ -43,7 +43,19 @@ ament_target_dependencies(path_ops ${dependencies} ) -install(TARGETS conversions path_ops +add_library(tf_help SHARED + src/tf_help.cpp +) + +target_link_libraries(tf_help + conversions +) + +ament_target_dependencies(tf_help + ${dependencies} +) + +install(TARGETS conversions path_ops tf_help ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -60,7 +72,7 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(conversions path_ops) +ament_export_libraries(conversions path_ops tf_help) ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp index a7a2f010ee8..15dd3b9e4dc 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp @@ -93,18 +93,26 @@ param_t loadParameterWithDeprecation( const nav2_util::LifecycleNode::SharedPtr & nh, const std::string current_name, const std::string old_name, const param_t & default_value) { - param_t value = 0; - if (nh->get_parameter(current_name, value)) { - return value; - } - if (nh->get_parameter(old_name, value)) { + nav2_util::declare_parameter_if_not_declared( + nh, current_name, rclcpp::ParameterValue(default_value)); + nav2_util::declare_parameter_if_not_declared( + nh, old_name, rclcpp::ParameterValue(default_value)); + + param_t current_name_value; + nh->get_parameter(current_name, current_name_value); + param_t old_name_value; + nh->get_parameter(old_name, old_name_value); + + if (old_name_value != current_name_value && old_name_value != default_value) { RCLCPP_WARN( nh->get_logger(), "Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str()); - return value; + // set both parameters to the same value + nh->set_parameters({rclcpp::Parameter(current_name, old_name_value)}); + current_name_value = old_name_value; } - return default_value; + return current_name_value; } /** diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp index dc49c99a0d6..3b4687bd108 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp @@ -37,6 +37,7 @@ #include #include +#include "tf2_ros/buffer.h" #include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_2d_msgs/msg/pose2_d_stamped.hpp" @@ -55,46 +56,12 @@ namespace nav_2d_utils * @return True if successful transform */ bool transformPose( - const std::shared_ptr tf, const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, geometry_msgs::msg::PoseStamped & out_pose, - rclcpp::Duration & transform_tolerance) -{ - if (in_pose.header.frame_id == frame) { - out_pose = in_pose; - return true; - } - - try { - tf->transform(in_pose, out_pose, frame); - return true; - } catch (tf2::ExtrapolationException & ex) { - auto transform = tf->lookupTransform(frame, in_pose.header.frame_id, tf2::TimePointZero); - if ((rclcpp::Time(in_pose.header.stamp) - rclcpp::Time(transform.header.stamp)) > - transform_tolerance) - { - RCLCPP_ERROR( - rclcpp::get_logger("tf_help"), - "Transform data too old when converting from %s to %s", - in_pose.header.frame_id.c_str(), - frame.c_str()); - RCLCPP_ERROR( - rclcpp::get_logger("tf_help"), - "Data time: %ds %uns, Transform time: %ds %uns", - in_pose.header.stamp.sec, in_pose.header.stamp.nanosec, - transform.header.stamp.sec, transform.header.stamp.nanosec); - return false; - } else { - tf2::doTransform(in_pose, out_pose, transform); - return true; - } - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - rclcpp::get_logger("tf_help"), - "Exception in transformPose: %s", ex.what()); - return false; - } - return false; -} + const std::shared_ptr tf, + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose, + rclcpp::Duration & transform_tolerance +); /** * @brief Transform a Pose2DStamped from one frame to another while catching exceptions @@ -107,19 +74,12 @@ bool transformPose( * @return True if successful transform */ bool transformPose( - const std::shared_ptr tf, const std::string frame, - const nav_2d_msgs::msg::Pose2DStamped & in_pose, nav_2d_msgs::msg::Pose2DStamped & out_pose, - rclcpp::Duration & transform_tolerance) -{ - geometry_msgs::msg::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose); - geometry_msgs::msg::PoseStamped out_3d_pose; - - bool ret = transformPose(tf, frame, in_3d_pose, out_3d_pose, transform_tolerance); - if (ret) { - out_pose = poseStampedToPose2D(out_3d_pose); - } - return ret; -} + const std::shared_ptr tf, + const std::string frame, + const nav_2d_msgs::msg::Pose2DStamped & in_pose, + nav_2d_msgs::msg::Pose2DStamped & out_pose, + rclcpp::Duration & transform_tolerance +); } // namespace nav_2d_utils diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index 8e322a300b9..75d5cda8bbb 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 - 0.3.4 + 0.4.7 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp b/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp new file mode 100644 index 00000000000..ac4b7a6a4b0 --- /dev/null +++ b/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp @@ -0,0 +1,115 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * 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 copyright holder 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 HOLDER 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. + */ + +#include +#include +#include "nav_2d_utils/tf_help.hpp" + +namespace nav_2d_utils +{ + +bool transformPose( + const std::shared_ptr tf, + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose, + rclcpp::Duration & transform_tolerance +) +{ + if (in_pose.header.frame_id == frame) { + out_pose = in_pose; + return true; + } + + try { + tf->transform(in_pose, out_pose, frame); + return true; + } catch (tf2::ExtrapolationException & ex) { + auto transform = tf->lookupTransform( + frame, + in_pose.header.frame_id, + tf2::TimePointZero + ); + if ( + (rclcpp::Time(in_pose.header.stamp) - rclcpp::Time(transform.header.stamp)) > + transform_tolerance) + { + RCLCPP_ERROR( + rclcpp::get_logger("tf_help"), + "Transform data too old when converting from %s to %s", + in_pose.header.frame_id.c_str(), + frame.c_str() + ); + RCLCPP_ERROR( + rclcpp::get_logger("tf_help"), + "Data time: %ds %uns, Transform time: %ds %uns", + in_pose.header.stamp.sec, + in_pose.header.stamp.nanosec, + transform.header.stamp.sec, + transform.header.stamp.nanosec + ); + return false; + } else { + tf2::doTransform(in_pose, out_pose, transform); + return true; + } + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + rclcpp::get_logger("tf_help"), + "Exception in transformPose: %s", + ex.what() + ); + return false; + } + return false; +} + +bool transformPose( + const std::shared_ptr tf, + const std::string frame, + const nav_2d_msgs::msg::Pose2DStamped & in_pose, + nav_2d_msgs::msg::Pose2DStamped & out_pose, + rclcpp::Duration & transform_tolerance +) +{ + geometry_msgs::msg::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose); + geometry_msgs::msg::PoseStamped out_3d_pose; + + bool ret = transformPose(tf, frame, in_3d_pose, out_3d_pose, transform_tolerance); + if (ret) { + out_pose = poseStampedToPose2D(out_3d_pose); + } + return ret; +} +} // namespace nav_2d_utils diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp index 7e25916b8bf..bb597fbd424 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp @@ -52,27 +52,27 @@ class LifecycleManagerClient * @brief Make start up service call * @return true or false */ - bool startup(); + bool startup(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Make shutdown service call * @return true or false */ - bool shutdown(); + bool shutdown(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Make pause service call * @return true or false */ - bool pause(); + bool pause(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Make resume service call * @return true or false */ - bool resume(); + bool resume(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Make reset service call * @return true or false */ - bool reset(); + bool reset(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Check if lifecycle node manager server is active * @return ACTIVE or INACTIVE or TIMEOUT @@ -103,7 +103,9 @@ class LifecycleManagerClient * @brief A generic method used to call startup, shutdown, etc. * @param command */ - bool callService(uint8_t command); + bool callService( + uint8_t command, + const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); // The node to use for the service call rclcpp::Node::SharedPtr node_; diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index f6cf8d0978a..d56290206e6 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 0.3.4 + 0.4.7 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index 8cb747d453a..c7931539f7b 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -40,33 +40,33 @@ LifecycleManagerClient::LifecycleManagerClient(const std::string & name) } bool -LifecycleManagerClient::startup() +LifecycleManagerClient::startup(const std::chrono::nanoseconds timeout) { - return callService(ManageLifecycleNodes::Request::STARTUP); + return callService(ManageLifecycleNodes::Request::STARTUP, timeout); } bool -LifecycleManagerClient::shutdown() +LifecycleManagerClient::shutdown(const std::chrono::nanoseconds timeout) { - return callService(ManageLifecycleNodes::Request::SHUTDOWN); + return callService(ManageLifecycleNodes::Request::SHUTDOWN, timeout); } bool -LifecycleManagerClient::pause() +LifecycleManagerClient::pause(const std::chrono::nanoseconds timeout) { - return callService(ManageLifecycleNodes::Request::PAUSE); + return callService(ManageLifecycleNodes::Request::PAUSE, timeout); } bool -LifecycleManagerClient::resume() +LifecycleManagerClient::resume(const std::chrono::nanoseconds timeout) { - return callService(ManageLifecycleNodes::Request::RESUME); + return callService(ManageLifecycleNodes::Request::RESUME, timeout); } bool -LifecycleManagerClient::reset() +LifecycleManagerClient::reset(const std::chrono::nanoseconds timeout) { - return callService(ManageLifecycleNodes::Request::RESET); + return callService(ManageLifecycleNodes::Request::RESET, timeout); } SystemStatus @@ -88,7 +88,7 @@ LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout) auto future_result = is_active_client_->async_send_request(request); if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { return SystemStatus::TIMEOUT; } @@ -101,7 +101,7 @@ LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout) } bool -LifecycleManagerClient::callService(uint8_t command) +LifecycleManagerClient::callService(uint8_t command, const std::chrono::nanoseconds timeout) { auto request = std::make_shared(); request->command = command; @@ -122,7 +122,13 @@ LifecycleManagerClient::callService(uint8_t command) node_->get_logger(), "Sending %s request", manage_service_name_.c_str()); auto future_result = manager_client_->async_send_request(request); - rclcpp::spin_until_future_complete(node_, future_result); + + if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != + rclcpp::FutureReturnCode::SUCCESS) + { + return false; + } + return future_result.get()->success; } diff --git a/nav2_map_server/include/nav2_map_server/map_saver.hpp b/nav2_map_server/include/nav2_map_server/map_saver.hpp index 2c096e3dba4..9c9b57d2928 100644 --- a/nav2_map_server/include/nav2_map_server/map_saver.hpp +++ b/nav2_map_server/include/nav2_map_server/map_saver.hpp @@ -86,12 +86,6 @@ class MapSaver : public nav2_util::LifecycleNode * @return Success or Failure */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when Error is raised - * @param state Lifecycle Node's state - * @return Success or Failure - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; /** * @brief Map saving service callback @@ -109,6 +103,8 @@ class MapSaver : public nav2_util::LifecycleNode // Default values for map thresholds double free_thresh_default_; double occupied_thresh_default_; + // param for handling QoS configuration + bool map_subscribe_transient_local_; // The name of the service for saving a map from topic const std::string save_map_service_name_{"save_map"}; diff --git a/nav2_map_server/include/nav2_map_server/map_server.hpp b/nav2_map_server/include/nav2_map_server/map_server.hpp index d3ace49e90a..700b1753ea2 100644 --- a/nav2_map_server/include/nav2_map_server/map_server.hpp +++ b/nav2_map_server/include/nav2_map_server/map_server.hpp @@ -77,12 +77,6 @@ class MapServer : public nav2_util::LifecycleNode * @return Success or Failure */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when Error is raised - * @param state Lifecycle Node's state - * @return Success or Failure - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; /** * @brief Load the map YAML, image from map file name and diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index d1c9aa8e5ec..c66428fb35d 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 0.3.4 + 0.4.7 Refactored map server for ROS2 Navigation diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 6941b354fe7..7375c64da8a 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -78,7 +78,7 @@ char * dirname(char * path) /* This assignment is ill-designed but the XPG specs require to return a string containing "." in any case no directory part is found and so a static and constant string is required. */ - path = reinterpret_cast(dot); + path = const_cast(&dot[0]); } return path; diff --git a/nav2_map_server/src/map_saver/main_server.cpp b/nav2_map_server/src/map_saver/main_server.cpp index c7d2c695eba..320e13cc950 100644 --- a/nav2_map_server/src/map_saver/main_server.cpp +++ b/nav2_map_server/src/map_saver/main_server.cpp @@ -25,15 +25,8 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); auto logger = rclcpp::get_logger("map_saver_server"); - - try { - auto service_node = std::make_shared(); - rclcpp::spin(service_node->get_node_base_interface()); - rclcpp::shutdown(); - return 0; - } catch (std::exception & ex) { - RCLCPP_ERROR(logger, ex.what()); - RCLCPP_ERROR(logger, "Exiting"); - return -1; - } + auto service_node = std::make_shared(); + rclcpp::spin(service_node->get_node_base_interface()); + rclcpp::shutdown(); + return 0; } diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 001e2ed28b6..563e0d52908 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -50,6 +50,8 @@ MapSaver::MapSaver() free_thresh_default_ = declare_parameter("free_thresh_default", 0.25), occupied_thresh_default_ = declare_parameter("occupied_thresh_default", 0.65); + // false only of foxy for backwards compatibility + map_subscribe_transient_local_ = declare_parameter("map_subscribe_transient_local", false); } MapSaver::~MapSaver() @@ -94,13 +96,6 @@ MapSaver::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -MapSaver::on_error(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn MapSaver::on_shutdown(const rclcpp_lifecycle::State & /*state*/) { @@ -180,8 +175,15 @@ bool MapSaver::saveMapTopicToFile( // Add new subscription for incoming map topic. // Utilizing local rclcpp::Node (rclcpp_node_) from nav2_util::LifecycleNode // as a map listener. + rclcpp::QoS map_qos = rclcpp::SystemDefaultsQoS(); // initialize to default + if (map_subscribe_transient_local_) { + map_qos = rclcpp::QoS(10); + map_qos.transient_local(); + map_qos.reliable(); + map_qos.keep_last(1); + } auto map_sub = rclcpp_node_->create_subscription( - map_topic_loc, rclcpp::SystemDefaultsQoS(), mapCallback); + map_topic_loc, map_qos, mapCallback); rclcpp::Time start_time = now(); while (rclcpp::ok()) { @@ -208,7 +210,6 @@ bool MapSaver::saveMapTopicToFile( return false; } - RCLCPP_ERROR(get_logger(), "This situation should never appear"); return false; } diff --git a/nav2_map_server/src/map_server/main.cpp b/nav2_map_server/src/map_server/main.cpp index 8e57cc09bf9..693b3b9a0b1 100644 --- a/nav2_map_server/src/map_server/main.cpp +++ b/nav2_map_server/src/map_server/main.cpp @@ -23,15 +23,8 @@ int main(int argc, char ** argv) { std::string node_name("map_server"); - try { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node->get_node_base_interface()); - rclcpp::shutdown(); - return 0; - } catch (std::exception & ex) { - RCLCPP_ERROR(rclcpp::get_logger(node_name.c_str()), ex.what()); - RCLCPP_ERROR(rclcpp::get_logger(node_name.c_str()), "Exiting"); - return -1; - } + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); } diff --git a/nav2_map_server/src/map_server/map_server.cpp b/nav2_map_server/src/map_server/map_server.cpp index 2c2e174f9c9..d633af10a39 100644 --- a/nav2_map_server/src/map_server/map_server.cpp +++ b/nav2_map_server/src/map_server/map_server.cpp @@ -154,13 +154,6 @@ MapServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -MapServer::on_error(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn MapServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/) { diff --git a/nav2_map_server/test/CMakeLists.txt b/nav2_map_server/test/CMakeLists.txt index 6d3a9f00734..c36d920be06 100644 --- a/nav2_map_server/test/CMakeLists.txt +++ b/nav2_map_server/test/CMakeLists.txt @@ -5,3 +5,4 @@ add_definitions( -DTEST_DIRECTORY=\"${CMAKE_CURRENT_SOURCE_DIR}\") add_subdirectory(unit) add_subdirectory(component) +add_subdirectory(map_saver_cli) diff --git a/nav2_map_server/test/component/test_map_server_node.cpp b/nav2_map_server/test/component/test_map_server_node.cpp index 373f8783056..6fa14d9b98e 100644 --- a/nav2_map_server/test/component/test_map_server_node.cpp +++ b/nav2_map_server/test/component/test_map_server_node.cpp @@ -58,6 +58,7 @@ class MapServerTestFixture : public ::testing::Test { lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE); lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP); + lifecycle_client_->change_state(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); } template diff --git a/nav2_map_server/test/map_saver_cli/CMakeLists.txt b/nav2_map_server/test/map_saver_cli/CMakeLists.txt new file mode 100644 index 00000000000..f7e3b52950d --- /dev/null +++ b/nav2_map_server/test/map_saver_cli/CMakeLists.txt @@ -0,0 +1,13 @@ +include_directories(${PROJECT_SOURCE_DIR}/test) + +# map_saver CLI +ament_add_gtest(test_map_saver_cli + test_map_saver_cli.cpp + ${PROJECT_SOURCE_DIR}/test/test_constants.cpp +) + +ament_target_dependencies(test_map_saver_cli rclcpp nav_msgs) +target_link_libraries(test_map_saver_cli + stdc++fs + ${dependencies} +) diff --git a/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp b/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp new file mode 100644 index 00000000000..5b0c3a0339e --- /dev/null +++ b/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp @@ -0,0 +1,146 @@ +// Copyright (c) 2020 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. + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +TEST(MapSaverCLI, CLITest) +{ + std::string path = "/tmp/"; + std::string file = "test_map"; + std::string file_path = path + file; + + rclcpp::init(0, nullptr); + + auto node = std::make_shared("CLI_Test_Node"); + RCLCPP_INFO(node->get_logger(), "Testing Map Saver CLI"); + + auto publisher = node->create_publisher( + "/map", + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + auto msg = std::make_unique(); + msg->header.frame_id = "map"; + msg->header.stamp = node->now(); + msg->info.map_load_time = node->now(); + msg->info.resolution = 0.05; + msg->info.width = 3; + msg->info.height = 3; + msg->info.origin.position.x = 0.0; + msg->info.origin.position.y = 0.0; + msg->info.origin.orientation.w = 1.0; + msg->data.resize(9); + msg->data[0] = 0; + msg->data[2] = 100; + msg->data[1] = 101; + msg->data[3] = 50; + + RCLCPP_INFO(node->get_logger(), "Publishing occupancy grid..."); + + publisher->publish(std::move(msg)); + + rclcpp::Rate(1).sleep(); + + // succeed on real map + RCLCPP_INFO(node->get_logger(), "Calling saver..."); + + EXPECT_FALSE(std::experimental::filesystem::exists(file_path + ".yaml")); + + std::string command = + std::string( + "ros2 run nav2_map_server map_saver_cli -f ") + file_path; + auto return_code = system(command.c_str()); + EXPECT_EQ(return_code, 0); + + rclcpp::Rate(0.25).sleep(); + + RCLCPP_INFO(node->get_logger(), "Checking on file..."); + + EXPECT_TRUE(std::experimental::filesystem::exists(file_path + ".pgm")); + EXPECT_EQ(std::experimental::filesystem::file_size(file_path + ".pgm"), 20ul); + + if (std::experimental::filesystem::exists(file_path + ".yaml")) { + std::experimental::filesystem::remove(file_path + ".yaml"); + } + if (std::experimental::filesystem::exists(file_path + ".pgm")) { + std::experimental::filesystem::remove(file_path + ".pgm"); + } + + // fail on bogus map + RCLCPP_INFO(node->get_logger(), "Calling saver..."); + + EXPECT_FALSE(std::experimental::filesystem::exists(file_path + ".yaml")); + + command = + std::string( + "ros2 run nav2_map_server map_saver_cli " + "-t map_failure --occ 100 --free 2 --mode trinary --fmt png -f ") + file_path + + std::string("--ros-args __node:=map_saver_test_node"); + return_code = system(command.c_str()); + EXPECT_EQ(return_code, 65280); + + rclcpp::Rate(0.25).sleep(); + + RCLCPP_INFO(node->get_logger(), "Checking on file..."); + + EXPECT_FALSE(std::experimental::filesystem::exists(file_path + ".yaml")); + + RCLCPP_INFO(node->get_logger(), "Testing help..."); + command = + std::string( + "ros2 run nav2_map_server map_saver_cli -h"); + return_code = system(command.c_str()); + EXPECT_EQ(return_code, 0); + + rclcpp::Rate(0.5).sleep(); + + RCLCPP_INFO(node->get_logger(), "Testing invalid mode..."); + command = + std::string( + "ros2 run nav2_map_server map_saver_cli --mode fake_mode"); + return_code = system(command.c_str()); + EXPECT_EQ(return_code, 0); + + rclcpp::Rate(0.5).sleep(); + + RCLCPP_INFO(node->get_logger(), "Testing missing argument..."); + command = + std::string( + "ros2 run nav2_map_server map_saver_cli --mode"); + return_code = system(command.c_str()); + EXPECT_EQ(return_code, 65280); + + rclcpp::Rate(0.5).sleep(); + + RCLCPP_INFO(node->get_logger(), "Testing wrong argument..."); + command = + std::string( + "ros2 run nav2_map_server map_saver_cli --free 0 0"); + return_code = system(command.c_str()); + EXPECT_EQ(return_code, 65280); + + rclcpp::Rate(0.5).sleep(); + + command = + std::string( + "ros2 run nav2_map_server map_saver_cli --ros-args -r __node:=map_saver_test_node"); + return_code = system(command.c_str()); + EXPECT_EQ(return_code, 0); +} diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index b69bb602eca..6d2a6f2f356 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 0.3.4 + 0.4.7 Messages and service files for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp index a3e79cce733..a4ce84404a8 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp @@ -219,6 +219,10 @@ class NavFn */ void updateCellAstar(int n); + /** + * @brief Set up navigation potential arrays for new propagation + * @param keepit whether or not use COST_NEUTRAL + */ void setupNavFn(bool keepit = false); /** @@ -253,18 +257,24 @@ class NavFn */ int calcPath(int n, int * st = NULL); + /** + * @brief Calculate gradient at a cell + * @param n Cell number + * @return float norm + */ float gradCell(int n); /**< calculates gradient at cell , returns norm */ + float pathStep; /**< step size for following gradient */ /** display callback */ /**< is the number of cycles between updates */ - void display(void fn(NavFn * nav), int n = 100); - int displayInt; /**< save second argument of display() above */ - void (* displayFn)(NavFn * nav); /**< display function itself */ + // void display(void fn(NavFn * nav), int n = 100); + // int displayInt; /**< save second argument of display() above */ + // void (* displayFn)(NavFn * nav); /**< display function itself */ /** save costmap */ /**< write out costmap and start/goal states as fname.pgm and fname.txt */ - void savemap(const char * fname); + // void savemap(const char * fname); }; } // namespace nav2_navfn_planner diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index e6637ce4a36..66f57790b78 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -37,61 +37,114 @@ namespace nav2_navfn_planner class NavfnPlanner : public nav2_core::GlobalPlanner { public: + /** + * @brief constructor + */ NavfnPlanner(); + + /** + * @brief destructor + */ ~NavfnPlanner(); - // plugin configure + /** + * @brief Configuring plugin + * @param parent Lifecycle node pointer + * @param name Name of plugin map + * @param tf Shared ptr of TF2 buffer + * @param costmap_ros Costmap2DROS object + */ void configure( rclcpp_lifecycle::LifecycleNode::SharedPtr parent, std::string name, std::shared_ptr tf, std::shared_ptr costmap_ros) override; - // plugin cleanup + /** + * @brief Cleanup lifecycle node + */ void cleanup() override; - // plugin activate + /** + * @brief Activate lifecycle node + */ void activate() override; - // plugin deactivate + /** + * @brief Deactivate lifecycle node + */ void deactivate() override; - // plugin create path + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @return nav_msgs::Path of the generated path + */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; protected: - // Compute a plan given start and goal poses, provided in global world frame. + /** + * @brief Compute a plan given start and goal poses, provided in global world frame. + * @param start Start pose + * @param goal Goal pose + * @param tolerance Relaxation constraint in x and y + * @param plan Path to be computed + * @return true if can find the path + */ bool makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, nav_msgs::msg::Path & plan); - // Compute the navigation function given a seed point in the world to start from + /** + * @brief Compute the navigation function given a seed point in the world to start from + * @param world_point Point in world coordinate frame + * @return true if can compute + */ bool computePotential(const geometry_msgs::msg::Point & world_point); - // Compute a plan to a goal from a potential - must call computePotential first + /** + * @brief Compute a plan to a goal from a potential - must call computePotential first + * @param goal Goal pose + * @param plan Path to be computed + * @return true if can compute a plan path + */ bool getPlanFromPotential( const geometry_msgs::msg::Pose & goal, nav_msgs::msg::Path & plan); - // Remove artifacts at the end of the path - originated from planning on a discretized world + /** + * @brief Remove artifacts at the end of the path - originated from planning on a discretized world + * @param goal Goal pose + * @param plan Computed path + */ void smoothApproachToGoal( const geometry_msgs::msg::Pose & goal, nav_msgs::msg::Path & plan); - // Compute the potential, or navigation cost, at a given point in the world - // - must call computePotential first + /** + * @brief Compute the potential, or navigation cost, at a given point in the world + * must call computePotential first + * @param world_point Point in world coordinate frame + * @return double point potential (navigation cost) + */ double getPointPotential(const geometry_msgs::msg::Point & world_point); // Check for a valid potential value at a given point in the world // - must call computePotential first // - currently unused - bool validPointPotential(const geometry_msgs::msg::Point & world_point); - bool validPointPotential(const geometry_msgs::msg::Point & world_point, double tolerance); - - // Compute the squared distance between two points + // bool validPointPotential(const geometry_msgs::msg::Point & world_point); + // bool validPointPotential(const geometry_msgs::msg::Point & world_point, double tolerance); + + /** + * @brief Compute the squared distance between two points + * @param p1 Point 1 + * @param p2 Point 2 + * @return double squared distance between two points + */ inline double squared_distance( const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2) @@ -101,16 +154,36 @@ class NavfnPlanner : public nav2_core::GlobalPlanner return dx * dx + dy * dy; } - // Transform a point from world to map frame + /** + * @brief Transform a point from world to map frame + * @param wx double of world X coordinate + * @param wy double of world Y coordinate + * @param mx int of map X coordinate + * @param my int of map Y coordinate + * @return true if can transform + */ bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my); - // Transform a point from map to world frame + /** + * @brief Transform a point from map to world frame + * @param mx double of map X coordinate + * @param my double of map Y coordinate + * @param wx double of world X coordinate + * @param wy double of world Y coordinate + */ void mapToWorld(double mx, double my, double & wx, double & wy); - // Set the corresponding cell cost to be free space + /** + * @brief Set the corresponding cell cost to be free space + * @param mx int of map X coordinate + * @param my int of map Y coordinate + */ void clearRobotCell(unsigned int mx, unsigned int my); - // Determine if a new planner object should be made + /** + * @brief Determine if a new planner object should be made + * @return true if planner object is out of date + */ bool isPlannerOutOfDate(); // Planner based on ROS1 NavFn algorithm @@ -137,6 +210,16 @@ class NavfnPlanner : public nav2_core::GlobalPlanner // Whether to use the astar planner or default dijkstras bool use_astar_; + + // Subscription for parameter change + rclcpp::AsyncParametersClient::SharedPtr parameters_client_; + rclcpp::Subscription::SharedPtr parameter_event_sub_; + + /** + * @brief Callback executed when a paramter change is detected + * @param event ParameterEvent message + */ + void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); }; } // namespace nav2_navfn_planner diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 7f28ff36e57..bf52cb0e117 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 0.3.4 + 0.4.7 TODO Steve Macenski Carlos Orduno diff --git a/nav2_navfn_planner/src/navfn.cpp b/nav2_navfn_planner/src/navfn.cpp index 8025ff150dd..7cc1002892a 100644 --- a/nav2_navfn_planner/src/navfn.cpp +++ b/nav2_navfn_planner/src/navfn.cpp @@ -55,6 +55,8 @@ namespace nav2_navfn_planner // if the size of the environment does not change // +// Example usage: +/* int create_nav_plan_astar( COSTTYPE * costmap, int nx, int ny, @@ -100,7 +102,7 @@ create_nav_plan_astar( return len; } - +*/ // // create nav fn buffers @@ -129,8 +131,8 @@ NavFn::NavFn(int xs, int ys) start[0] = start[1] = 0; // display function - displayFn = NULL; - displayInt = 0; + // displayFn = NULL; + // displayInt = 0; // path buffers npathbuf = npath = 0; @@ -605,9 +607,9 @@ NavFn::propNavFnDijkstra(int cycles, bool atStart) updateCell(*pb++); } - if (displayInt > 0 && (cycle % displayInt) == 0) { - displayFn(this); - } + // if (displayInt > 0 && (cycle % displayInt) == 0) { + // displayFn(this); + // } // swap priority blocks curP <=> nextP curPe = nextPe; @@ -691,9 +693,9 @@ NavFn::propNavFnAstar(int cycles) updateCellAstar(*pb++); } - if (displayInt > 0 && (cycle % displayInt) == 0) { - displayFn(this); - } + // if (displayInt > 0 && (cycle % displayInt) == 0) { + // displayFn(this); + // } // swap priority blocks curP <=> nextP curPe = nextPe; @@ -983,12 +985,12 @@ NavFn::gradCell(int n) // is the number of cycles to wait before displaying, // use 0 to turn it off -void -NavFn::display(void fn(NavFn * nav), int n) -{ - displayFn = fn; - displayInt = n; -} +// void +// NavFn::display(void fn(NavFn * nav), int n) +// { +// displayFn = fn; +// displayInt = n; +// } // @@ -996,35 +998,35 @@ NavFn::display(void fn(NavFn * nav), int n) // saves costmap and start/goal // -void -NavFn::savemap(const char * fname) -{ - char fn[4096]; - - RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "[NavFn] Saving costmap and start/goal points"); - // write start and goal points - snprintf(fn, sizeof(fn), "%s.txt", fname); - FILE * fp = fopen(fn, "w"); - if (!fp) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Can't open file %s", fn); - return; - } - fprintf(fp, "Goal: %d %d\nStart: %d %d\n", goal[0], goal[1], start[0], start[1]); - fclose(fp); - - // write cost array - if (!costarr) { - return; - } - snprintf(fn, sizeof(fn), "%s.pgm", fname); - fp = fopen(fn, "wb"); - if (!fp) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Can't open file %s", fn); - return; - } - fprintf(fp, "P5\n%d\n%d\n%d\n", nx, ny, 0xff); - fwrite(costarr, 1, nx * ny, fp); - fclose(fp); -} +// void +// NavFn::savemap(const char * fname) +// { +// char fn[4096]; + +// RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "[NavFn] Saving costmap and start/goal points"); +// // write start and goal points +// snprintf(fn, sizeof(fn), "%s.txt", fname); +// FILE * fp = fopen(fn, "w"); +// if (!fp) { +// RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Can't open file %s", fn); +// return; +// } +// fprintf(fp, "Goal: %d %d\nStart: %d %d\n", goal[0], goal[1], start[0], start[1]); +// fclose(fp); + +// // write cost array +// if (!costarr) { +// return; +// } +// snprintf(fn, sizeof(fn), "%s.pgm", fname); +// fp = fopen(fn, "wb"); +// if (!fp) { +// RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Can't open file %s", fn); +// return; +// } +// fprintf(fp, "P5\n%d\n%d\n%d\n", nx, ny, 0xff); +// fwrite(costarr, 1, nx * ny, fp); +// fclose(fp); +// } } // namespace nav2_navfn_planner diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 29c97db6342..975e5f82881 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -19,6 +19,8 @@ // the Global Dynamic Window Approach. IEEE. // https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf +// #define BENCHMARK_TESTING + #include "nav2_navfn_planner/navfn_planner.hpp" #include @@ -38,6 +40,8 @@ using namespace std::chrono_literals; using nav2_util::declare_parameter_if_not_declared; +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; namespace nav2_navfn_planner { @@ -83,6 +87,16 @@ NavfnPlanner::configure( planner_ = std::make_unique( costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); + + // Setup callback for changes to parameters. + parameters_client_ = std::make_shared( + node_->get_node_base_interface(), + node_->get_node_topics_interface(), + node_->get_node_graph_interface(), + node_->get_node_services_interface()); + + parameter_event_sub_ = parameters_client_->on_parameter_event( + std::bind(&NavfnPlanner::on_parameter_event_callback, this, _1)); } void @@ -114,6 +128,10 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { +#ifdef BENCHMARK_TESTING + steady_clock::time_point a = steady_clock::now(); +#endif + // Update planner based on the new costmap size if (isPlannerOutOfDate()) { planner_->setNavArr( @@ -128,6 +146,14 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( node_->get_logger(), "%s: failed to create plan with " "tolerance %.2f.", name_.c_str(), tolerance_); } + +#ifdef BENCHMARK_TESTING + steady_clock::time_point b = steady_clock::now(); + duration time_span = duration_cast>(b - a); + std::cout << "It took " << time_span.count() * 1000 << + " milliseconds with " << num_iterations << " iterations." << std::endl; +#endif + return path; } @@ -152,6 +178,9 @@ NavfnPlanner::makePlan( // clear the plan, just in case plan.poses.clear(); + plan.header.stamp = node_->now(); + plan.header.frame_id = global_frame_; + // TODO(orduno): add checks for start and goal reference frame -- should be in global frame double wx = start.position.x; @@ -174,6 +203,8 @@ NavfnPlanner::makePlan( // clear the starting cell within the costmap because we know it can't be an obstacle clearRobotCell(mx, my); + std::unique_lock lock(*(costmap_->getMutex())); + // make sure to resize the underlying array that Navfn uses planner_->setNavArr( costmap_->getSizeInCellsX(), @@ -181,6 +212,8 @@ NavfnPlanner::makePlan( planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_); + lock.unlock(); + int map_start[2]; map_start[0] = mx; map_start[1] = my; @@ -310,22 +343,22 @@ NavfnPlanner::getPlanFromPotential( planner_->setStart(map_goal); - int path_len = planner_->calcPath(costmap_->getSizeInCellsX() * 4); + const int & max_cycles = (costmap_->getSizeInCellsX() >= costmap_->getSizeInCellsY()) ? + (costmap_->getSizeInCellsX() * 4) : (costmap_->getSizeInCellsY() * 4); + + int path_len = planner_->calcPath(max_cycles); if (path_len == 0) { - RCLCPP_DEBUG(node_->get_logger(), "No path found\n"); return false; } - RCLCPP_DEBUG(node_->get_logger(), "Path found, %d steps\n", path_len); + auto cost = planner_->getLastPathCost(); + RCLCPP_DEBUG(node_->get_logger(), "Path found, %d steps, %f cost\n", path_len, cost); // extract the plan float * x = planner_->getPathX(); float * y = planner_->getPathY(); int len = planner_->getPathLen(); - plan.header.stamp = node_->now(); - plan.header.frame_id = global_frame_; - for (int i = len - 1; i >= 0; --i) { // convert the plan to world coordinates double world_x, world_y; @@ -357,51 +390,47 @@ NavfnPlanner::getPointPotential(const geometry_msgs::msg::Point & world_point) return planner_->potarr[index]; } -bool -NavfnPlanner::validPointPotential(const geometry_msgs::msg::Point & world_point) -{ - return validPointPotential(world_point, tolerance_); -} - -bool -NavfnPlanner::validPointPotential( - const geometry_msgs::msg::Point & world_point, double tolerance) -{ - const double resolution = costmap_->getResolution(); - - geometry_msgs::msg::Point p = world_point; - double potential = getPointPotential(p); - if (potential < POT_HIGH) { - // world_point is reachable by itself - return true; - } else { - // world_point, is not reachable. Trying to find any - // reachable point within its tolerance region - p.y = world_point.y - tolerance; - while (p.y <= world_point.y + tolerance) { - p.x = world_point.x - tolerance; - while (p.x <= world_point.x + tolerance) { - potential = getPointPotential(p); - if (potential < POT_HIGH) { - return true; - } - p.x += resolution; - } - p.y += resolution; - } - } - - return false; -} +// bool +// NavfnPlanner::validPointPotential(const geometry_msgs::msg::Point & world_point) +// { +// return validPointPotential(world_point, tolerance_); +// } + +// bool +// NavfnPlanner::validPointPotential( +// const geometry_msgs::msg::Point & world_point, double tolerance) +// { +// const double resolution = costmap_->getResolution(); + +// geometry_msgs::msg::Point p = world_point; +// double potential = getPointPotential(p); +// if (potential < POT_HIGH) { +// // world_point is reachable by itself +// return true; +// } else { +// // world_point, is not reachable. Trying to find any +// // reachable point within its tolerance region +// p.y = world_point.y - tolerance; +// while (p.y <= world_point.y + tolerance) { +// p.x = world_point.x - tolerance; +// while (p.x <= world_point.x + tolerance) { +// potential = getPointPotential(p); +// if (potential < POT_HIGH) { +// return true; +// } +// p.x += resolution; +// } +// p.y += resolution; +// } +// } + +// return false; +// } bool NavfnPlanner::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) { if (wx < costmap_->getOriginX() || wy < costmap_->getOriginY()) { - RCLCPP_ERROR( - node_->get_logger(), "worldToMap failed: wx,wy: %f,%f, " - "size_x,size_y: %d,%d", wx, wy, - costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); return false; } @@ -436,6 +465,29 @@ NavfnPlanner::clearRobotCell(unsigned int mx, unsigned int my) costmap_->setCost(mx, my, nav2_costmap_2d::FREE_SPACE); } +void +NavfnPlanner::on_parameter_event_callback( + const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +{ + for (auto & changed_parameter : event->changed_parameters) { + const auto & type = changed_parameter.value.type; + const auto & name = changed_parameter.name; + const auto & value = changed_parameter.value; + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == name_ + ".tolerance") { + tolerance_ = value.double_value; + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == name_ + ".use_astar") { + use_astar_ = value.bool_value; + } else if (name == name_ + ".allow_unknown") { + allow_unknown_ = value.bool_value; + } + } + } +} + } // namespace nav2_navfn_planner #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 3e67e6d089e..175b69165c6 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -58,6 +58,17 @@ class PlannerServer : public nav2_util::LifecycleNode using PlannerMap = std::unordered_map; + /** + * @brief Method to get plan from the desired plugin + * @param start starting pose + * @param goal goal request + * @return Path + */ + nav_msgs::msg::Path getPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal, + const std::string & planner_id); + protected: /** * @brief Configure member variables and initializes planner @@ -89,14 +100,9 @@ class PlannerServer : public nav2_util::LifecycleNode * @return SUCCESS or FAILURE */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when in error state - * @param state Reference to LifeCycle node state - * @return SUCCESS or FAILURE - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; - using ActionServer = nav2_util::SimpleActionServer; + using ActionT = nav2_msgs::action::ComputePathToPose; + using ActionServer = nav2_util::SimpleActionServer; // Our action server implements the ComputePathToPose action std::unique_ptr action_server_; @@ -135,9 +141,6 @@ class PlannerServer : public nav2_util::LifecycleNode // Publishers for the path rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; - - // Whether we've published the single planner warning yet - bool single_planner_warning_given_{false}; }; } // namespace nav2_planner diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 8fdf85eb178..e762c67d0e7 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 0.3.4 + 0.4.7 TODO Steve Macenski Apache-2.0 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index edf273e87fe..48eede36868 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -46,7 +46,7 @@ PlannerServer::PlannerServer() // Declare this node's parameters declare_parameter("planner_plugins", default_ids_); - declare_parameter("expected_planner_frequency", 20.0); + declare_parameter("expected_planner_frequency", 1.0); // Setup the global costmap costmap_ros_ = std::make_shared( @@ -59,10 +59,8 @@ PlannerServer::PlannerServer() PlannerServer::~PlannerServer() { RCLCPP_INFO(get_logger(), "Destroying"); - PlannerMap::iterator it; - for (it = planners_.begin(); it != planners_.end(); ++it) { - it->second.reset(); - } + planners_.clear(); + costmap_thread_.reset(); } nav2_util::CallbackReturn @@ -104,7 +102,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_FATAL( get_logger(), "Failed to create global planner. Exception: %s", ex.what()); - exit(-1); + return nav2_util::CallbackReturn::FAILURE; } } @@ -112,17 +110,20 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) planner_ids_concat_ += planner_ids_[i] + std::string(" "); } + RCLCPP_INFO( + get_logger(), + "Planner Server has %s planners available.", planner_ids_concat_.c_str()); + double expected_planner_frequency; get_parameter("expected_planner_frequency", expected_planner_frequency); if (expected_planner_frequency > 0) { max_planner_duration_ = 1 / expected_planner_frequency; } else { - max_planner_duration_ = 0.0; - RCLCPP_WARN( get_logger(), - "The expected planner frequency parameter is %.4f Hz. The value has to be greater" - " than 0.0 to turn on displaying warning messages", expected_planner_frequency); + "The expected planner frequency parameter is %.4f Hz. The value should to be greater" + " than 0.0 to turn on duration overrrun warning messages", expected_planner_frequency); + max_planner_duration_ = 0.0; } // Initialize pubs & subs @@ -186,17 +187,11 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state) it->second->cleanup(); } planners_.clear(); + costmap_ = nullptr; return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -PlannerServer::on_error(const rclcpp_lifecycle::State &) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn PlannerServer::on_shutdown(const rclcpp_lifecycle::State &) { @@ -214,13 +209,8 @@ PlannerServer::computePlan() auto result = std::make_shared(); try { - if (action_server_ == nullptr) { - RCLCPP_DEBUG(get_logger(), "Action server unavailable. Stopping."); - return; - } - - if (!action_server_->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "Action server is inactive. Stopping."); + if (action_server_ == nullptr || !action_server_->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); return; } @@ -232,39 +222,15 @@ PlannerServer::computePlan() geometry_msgs::msg::PoseStamped start; if (!costmap_ros_->getRobotPose(start)) { - RCLCPP_ERROR(this->get_logger(), "Could not get robot pose"); + action_server_->terminate_current(); return; } if (action_server_->is_preempt_requested()) { - RCLCPP_INFO(get_logger(), "Preempting the goal pose."); goal = action_server_->accept_pending_goal(); } - RCLCPP_DEBUG( - get_logger(), "Attempting to a find path from (%.2f, %.2f) to " - "(%.2f, %.2f).", start.pose.position.x, start.pose.position.y, - goal->pose.pose.position.x, goal->pose.pose.position.y); - - if (planners_.find(goal->planner_id) != planners_.end()) { - result->path = planners_[goal->planner_id]->createPlan(start, goal->pose); - } else { - if (planners_.size() == 1 && goal->planner_id.empty()) { - if (!single_planner_warning_given_) { - single_planner_warning_given_ = true; - RCLCPP_WARN( - get_logger(), "No planners specified in action call. " - "Server will use only plugin %s in server." - " This warning will appear once.", planner_ids_concat_.c_str()); - } - result->path = planners_[planners_.begin()->first]->createPlan(start, goal->pose); - } else { - RCLCPP_ERROR( - get_logger(), "planner %s is not a valid planner. " - "Planner names are: %s", goal->planner_id.c_str(), - planner_ids_concat_.c_str()); - } - } + result->path = getPlan(start, goal->pose, goal->planner_id); if (result->path.poses.size() == 0) { RCLCPP_WARN( @@ -282,7 +248,6 @@ PlannerServer::computePlan() goal->pose.pose.position.y); // Publish the plan for visualization purposes - RCLCPP_DEBUG(get_logger(), "Publishing the valid path"); publishPlan(result->path); auto cycle_duration = steady_clock_.now() - start_time; @@ -296,8 +261,6 @@ PlannerServer::computePlan() } action_server_->succeeded_current(result); - - return; } catch (std::exception & ex) { RCLCPP_WARN( get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"", @@ -306,24 +269,50 @@ PlannerServer::computePlan() // TODO(orduno): provide information about fail error to parent task, // for example: couldn't get costmap update action_server_->terminate_current(); - return; - } catch (...) { - RCLCPP_WARN( - get_logger(), "Plan calculation failed, " - "An unexpected error has occurred. The planner server" - " may not be able to continue operating correctly."); - // TODO(orduno): provide information about fail error to parent task, - // for example: couldn't get costmap update - action_server_->terminate_current(); - return; } } +nav_msgs::msg::Path +PlannerServer::getPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal, + const std::string & planner_id) +{ + RCLCPP_DEBUG( + get_logger(), "Attempting to a find path from (%.2f, %.2f) to " + "(%.2f, %.2f).", start.pose.position.x, start.pose.position.y, + goal.pose.position.x, goal.pose.position.y); + + if (planners_.find(planner_id) != planners_.end()) { + return planners_[planner_id]->createPlan(start, goal); + } else { + if (planners_.size() == 1 && planner_id.empty()) { + RCLCPP_WARN_ONCE( + get_logger(), "No planners specified in action call. " + "Server will use only plugin %s in server." + " This warning will appear once.", planner_ids_concat_.c_str()); + return planners_[planners_.begin()->first]->createPlan(start, goal); + } else { + RCLCPP_ERROR( + get_logger(), "planner %s is not a valid planner. " + "Planner names are: %s", planner_id.c_str(), + planner_ids_concat_.c_str()); + } + } + + return nav_msgs::msg::Path(); +} + void PlannerServer::publishPlan(const nav_msgs::msg::Path & path) { auto msg = std::make_unique(path); - plan_publisher_->publish(std::move(msg)); + if ( + plan_publisher_->is_activated() && + this->count_subscribers(plan_publisher_->get_topic_name()) > 0) + { + plan_publisher_->publish(std::move(msg)); + } } } // namespace nav2_planner diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index 04aed1b02a1..7edb732d00c 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp @@ -107,7 +107,7 @@ class Recovery : public nav2_core::Recovery collision_checker_ = collision_checker; - vel_pub_ = node_->create_publisher("cmd_vel", 1); + vel_pub_ = node_->template create_publisher("cmd_vel", 1); onConfigure(); } @@ -175,7 +175,7 @@ class Recovery : public nav2_core::Recovery // Initialize the ActionT result auto result = std::make_shared(); - rclcpp::Rate loop_rate(cycle_frequency_); + rclcpp::WallRate loop_rate(cycle_frequency_); while (rclcpp::ok()) { if (action_server_->is_cancel_requested()) { diff --git a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp index 1c02fc1f598..26c497d4585 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp @@ -37,7 +37,7 @@ class RecoveryServer : public nav2_util::LifecycleNode RecoveryServer(); ~RecoveryServer(); - void loadRecoveryPlugins(); + bool loadRecoveryPlugins(); protected: // Implement the lifecycle interface @@ -46,7 +46,6 @@ class RecoveryServer : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; std::shared_ptr tf_; std::shared_ptr transform_listener_; diff --git a/nav2_recoveries/package.xml b/nav2_recoveries/package.xml index 5bd42ecbb60..62f2f79540c 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_recoveries/package.xml @@ -2,7 +2,7 @@ nav2_recoveries - 0.3.4 + 0.4.7 TODO Carlos Orduno Steve Macenski diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index 3f258402036..b429c8bdcf3 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -120,10 +120,12 @@ bool BackUp::isCollisionFree( double sim_position_change; const double diff_dist = abs(command_x_) - distance; const int max_cycle_count = static_cast(cycle_frequency_ * simulate_ahead_time_); + geometry_msgs::msg::Pose2D init_pose = pose2d; while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->linear.x * (cycle_count / cycle_frequency_); - pose2d.x += sim_position_change; + pose2d.x = init_pose.x + sim_position_change * cos(init_pose.theta); + pose2d.y = init_pose.y + sim_position_change * sin(init_pose.theta); cycle_count++; if (diff_dist - abs(sim_position_change) <= 0.) { diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index e24be17d899..103623b15ca 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -150,10 +150,11 @@ bool Spin::isCollisionFree( int cycle_count = 0; double sim_position_change; const int max_cycle_count = static_cast(cycle_frequency_ * simulate_ahead_time_); + geometry_msgs::msg::Pose2D init_pose = pose2d; while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->angular.z * (cycle_count / cycle_frequency_); - pose2d.theta += sim_position_change; + pose2d.theta = init_pose.theta + sim_position_change; cycle_count++; if (abs(relative_yaw) - abs(sim_position_change) <= 0.) { diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index 49b7428d1f3..bd78ae85f85 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_recoveries/src/recovery_server.cpp @@ -88,13 +88,15 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) } } recovery_types_.resize(recovery_ids_.size()); - loadRecoveryPlugins(); + if (!loadRecoveryPlugins()) { + return nav2_util::CallbackReturn::FAILURE; + } return nav2_util::CallbackReturn::SUCCESS; } -void +bool RecoveryServer::loadRecoveryPlugins() { auto node = shared_from_this(); @@ -112,9 +114,11 @@ RecoveryServer::loadRecoveryPlugins() get_logger(), "Failed to create recovery %s of type %s." " Exception: %s", recovery_ids_[i].c_str(), recovery_types_[i].c_str(), ex.what()); - exit(-1); + return false; } } + + return true; } nav2_util::CallbackReturn @@ -162,13 +166,6 @@ RecoveryServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -RecoveryServer::on_error(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn RecoveryServer::on_shutdown(const rclcpp_lifecycle::State &) { diff --git a/nav2_recoveries/test/test_recoveries.cpp b/nav2_recoveries/test/test_recoveries.cpp index a838803f87b..6f5c83912ed 100644 --- a/nav2_recoveries/test/test_recoveries.cpp +++ b/nav2_recoveries/test/test_recoveries.cpp @@ -157,7 +157,7 @@ class RecoveryTest : public ::testing::Test auto future_goal = client_->async_send_goal(goal); if (rclcpp::spin_until_future_complete(node_lifecycle_, future_goal) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { std::cout << "failed sending goal" << std::endl; // failed sending the goal diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt new file mode 100644 index 00000000000..9c87714fad7 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_regulated_pure_pursuit_controller) + +find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_util REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(tf2 REQUIRED) + +nav2_package() +set(CMAKE_CXX_STANDARD 17) + +include_directories( + include +) + +set(dependencies + rclcpp + geometry_msgs + nav2_costmap_2d + pluginlib + nav_msgs + nav2_util + nav2_core + tf2 +) + +set(library_name nav2_regulated_pure_pursuit_controller) + +add_library(${library_name} SHARED + src/regulated_pure_pursuit_controller.cpp) + +# prevent pluginlib from using boost +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +ament_target_dependencies(${library_name} + ${dependencies} +) + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + add_subdirectory(test) +endif() + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) + +pluginlib_export_plugin_description_file(nav2_core nav2_regulated_pure_pursuit_controller.xml) + +ament_package() + diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md new file mode 100644 index 00000000000..285797499c9 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -0,0 +1,147 @@ +# Nav2 Regulated Pure Pursuit Controller + +This is a controller (local trajectory planner) that implements a variant on the pure pursuit algorithm to track a path. This variant we call the Regulated Pure Pursuit Algorithm, due to its additional regulation terms on collision and linear speed. It also implements the basics behind the Adaptive Pure Pursuit algorithm to vary lookahead distances by current speed. It was developed by [Shrijit Singh](https://www.linkedin.com/in/shrijitsingh99/) and [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/) as part of the Nav2 working group. + +Code based on a simplified version of this controller is referenced in the [Writing a New Nav2 Controller](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html) tutorial. + +This plugin implements the `nav2_core::Controller` interface allowing it to be used across the navigation stack as a local trajectory planner in the controller server's action server (`controller_server`). + +It builds on top of the ordinary pure pursuit algorithm in a number of ways. It also implements all the common variants of the pure pursuit algorithm such as adaptive pure pursuit. This controller is suitable for use on all types of robots, including differential, legged, and ackermann steering vehicles. It may also be used on omni-directional platforms, but won't be able to fully leverage the lateral movements of the base (you may consider DWB instead). + +This controller has been measured to run at well over 1 kHz on a modern intel processor. + +

+ +

+ +## Pure Pursuit Basics + +The Pure Pursuit algorithm has been in use for over 30 years. You can read more about the details of the pure pursuit controller in its [introduction paper](http://www.enseignement.polytechnique.fr/profs/informatique/Eric.Goubault/MRIS/coulter_r_craig_1992_1.pdf). The core idea is to find a point on the path in front of the robot and find the linear and angular velocity to help drive towards it. Once it moves forward, a new point is selected, and the process repeats until the end of the path. The distance used to find the point to drive towards is the `lookahead` distance. + +In order to simply book-keeping, the global path is continuously pruned to the closest point to the robot (see the figure below) so that we only have to process useful path points. Then, the section of the path within the local costmap bounds is transformed to the robot frame and a lookahead point is determined using a predefined distance. + +Finally, the lookahead point will be given to the pure pursuit algorithm which finds the curvature of the path required to drive the robot to the lookahead point. This curvature is then applied to the velocity commands to allow the robot to drive. + +![Lookahead algorithm](./doc/lookahead_algorithm.png) + +## Regulated Pure Pursuit Features + +We have created a new variation on the pure pursuit algorithm that we dubb the Regulated Pure Pursuit algorithm. We combine the features of the Adaptive Pure Pursuit algorithm with rules around linear velocity with a focus on consumer, industrial, and service robot's needs. We also implement several common-sense safety mechanisms like collision detection and ensuring that commands are kinematically feasible that are missing from all other variants of pure pursuit (for some remarkable reason). + +The Regulated Pure Pursuit controller implements active collision detection. We use a parameter to set the maximum allowable time before a potential collision on the current velocity command. Using the current linear and angular velocity, we project forward in time that duration and check for collisions. Intuitively, you may think that collision checking between the robot and the lookahead point seems logical. However, if you're maneuvering in tight spaces, it makes alot of sense to only search forward a given amount of time to give the system a little leeway to get itself out. In confined spaces especially, we want to make sure that we're collision checking a reasonable amount of space for the current action being taken (e.g. if moving at 0.1 m/s, it makes no sense to look 10 meters ahead to the carrot, or 100 seconds into the future). This helps look further at higher speeds / angular rotations and closer with fine, slow motions in constrained environments so it doesn't over report collisions from valid motions near obstacles. If you set the maximum allowable to a large number, it will collision check all the way, but not exceeding, the lookahead point. We visualize the collision checking arc on the `lookahead_arc` topic. + +The regulated pure pursuit algorithm also makes use of the common variations on the pure pursuit algorithm. We implement the adaptive pure pursuit's main contribution of having velocity-scaled lookahead point distances. This helps make the controller more stable over a larger range of potential linear velocities. There are parameters for setting the lookahead gain (or lookahead time) and thresholded values for minimum and maximum. + +We also implement kinematic speed limits on the linear velocities in operations and angular velocities during pure rotations. This makes sure that the output commands are smooth, safe, and kinematically feasible. This is especially important at the beginning and end of a path tracking task, where you are ramping up to speed and slowing down to the goal. + +The final minor improvement we make is slowing on approach to the goal. Knowing that the optimal lookahead distance is `X`, we can take the difference in `X` and the actual distance of the lookahead point found to find the lookahead point error. During operations, the variation in this error should be exceptionally small and won't be triggered. However, at the end of the path, there are no more points at a lookahead distance away from the robot, so it uses the last point on the path. So as the robot approaches a target, its error will grow and the robot's velocity will be reduced proportional to this error until a minimum threshold. This is also tracked by the kinematic speed limits to ensure drivability. + +The major improvements that this work implements is the regulations on the linear velocity based on some cost functions. They were selected to remove long-standing bad behavior within the pure pursuit algorithm. Normal Pure Pursuit has an issue with overshoot and poor handling in particularly high curvature (or extremely rapidly changing curvature) environments. It is commonly known that this will cause the robot to overshoot from the path and potentially collide with the environment. These cost functions in the Regulated Pure Pursuit algorithm were also chosen based on common requirements and needs of mobile robots uses in service, commercial, and industrial use-cases; scaling by curvature creates intuitive behavior of slowing the robot when making sharp turns and slowing when its near a potential collision so that small variations don't clip obstacles. This is also really useful when working in partially observable environments (like turning in and out of aisles / hallways often) so that you slow before a sharp turn into an unknown dynamic environment to be more conservative in case something is in the way immediately requiring a stop. + +The cost functions penalize the robot's speed based on its proximity to obstacles and the curvature of the path. This is helpful to slow the robot when moving close to things in narrow spaces and scaling down the linear velocity by curvature helps to stabilize the controller over a larger range of lookahead point distances. This also has the added benefit of removing the sensitive tuning of the lookahead point / range, as the robot will track paths far better. Tuning is still required, but it is substantially easier to get reasonable behavior with minor adjustments. + +An unintended tertiary benefit of scaling the linear velocities by curvature is that a robot will natively rotate to rough path heading when using holonomic planners that don't start aligned with the robot pose orientation. As the curvature will be very high, the linear velocity drops and the angular velocity takes over to rotate to heading. While not perfect, it does dramatically reduce the need to rotate to a close path heading before following and opens up a broader range of planning techniques. Pure Pursuit controllers otherwise would be completely unable to recover from this in even modestly confined spaces. + +Mixing the proximity and curvature regulated linear velocities with the time-scaled collision checker, we see a near-perfect combination allowing the regulated pure pursuit algorithm to handle high starting deviations from the path and navigate collision-free in tight spaces without overshoot. + +## Configuration + +| Parameter | Description | +|-----|----| +| `desired_linear_vel` | The desired maximum linear velocity to use. | +| `max_linear_accel` | Acceleration for linear velocity | +| `max_linear_decel` | Deceleration for linear velocity | +| `lookahead_dist` | The lookahead distance to use to find the lookahead point | +| `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances | +| `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances | +| `lookahead_time` | The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain. | +| `rotate_to_heading_angular_vel` | If rotate to heading is used, this is the angular velocity to use. | +| `transform_tolerance` | The TF transform tolerance | +| `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 | +| `use_approach_linear_velocity_scaling` | Whether to scale the linear velocity down on approach to the goal for a smooth stop | +| `max_allowed_time_to_collision` | The time to project a velocity command to check for collisions | +| `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 | +| `cost_scaling_gain` | A multiplier gain, which should be <= 1.0, used to further scale the speed when an obstacle is within `cost_scaling_dist`. Lower value reduces speed more quickly. | +| `inflation_cost_scaling_factor` | The value of `cost_scaling_factor` set for the inflation layer in the local costmap. The value should be exactly the same for accurately computing distance from obstacles using the inflated cell values | +| `regulated_linear_scaling_min_radius` | The turning radius for which the regulation features are triggered. Remember, sharper turns have smaller radii | +| `regulated_linear_scaling_min_speed` | The minimum speed for which the regulated features can send, to ensure process is still achievable even in high cost spaces with high curvature. | +| `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. | +| `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | +| `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | +| `goal_dist_tol` | XY tolerance from goal to rotate to the goal heading, if `use_rotate_to_heading` is enabled. This should match or be smaller than the `GoalChecker`'s translational goal tolerance. | + + +Example fully-described XML with default parameter values: + +``` +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" + controller_plugins: ["FollowPath"] + + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + FollowPath: + plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + desired_linear_vel: 0.5 + max_linear_accel: 2.5 + max_linear_decel: 2.5 + lookahead_dist: 0.6 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 1.8 + transform_tolerance: 0.1 + use_velocity_scaled_lookahead_dist: false + min_approach_linear_velocity: 0.05 + use_approach_linear_velocity_scaling: true + max_allowed_time_to_collision: 1.0 + use_regulated_linear_velocity_scaling: true + use_cost_regulated_linear_velocity_scaling: false + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: true + rotate_to_heading_min_angle: 0.785 + max_angular_accel: 3.2 + goal_dist_tol: 0.25 + cost_scaling_dist: 0.3 + cost_scaling_gain: 1.0 + inflation_cost_scaling_factor: 3.0 +``` + +## Topics + +| Topic | Type | Description | +|-----|----|----| +| `lookahead_point` | `geometry_msgs/PointStamped` | The current lookahead point on the path | +| `lookahead_arc` | `nav_msgs/Path` | The drivable arc between the robot and the carrot. Arc length depends on `max_allowed_time_to_collision`, forward simulating from the robot pose at the commanded `Twist` by that time. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. | + +Note: The `lookahead_arc` is also a really great speed indicator, when "full" to carrot or max time, you know you're at full speed. If 20% less, you can tell the robot is approximately 20% below maximum speed. Think of it as the collision checking bounds but also a speed guage. + +## Notes to users + +By default, the `use_cost_regulated_linear_velocity_scaling` is set to `false` because the generic sandbox environment we have setup is the TB3 world. This is a highly constrained environment so it overly triggers to slow the robot as everywhere has high costs. This is recommended to be set to `true` when not working in constantly high-cost spaces. + +To tune to get Adaptive Pure Pursuit behaviors, set all boolean parameters to false except `use_velocity_scaled_lookahead_dist` and make sure to tune `lookahead_time`, `min_lookahead_dist` and `max_lookahead_dist`. + +To tune to get Pure Pursuit behaviors, set all boolean parameters to false and make sure to tune `lookahead_dist`. + +Currently, there is no rotate to goal behaviors, so it is expected that the path approach orientations are the orientations of the goal or the goal checker has been set with a generous `min_theta_velocity_threshold`. Implementations for rotating to goal heading are on the way. + +The choice of lookahead distances are highly dependent on robot size, responsiveness, controller update rate, and speed. Please make sure to tune this for your platform, although the `regulated` features do largely make heavy tuning of this value unnecessary. If you see wiggling, increase the distance or scale. If it's not converging as fast to the path as you'd like, decrease it. diff --git a/nav2_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png b/nav2_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png new file mode 100644 index 00000000000..8507ddd3e03 Binary files /dev/null and b/nav2_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png differ 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 new file mode 100644 index 00000000000..eb2026a429c --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -0,0 +1,251 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 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. + +#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include "nav2_core/controller.hpp" +#include "rclcpp/rclcpp.hpp" +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +/** + * @class nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + * @brief Regulated pure pursuit controller plugin + */ +class RegulatedPurePursuitController : public nav2_core::Controller +{ +public: + /** + * @brief Constructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + */ + RegulatedPurePursuitController() = default; + + /** + * @brief Destrructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + */ + ~RegulatedPurePursuitController() override = default; + + /** + * @brief Configure controller state machine + * @param parent WeakPtr to node + * @param name Name of plugin + * @param tf TF buffer + * @param costmap_ros Costmap2DROS object of environment + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent, + std::string name, const std::shared_ptr & tf, + const std::shared_ptr & costmap_ros) override; + + /** + * @brief Cleanup controller state machine + */ + void cleanup() override; + + /** + * @brief Activate controller state machine + */ + void activate() override; + + /** + * @brief Deactivate controller state machine + */ + void deactivate() override; + + /** + * @brief Compute the best command given the current pose and velocity, with possible debug information + * + * Same as above computeVelocityCommands, but with debug results. + * If the results pointer is not null, additional information about the twists + * evaluated will be in results after the call. + * + * @param pose Current robot pose + * @param velocity Current robot velocity + * @param results Output param, if not NULL, will be filled in with full evaluation results + * @return Best command + */ + geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & velocity) override; + + /** + * @brief nav2_core setPlan - Sets the global plan + * @param path The global plan + */ + void setPlan(const nav_msgs::msg::Path & path) override; + +protected: + /** + * @brief Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses + * @param pose pose to transform + * @return Path in new frame + */ + nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose); + + /** + * @brief Transform a pose to another frame. + * @param frame Frame ID to transform to + * @param in_pose Pose input to transform + * @param out_pose transformed output + * @return bool if successful + */ + bool transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const; + + /** + * @brief Get lookahead distance + * @param cmd the current speed to use to compute lookahead point + * @return lookahead distance + */ + double getLookAheadDistance(const geometry_msgs::msg::Twist &); + + /** + * @brief Creates a PointStamped message for visualization + * @param carrot_pose Input carrot point as a PoseStamped + * @return CarrotMsg a carrot point marker, PointStamped + */ + std::unique_ptr createCarrotMsg( + const geometry_msgs::msg::PoseStamped & carrot_pose); + + /** + * @brief Whether robot should rotate to rough path heading + * @param carrot_pose current lookahead point + * @param angle_to_path Angle of robot output relatie to carrot marker + * @return Whether should rotate to path heading + */ + bool shouldRotateToPath( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path); + + /** + * @brief Whether robot should rotate to final goal orientation + * @param carrot_pose current lookahead point + * @return Whether should rotate to goal heading + */ + bool shouldRotateToGoalHeading(const geometry_msgs::msg::PoseStamped & carrot_pose); + + /** + * @brief Create a smooth and kinematically smoothed rotation command + * @param linear_vel linear velocity + * @param angular_vel angular velocity + * @param angle_to_path Angle of robot output relatie to carrot marker + * @param curr_speed the current robot speed + */ + void rotateToHeading(double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed); + + /** + * @brief Whether collision is imminent + * @param robot_pose Pose of robot + * @param carrot_pose Pose of carrot + * @param linear_vel linear velocity to forward project + * @param angular_vel angular velocity to forward project + * @return Whether collision is imminent + */ + bool isCollisionImminent( + const geometry_msgs::msg::PoseStamped &, + const double &, const double &); + + /** + * @brief Whether point is in collision + * @param x Pose of pose x + * @param y Pose of pose y + * @return Whether in collision + */ + bool inCollision(const double & x, const double & y); + + /** + * @brief Cost at a point + * @param x Pose of pose x + * @param y Pose of pose y + * @return Cost of pose in costmap + */ + double costAtPose(const double & x, const double & y); + + /** + * @brief apply regulation constraints to the system + * @param linear_vel robot command linear velocity input + * @param dist_error error in the carrot distance and lookahead distance + * @param lookahead_dist optimal lookahead distance + * @param curvature curvature of path + * @param speed Speed of robot + * @param pose_cost cost at this pose + */ + void applyConstraints( + const double & dist_error, const double & lookahead_dist, + const double & curvature, const geometry_msgs::msg::Twist & speed, + const double & pose_cost, double & linear_vel); + + /** + * @brief Get lookahead point + * @param lookahead_dist Optimal lookahead distance + * @param path Current global path + * @return Lookahead point + */ + geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &); + + std::shared_ptr tf_; + std::string plugin_name_; + std::shared_ptr costmap_ros_; + nav2_costmap_2d::Costmap2D * costmap_; + rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; + + double desired_linear_vel_; + double lookahead_dist_; + double rotate_to_heading_angular_vel_; + double max_lookahead_dist_; + double min_lookahead_dist_; + double lookahead_time_; + double max_linear_accel_; + double max_linear_decel_; + bool use_velocity_scaled_lookahead_dist_; + tf2::Duration transform_tolerance_; + bool use_approach_vel_scaling_; + double min_approach_linear_velocity_; + double control_duration_; + double max_allowed_time_to_collision_; + bool use_regulated_linear_velocity_scaling_; + bool use_cost_regulated_linear_velocity_scaling_; + double cost_scaling_dist_; + double cost_scaling_gain_; + double inflation_cost_scaling_factor_; + double regulated_linear_scaling_min_radius_; + double regulated_linear_scaling_min_speed_; + bool use_rotate_to_heading_; + double max_angular_accel_; + double rotate_to_heading_min_angle_; + double goal_dist_tol_; + + nav_msgs::msg::Path global_plan_; + std::shared_ptr> global_path_pub_; + std::shared_ptr> carrot_pub_; + std::shared_ptr> carrot_arc_pub_; +}; + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml b/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml new file mode 100644 index 00000000000..d695bcec7f8 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml @@ -0,0 +1,10 @@ + + + + + nav2_regulated_pure_pursuit_controller + + + + + diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml new file mode 100644 index 00000000000..14c4e1c1d91 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -0,0 +1,31 @@ + + + + nav2_regulated_pure_pursuit_controller + 0.4.7 + Regulated Pure Pursuit Controller + Steve Macenski + Shrijit Singh + Apache-2.0 + + ament_cmake + + nav2_common + nav2_core + nav2_util + nav2_costmap_2d + rclcpp + geometry_msgs + nav2_msgs + pluginlib + tf2 + + ament_cmake_gtest + ament_lint_common + ament_lint_auto + + + ament_cmake + + + 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 new file mode 100644 index 00000000000..b8130f37e7f --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -0,0 +1,563 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 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. + +#include +#include +#include + +#include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" +#include "nav2_core/exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" + +using std::hypot; +using std::min; +using std::max; +using std::abs; +using nav2_util::declare_parameter_if_not_declared; +using nav2_util::geometry_utils::euclidean_distance; +using namespace nav2_costmap_2d; // NOLINT + +namespace nav2_regulated_pure_pursuit_controller +{ + +/** + * Find element in iterator with the minimum calculated value + */ +template +Iter min_by(Iter begin, Iter end, Getter getCompareVal) +{ + if (begin == end) { + return end; + } + auto lowest = getCompareVal(*begin); + Iter lowest_it = begin; + for (Iter it = ++begin; it != end; ++it) { + auto comp = getCompareVal(*it); + if (comp < lowest) { + lowest = comp; + lowest_it = it; + } + } + return lowest_it; +} + +void RegulatedPurePursuitController::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + std::string name, const std::shared_ptr & tf, + const std::shared_ptr & costmap_ros) +{ + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); + tf_ = tf; + plugin_name_ = name; + logger_ = node->get_logger(); + + double transform_tolerance = 0.1; + double control_frequency = 20.0; + + declare_parameter_if_not_declared( + node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_accel", rclcpp::ParameterValue(2.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_decel", rclcpp::ParameterValue(2.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.9)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", + rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_approach_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_allowed_time_to_collision", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue(3.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".goal_dist_tol", rclcpp::ParameterValue(0.25)); + + node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); + node->get_parameter(plugin_name_ + ".max_linear_accel", max_linear_accel_); + node->get_parameter(plugin_name_ + ".max_linear_decel", max_linear_decel_); + node->get_parameter(plugin_name_ + ".lookahead_dist", lookahead_dist_); + node->get_parameter(plugin_name_ + ".min_lookahead_dist", min_lookahead_dist_); + node->get_parameter(plugin_name_ + ".max_lookahead_dist", max_lookahead_dist_); + node->get_parameter(plugin_name_ + ".lookahead_time", lookahead_time_); + node->get_parameter(plugin_name_ + ".rotate_to_heading_angular_vel", rotate_to_heading_angular_vel_); + node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance); + node->get_parameter(plugin_name_ + ".use_velocity_scaled_lookahead_dist", + use_velocity_scaled_lookahead_dist_); + node->get_parameter(plugin_name_ + ".min_approach_linear_velocity", min_approach_linear_velocity_); + node->get_parameter(plugin_name_ + ".use_approach_linear_velocity_scaling", use_approach_vel_scaling_); + node->get_parameter(plugin_name_ + ".max_allowed_time_to_collision", max_allowed_time_to_collision_); + node->get_parameter(plugin_name_ + ".use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_); + node->get_parameter(plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_); + node->get_parameter(plugin_name_ + ".cost_scaling_dist", cost_scaling_dist_); + node->get_parameter(plugin_name_ + ".cost_scaling_gain", cost_scaling_gain_); + node->get_parameter(plugin_name_ + ".inflation_cost_scaling_factor", inflation_cost_scaling_factor_); + node->get_parameter(plugin_name_ + ".regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_); + node->get_parameter(plugin_name_ + ".regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_); + node->get_parameter(plugin_name_ + ".use_rotate_to_heading", use_rotate_to_heading_); + node->get_parameter(plugin_name_ + ".rotate_to_heading_min_angle", rotate_to_heading_min_angle_); + node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_); + node->get_parameter(plugin_name_ + ".goal_dist_tol", goal_dist_tol_); + node->get_parameter("controller_frequency", control_frequency); + + transform_tolerance_ = tf2::durationFromSec(transform_tolerance); + control_duration_ = 1.0 / control_frequency; + + if (inflation_cost_scaling_factor_ <= 0.0) { + RCLCPP_WARN(logger_, "The value inflation_cost_scaling_factor is incorrectly set, " + "it should be >0. Disabling cost regulated linear velocity scaling."); + use_cost_regulated_linear_velocity_scaling_ = false; + } + + global_path_pub_ = node->create_publisher("received_global_plan", 1); + carrot_pub_ = node->create_publisher("lookahead_point", 1); + carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); +} + +void RegulatedPurePursuitController::cleanup() +{ + RCLCPP_INFO( + logger_, + "Cleaning up controller: %s of type" + " regulated_pure_pursuit_controller::RegulatedPurePursuitController", + plugin_name_.c_str()); + global_path_pub_.reset(); + carrot_pub_.reset(); + carrot_arc_pub_.reset(); +} + +void RegulatedPurePursuitController::activate() +{ + RCLCPP_INFO( + logger_, + "Activating controller: %s of type " + "regulated_pure_pursuit_controller::RegulatedPurePursuitController", + plugin_name_.c_str()); + global_path_pub_->on_activate(); + carrot_pub_->on_activate(); + carrot_arc_pub_->on_activate(); +} + +void RegulatedPurePursuitController::deactivate() +{ + RCLCPP_INFO( + logger_, + "Deactivating controller: %s of type " + "regulated_pure_pursuit_controller::RegulatedPurePursuitController", + plugin_name_.c_str()); + global_path_pub_->on_deactivate(); + carrot_pub_->on_deactivate(); + carrot_arc_pub_->on_deactivate(); +} + +std::unique_ptr RegulatedPurePursuitController::createCarrotMsg( + const geometry_msgs::msg::PoseStamped & carrot_pose) +{ + auto carrot_msg = std::make_unique(); + carrot_msg->header = carrot_pose.header; + carrot_msg->point.x = carrot_pose.pose.position.x; + carrot_msg->point.y = carrot_pose.pose.position.y; + carrot_msg->point.z = 0.01; // publish right over map to stand out + return carrot_msg; +} + +double RegulatedPurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twist & speed) +{ + // If using velocity-scaled look ahead distances, find and clamp the dist + // Else, use the static look ahead distance + double lookahead_dist = lookahead_dist_; + if (use_velocity_scaled_lookahead_dist_) { + lookahead_dist = speed.linear.x * lookahead_time_; + lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); + } + + return lookahead_dist; +} + +geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & speed) +{ + // Transform path to robot base frame + auto transformed_plan = transformGlobalPlan(pose); + + // Find look ahead distance and point on path and publish + const double lookahead_dist = getLookAheadDistance(speed); + auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); + carrot_pub_->publish(createCarrotMsg(carrot_pose)); + + double linear_vel, angular_vel; + + // Find distance^2 to look ahead point (carrot) in robot base frame + // This is the chord length of the circle + const double carrot_dist2 = + (carrot_pose.pose.position.x * carrot_pose.pose.position.x) + + (carrot_pose.pose.position.y * carrot_pose.pose.position.y); + + // Find curvature of circle (k = 1 / R) + double curvature = 0.0; + if (carrot_dist2 > 0.001) { + curvature = 2.0 * carrot_pose.pose.position.y / carrot_dist2; + } + + linear_vel = desired_linear_vel_; + + // Make sure we're in compliance with basic constraints + double angle_to_heading; + if (shouldRotateToGoalHeading(carrot_pose)) { + double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); + rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed); + } else if (shouldRotateToPath(carrot_pose, angle_to_heading)) { + rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); + } else { + applyConstraints( + fabs(lookahead_dist - sqrt(carrot_dist2)), + lookahead_dist, curvature, speed, + costAtPose(pose.pose.position.x, pose.pose.position.y), linear_vel); + + // Apply curvature to angular velocity after constraining linear velocity + angular_vel = linear_vel * curvature; + } + + // Collision checking on this velocity heading + if (isCollisionImminent(pose, linear_vel, angular_vel)) { + throw std::runtime_error("RegulatedPurePursuitController detected collision ahead!"); + } + + // populate and return message + geometry_msgs::msg::TwistStamped cmd_vel; + cmd_vel.header = pose.header; + cmd_vel.twist.linear.x = linear_vel; + cmd_vel.twist.angular.z = angular_vel; + return cmd_vel; +} + +bool RegulatedPurePursuitController::shouldRotateToPath( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) +{ + // Whether we should rotate robot to rough path heading + angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x); + return use_rotate_to_heading_ && fabs(angle_to_path) > rotate_to_heading_min_angle_; +} + +bool RegulatedPurePursuitController::shouldRotateToGoalHeading( + const geometry_msgs::msg::PoseStamped & carrot_pose) +{ + // Whether we should rotate robot to goal heading + double dist_to_goal = std::hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); + return use_rotate_to_heading_ && dist_to_goal < goal_dist_tol_; +} + +void RegulatedPurePursuitController::rotateToHeading( + double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed) +{ + // Rotate in place using max angular velocity / acceleration possible + linear_vel = 0.0; + const double sign = angle_to_path > 0.0 ? 1.0 : -1.0; + angular_vel = sign * rotate_to_heading_angular_vel_; + + const double & dt = control_duration_; + const double min_feasible_angular_speed = curr_speed.angular.z - max_angular_accel_ * dt; + const double max_feasible_angular_speed = curr_speed.angular.z + max_angular_accel_ * dt; + angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); +} + +geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( + const double & lookahead_dist, + const nav_msgs::msg::Path & transformed_plan) +{ + // Find the first pose which is at a distance greater than the lookahead distance + auto goal_pose_it = std::find_if( + transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) { + return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist; + }); + + // If the no pose is not far enough, take the last pose + if (goal_pose_it == transformed_plan.poses.end()) { + goal_pose_it = std::prev(transformed_plan.poses.end()); + } + + return *goal_pose_it; +} + +bool RegulatedPurePursuitController::isCollisionImminent( + const geometry_msgs::msg::PoseStamped & robot_pose, + const double & linear_vel, const double & angular_vel) +{ + // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in + // odom frame and the carrot_pose is in robot base frame. + + // check current point is OK + if (inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y)) { + return true; + } + + // visualization messages + nav_msgs::msg::Path arc_pts_msg; + arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); + arc_pts_msg.header.stamp = robot_pose.header.stamp; + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.frame_id = arc_pts_msg.header.frame_id; + pose_msg.header.stamp = arc_pts_msg.header.stamp; + + const double projection_time = costmap_->getResolution() / linear_vel; + + geometry_msgs::msg::Pose2D curr_pose; + curr_pose.x = robot_pose.pose.position.x; + curr_pose.y = robot_pose.pose.position.y; + curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); + + int i = 1; + while (true) { + // only forward simulate within time requested + if (i * projection_time > max_allowed_time_to_collision_) { + break; + } + + i++; + + // apply velocity at curr_pose over distance + curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta)); + curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); + curr_pose.theta += projection_time * angular_vel; + + // store it for visualization + pose_msg.pose.position.x = curr_pose.x; + pose_msg.pose.position.y = curr_pose.y; + pose_msg.pose.position.z = 0.01; + arc_pts_msg.poses.push_back(pose_msg); + + // check for collision at this point + if (inCollision(curr_pose.x, curr_pose.y)) { + carrot_arc_pub_->publish(arc_pts_msg); + return true; + } + } + + carrot_arc_pub_->publish(arc_pts_msg); + + return false; +} + +bool RegulatedPurePursuitController::inCollision(const double & x, const double & y) +{ + unsigned int mx, my; + costmap_->worldToMap(x, y, mx, my); + + unsigned char cost = costmap_->getCost(mx, my); + + if (costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) { + return cost >= INSCRIBED_INFLATED_OBSTACLE && cost != NO_INFORMATION; + } else { + return cost >= INSCRIBED_INFLATED_OBSTACLE; + } +} + +double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) +{ + unsigned int mx, my; + costmap_->worldToMap(x, y, mx, my); + + unsigned char cost = costmap_->getCost(mx, my); + return static_cast(cost); +} + +void RegulatedPurePursuitController::applyConstraints( + const double & dist_error, const double & lookahead_dist, + const double & curvature, const geometry_msgs::msg::Twist & curr_speed, + const double & pose_cost, double & linear_vel) +{ + double curvature_vel = linear_vel; + double cost_vel = linear_vel; + double approach_vel = linear_vel; + + // limit the linear velocity by curvature + const double radius = fabs(1.0 / curvature); + const double & min_rad = regulated_linear_scaling_min_radius_; + if (use_regulated_linear_velocity_scaling_ && radius < min_rad) { + curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); + } + + // limit the linear velocity by proximity to obstacles + if (use_cost_regulated_linear_velocity_scaling_ && + pose_cost != static_cast(NO_INFORMATION) && + pose_cost != static_cast(FREE_SPACE)) + { + const double inscribed_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius(); + const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) * + std::log(pose_cost / (INSCRIBED_INFLATED_OBSTACLE - 1)) + inscribed_radius; + + if (min_distance_to_obstacle < cost_scaling_dist_) { + cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_; + } + } + + // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed + linear_vel = std::min(cost_vel, curvature_vel); + linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); + + // if the actual lookahead distance is shorter than requested, that means we're at the + // end of the path. We'll scale linear velocity by error to slow to a smooth stop + if (use_approach_vel_scaling_ && dist_error > 2.0 * costmap_->getResolution()) { + double velocity_scaling = 1.0 - (dist_error / lookahead_dist); + double unbounded_vel = approach_vel * velocity_scaling; + if (unbounded_vel < min_approach_linear_velocity_) { + approach_vel = min_approach_linear_velocity_; + } else { + approach_vel *= velocity_scaling; + } + + // Use the lowest velocity between approach and other constraints, if all overlapping + linear_vel = std::min(linear_vel, approach_vel); + } + + // Limit linear velocities to be valid and kinematically feasible, v = v0 + a * dt + double & dt = control_duration_; + const double max_feasible_linear_speed = curr_speed.linear.x + max_linear_accel_ * dt; + const double min_feasible_linear_speed = curr_speed.linear.x - max_linear_decel_ * dt; + linear_vel = std::clamp(linear_vel, min_feasible_linear_speed, max_feasible_linear_speed); + linear_vel = std::clamp(linear_vel, 0.0, desired_linear_vel_); +} + +void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) +{ + global_plan_ = path; +} + +nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose) +{ + if (global_plan_.poses.empty()) { + throw nav2_core::PlannerException("Received plan with zero length"); + } + + // let's get the pose of the robot in the frame of the plan + geometry_msgs::msg::PoseStamped robot_pose; + if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) + { + throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame"); + } + + // We'll discard points on the plan that are outside the local costmap + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + const double max_costmap_dim = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()); + const double max_transform_dist = max_costmap_dim * costmap->getResolution() / 2.0; + + // First find the closest pose on the path to the robot + auto transformation_begin = + min_by( + global_plan_.poses.begin(), global_plan_.poses.end(), + [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(robot_pose, ps); + }); + + // Find points definitely outside of the costmap so we won't transform them. + auto transformation_end = std::find_if( + transformation_begin, end(global_plan_.poses), + [&](const auto & global_plan_pose) { + return euclidean_distance(robot_pose, global_plan_pose) > max_transform_dist; + }); + + // Lambda to transform a PoseStamped from global frame to local + auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = global_plan_.header.frame_id; + stamped_pose.header.stamp = robot_pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose); + return transformed_pose; + }; + + // Transform the near part of the global plan into the robot's frame of reference. + nav_msgs::msg::Path transformed_plan; + std::transform( + transformation_begin, transformation_end, + std::back_inserter(transformed_plan.poses), + transformGlobalPoseToLocal); + transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.stamp = robot_pose.header.stamp; + + // Remove the portion of the global plan that we've already passed so we don't + // process it on the next iteration (this is called path pruning) + global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); + global_path_pub_->publish(transformed_plan); + + if (transformed_plan.poses.empty()) { + throw nav2_core::PlannerException("Resulting plan has 0 poses in it."); + } + + return transformed_plan; +} + +bool RegulatedPurePursuitController::transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const +{ + if (in_pose.header.frame_id == frame) { + out_pose = in_pose; + return true; + } + + try { + tf_->transform(in_pose, out_pose, frame, transform_tolerance_); + out_pose.header.frame_id = frame; + return true; + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); + } + return false; +} + +} // namespace nav2_pure_pursuit_controller + +// Register this controller as a nav2_core plugin +PLUGINLIB_EXPORT_CLASS( + nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController, + nav2_core::Controller) diff --git a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt new file mode 100644 index 00000000000..aee6297fcbb --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt @@ -0,0 +1,10 @@ +# tests for regulated PP +ament_add_gtest(test_regulated_pp + test_regulated_pp.cpp +) +ament_target_dependencies(test_regulated_pp + ${dependencies} +) +target_link_libraries(test_regulated_pp + ${library_name} +) diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp new file mode 100644 index 00000000000..9389b43bff0 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -0,0 +1,324 @@ +// Copyright (c) 2021 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. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController +{ +public: + BasicAPIRPP() : nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController() {}; + + nav_msgs::msg::Path getPlan() {return global_plan_;}; + + double getSpeed() {return desired_linear_vel_;}; + + std::unique_ptr createCarrotMsgWrapper( + const geometry_msgs::msg::PoseStamped & carrot_pose) + { + return createCarrotMsg(carrot_pose); + }; + + void setVelocityScaledLookAhead() {use_velocity_scaled_lookahead_dist_ = true;}; + void setCostRegulationScaling() {use_cost_regulated_linear_velocity_scaling_ = true;}; + void resetVelocityRegulationScaling() {use_regulated_linear_velocity_scaling_ = false;}; + void resetVelocityApproachScaling() {use_approach_vel_scaling_ = false;}; + + double getLookAheadDistanceWrapper(const geometry_msgs::msg::Twist & twist) + { + return getLookAheadDistance(twist); + }; + + geometry_msgs::msg::PoseStamped getLookAheadPointWrapper( + const double & dist, const nav_msgs::msg::Path & path) + { + return getLookAheadPoint(dist, path); + }; + + bool shouldRotateToPathWrapper( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) + { + return shouldRotateToPath(carrot_pose, angle_to_path); + } + + bool shouldRotateToGoalHeadingWrapper(const geometry_msgs::msg::PoseStamped & carrot_pose) + { + return shouldRotateToGoalHeading(carrot_pose); + } + + void rotateToHeadingWrapper(double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed) + { + return rotateToHeading(linear_vel, angular_vel, angle_to_path, curr_speed); + } + + void applyConstraintsWrapper( + const double & dist_error, const double & lookahead_dist, + const double & curvature, const geometry_msgs::msg::Twist & curr_speed, + const double & pose_cost, double & linear_vel) + { + return applyConstraints(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + } + +}; + +TEST(RegulatedPurePursuitTest, basicAPI) +{ + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + + // instantiate + auto ctrl = std::make_shared(); + ctrl->configure(node, name, tf, costmap); + ctrl->activate(); + ctrl->deactivate(); + ctrl->cleanup(); + + // setPlan and get plan + nav_msgs::msg::Path path; + path.poses.resize(2); + path.poses[0].header.frame_id = "fake_frame"; + ctrl->setPlan(path); + EXPECT_EQ(ctrl->getPlan().poses.size(), 2ul); + EXPECT_EQ(ctrl->getPlan().poses[0].header.frame_id, std::string("fake_frame")); +} + +TEST(RegulatedPurePursuitTest, createCarrotMsg) +{ + auto ctrl = std::make_shared(); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "Hi!"; + pose.pose.position.x = 1.0; + pose.pose.position.y = 12.0; + pose.pose.orientation.w = 0.5; + + auto rtn = ctrl->createCarrotMsgWrapper(pose); + EXPECT_EQ(rtn->header.frame_id, std::string("Hi!")); + EXPECT_EQ(rtn->point.x, 1.0); + EXPECT_EQ(rtn->point.y, 12.0); + EXPECT_EQ(rtn->point.z, 0.01); +} + +TEST(RegulatedPurePursuitTest, lookaheadAPI) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + ctrl->configure(node, name, tf, costmap); + + geometry_msgs::msg::Twist twist; + + // test getLookAheadDistance + double rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.6); // default lookahead_dist + + // shouldn't be a function of speed + twist.linear.x = 10.0; + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.6); + + // now it should be a function of velocity, max out + ctrl->setVelocityScaledLookAhead(); + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.9); // 10 speed maxes out at max_lookahead_dist + + // check normal range + twist.linear.x = 0.35; + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_NEAR(rtn, 0.525, 0.0001); // 1.5 * 0.35 + + // check minimum range + twist.linear.x = 0.0; + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.3); + + // test getLookAheadPoint + double dist = 1.0; + nav_msgs::msg::Path path; + path.poses.resize(10); + for (uint i = 0; i != path.poses.size(); i++) { + path.poses[i].pose.position.x = static_cast(i); + } + + // test exact hits + auto pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 1.0); + + // test getting next closest point + dist = 3.8; + pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 4.0); + + // test end of path + dist = 100.0; + pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 9.0); +} + +TEST(RegulatedPurePursuitTest, rotateTests) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + ctrl->configure(node, name, tf, costmap); + + // shouldRotateToPath + geometry_msgs::msg::PoseStamped carrot; + double angle_to_path_rtn; + EXPECT_EQ(ctrl->shouldRotateToPathWrapper(carrot, angle_to_path_rtn), false); + + carrot.pose.position.x = 0.5; + carrot.pose.position.y = 0.25; + EXPECT_EQ(ctrl->shouldRotateToPathWrapper(carrot, angle_to_path_rtn), false); + + carrot.pose.position.x = 0.5; + carrot.pose.position.y = 1.0; + EXPECT_EQ(ctrl->shouldRotateToPathWrapper(carrot, angle_to_path_rtn), true); + + // shouldRotateToGoalHeading + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.0; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); + + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.24; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); + + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.26; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), false); + + // rotateToHeading + double lin_v = 10.0; + double ang_v = 0.5; + double angle_to_path = 0.4; + geometry_msgs::msg::Twist curr_speed; + curr_speed.angular.z = 1.75; + + // basic full speed at a speed + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_EQ(lin_v, 0.0); + EXPECT_EQ(ang_v, 1.8); + + // negative direction + angle_to_path = -0.4; + curr_speed.angular.z = -1.75; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_EQ(ang_v, -1.8); + + // kinematic clamping, no speed, some speed accelerating, some speed decelerating + angle_to_path = 0.4; + curr_speed.angular.z = 0.0; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_NEAR(ang_v, 0.16, 0.01); + + curr_speed.angular.z = 1.0; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_NEAR(ang_v, 1.16, 0.01); + + angle_to_path = -0.4; + curr_speed.angular.z = 1.0; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_NEAR(ang_v, 0.84, 0.01); +} + +TEST(RegulatedPurePursuitTest, applyConstraints) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + ctrl->configure(node, name, tf, costmap); + + double dist_error = 0.0; + double lookahead_dist = 0.6; + double curvature = 0.5; + geometry_msgs::msg::Twist curr_speed; + double pose_cost = 0.0; + double linear_vel = 0.0; + + // since costmaps here are bogus, we can't access them + ctrl->resetVelocityApproachScaling(); + + // test curvature regulation (default) + curr_speed.linear.x = 0.25; + ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + EXPECT_EQ(linear_vel, 0.25); // min set speed + + linear_vel = 1.0; + curvature = 0.7407; + curr_speed.linear.x = 0.5; + ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + EXPECT_NEAR(linear_vel, 0.5, 0.01); // lower by curvature + + linear_vel = 1.0; + curvature = 1000.0; + curr_speed.linear.x = 0.25; + ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + EXPECT_NEAR(linear_vel, 0.25, 0.01); // min out by curvature + + + // now try with cost regulation (turn off velocity and only cost) + // ctrl->setCostRegulationScaling(); + // ctrl->resetVelocityRegulationScaling(); + // curvature = 0.0; + + // min changable cost + // pose_cost = 1; + // linear_vel = 0.5; + // curr_speed.linear.x = 0.5; + // ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.498, 0.01); + + // max changing cost + // pose_cost = 127; + // curr_speed.linear.x = 0.255; + // ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.255, 0.01); + + // over max cost thresh + // pose_cost = 200; + // curr_speed.linear.x = 0.25; + // ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.25, 0.01); + + // test kinematic clamping + // pose_cost = 200; + // curr_speed.linear.x = 1.0; + // ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.5, 0.01); +} diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index fabf1e3170a..772759aa47f 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -84,6 +84,9 @@ private Q_SLOTS: // The (non-spinning) client node used to invoke the action client rclcpp::Node::SharedPtr client_node_; + // Timeout value when waiting for action servers to respnd + std::chrono::milliseconds server_timeout_; + // A timer used to check on the completion status of the action QBasicTimer timer_; diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index f8a42e9b37f..388cd4345cf 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 0.3.4 + 0.4.7 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 9d1b3156834..e227489171a 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -34,7 +34,9 @@ using nav2_util::geometry_utils::orientationAroundZAxis; GoalPoseUpdater GoalUpdater; Nav2Panel::Nav2Panel(QWidget * parent) -: Panel(parent), client_nav_("lifecycle_manager_navigation"), +: Panel(parent), + server_timeout_(10), + client_nav_("lifecycle_manager_navigation"), client_loc_("lifecycle_manager_localization") { // Create the control button and its tooltip @@ -115,7 +117,7 @@ Nav2Panel::Nav2Panel(QWidget * parent) // State entered when navigate_to_pose action is not active accumulating_ = new QState(); accumulating_->setObjectName("accumulating"); - accumulating_->assignProperty(start_reset_button_, "text", "Reset"); + accumulating_->assignProperty(start_reset_button_, "text", "Cancel Waypoint Mode"); accumulating_->assignProperty(start_reset_button_, "toolTip", cancel_waypoint_msg); accumulating_->assignProperty(start_reset_button_, "enabled", true); @@ -128,6 +130,18 @@ Nav2Panel::Nav2Panel(QWidget * parent) accumulating_->assignProperty(navigation_mode_button_, "toolTip", waypoint_goal_msg); accumulated_ = new QState(); + accumulated_->setObjectName("accumulated"); + accumulated_->assignProperty(start_reset_button_, "text", "Cancel"); + accumulated_->assignProperty(start_reset_button_, "toolTip", cancel_msg); + accumulated_->assignProperty(start_reset_button_, "enabled", true); + + accumulated_->assignProperty(pause_resume_button_, "text", "Pause"); + accumulated_->assignProperty(pause_resume_button_, "enabled", false); + accumulated_->assignProperty(pause_resume_button_, "toolTip", pause_msg); + + accumulated_->assignProperty(navigation_mode_button_, "text", "Start Navigation"); + accumulated_->assignProperty(navigation_mode_button_, "enabled", false); + accumulated_->assignProperty(navigation_mode_button_, "toolTip", waypoint_goal_msg); // State entered to cancel the navigate_to_pose action canceled_ = new QState(); @@ -183,12 +197,12 @@ Nav2Panel::Nav2Panel(QWidget * parent) idle_->addTransition(navigation_mode_button_, SIGNAL(clicked()), accumulating_); accumulating_->addTransition(navigation_mode_button_, SIGNAL(clicked()), accumulated_); accumulating_->addTransition(start_reset_button_, SIGNAL(clicked()), idle_); + accumulated_->addTransition(start_reset_button_, SIGNAL(clicked()), canceled_); // Internal state transitions canceled_->addTransition(canceled_, SIGNAL(entered()), idle_); reset_->addTransition(reset_, SIGNAL(entered()), initial_); resumed_->addTransition(resumed_, SIGNAL(entered()), idle_); - accumulated_->addTransition(accumulated_, SIGNAL(entered()), idle_); // Pause/Resume button click transitions idle_->addTransition(pause_resume_button_, SIGNAL(clicked()), paused_); @@ -203,6 +217,15 @@ Nav2Panel::Nav2Panel(QWidget * parent) runningTransition->setTargetState(idle_); running_->addTransition(runningTransition); + ROSActionQTransition * idleAccumulatedTransition = + new ROSActionQTransition(QActionState::INACTIVE); + idleAccumulatedTransition->setTargetState(accumulated_); + idle_->addTransition(idleAccumulatedTransition); + + ROSActionQTransition * accumulatedTransition = new ROSActionQTransition(QActionState::ACTIVE); + accumulatedTransition->setTargetState(idle_); + accumulated_->addTransition(accumulatedTransition); + initial_thread_ = new InitialThread(client_nav_, client_loc_); connect(initial_thread_, &InitialThread::finished, initial_thread_, &QObject::deleteLater); @@ -316,12 +339,12 @@ Nav2Panel::onPause() QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::pause, - &client_nav_)); + &client_nav_, std::placeholders::_1), server_timeout_); QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::pause, - &client_loc_)); + &client_loc_, std::placeholders::_1), server_timeout_); } void @@ -331,12 +354,12 @@ Nav2Panel::onResume() QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::resume, - &client_nav_)); + &client_nav_, std::placeholders::_1), server_timeout_); QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::resume, - &client_loc_)); + &client_loc_, std::placeholders::_1), server_timeout_); } void @@ -346,12 +369,12 @@ Nav2Panel::onStartup() QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::startup, - &client_nav_)); + &client_nav_, std::placeholders::_1), server_timeout_); QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::startup, - &client_loc_)); + &client_loc_, std::placeholders::_1), server_timeout_); } void @@ -361,12 +384,12 @@ Nav2Panel::onShutdown() QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::reset, - &client_nav_)); + &client_nav_, std::placeholders::_1), server_timeout_); QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::reset, - &client_loc_)); + &client_loc_, std::placeholders::_1), server_timeout_); timer_.stop(); } @@ -405,21 +428,23 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame) void Nav2Panel::onCancelButtonPressed() { - if (state_machine_.configuration().contains(accumulating_)) { + if (waypoint_follower_goal_handle_) { auto future_cancel = waypoint_follower_action_client_->async_cancel_goal(waypoint_follower_goal_handle_); - if (rclcpp::spin_until_future_complete(client_node_, future_cancel) != - rclcpp::executor::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel waypoint follower"); return; } - } else { + } + + if (navigation_goal_handle_) { auto future_cancel = navigation_action_client_->async_cancel_goal(navigation_goal_handle_); - if (rclcpp::spin_until_future_complete(client_node_, future_cancel) != - rclcpp::executor::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal"); return; @@ -446,7 +471,7 @@ Nav2Panel::onAccumulating() void Nav2Panel::timerEvent(QTimerEvent * event) { - if (state_machine_.configuration().contains(accumulating_)) { + if (state_machine_.configuration().contains(accumulated_)) { if (event->timerId() == timer_.timerId()) { if (!waypoint_follower_goal_handle_) { RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal"); @@ -522,8 +547,8 @@ Nav2Panel::startWaypointFollowing(std::vector p auto future_goal_handle = waypoint_follower_action_client_->async_send_goal(waypoint_follower_goal_, send_goal_options); - if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle) != - rclcpp::executor::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); return; @@ -562,8 +587,8 @@ Nav2Panel::startNavigation(geometry_msgs::msg::PoseStamped pose) auto future_goal_handle = navigation_action_client_->async_send_goal(navigation_goal_, send_goal_options); - if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle) != - rclcpp::executor::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); return; diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 9eae0646f2f..8d67f4a9785 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -7,7 +7,6 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_map_server REQUIRED) -find_package(nav2_bringup REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(visualization_msgs REQUIRED) @@ -29,7 +28,6 @@ set(dependencies rclcpp nav2_util nav2_map_server - nav2_bringup nav2_msgs nav_msgs visualization_msgs @@ -55,6 +53,7 @@ if(BUILD_TESTING) add_subdirectory(src/planning) add_subdirectory(src/localization) add_subdirectory(src/system) + add_subdirectory(src/system_failure) add_subdirectory(src/updown) add_subdirectory(src/waypoint_follower) add_subdirectory(src/recoveries/spin) diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 195973915f7..01a41e39d07 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 0.3.4 + 0.4.7 TODO Carlos Orduno Apache-2.0 @@ -59,6 +59,7 @@ launch launch_ros launch_testing + python3-zmq ament_cmake diff --git a/nav2_system_tests/src/planning/CMakeLists.txt b/nav2_system_tests/src/planning/CMakeLists.txt index 04745b348f5..9c67eadddf7 100644 --- a/nav2_system_tests/src/planning/CMakeLists.txt +++ b/nav2_system_tests/src/planning/CMakeLists.txt @@ -45,3 +45,12 @@ ament_add_test(test_planner_random TEST_EXECUTABLE=$ TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map.pgm ) + +ament_add_gtest(test_planner_plugin_failures + test_planner_plugins.cpp +) + +ament_target_dependencies(test_planner_plugin_failures rclcpp geometry_msgs nav2_msgs ${dependencies}) +target_link_libraries(test_planner_plugin_failures + stdc++fs +) diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index 534b8237301..1367735aa26 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -67,6 +67,12 @@ void PlannerTester::activate() // The navfn wrapper auto state = rclcpp_lifecycle::State(); planner_tester_ = std::make_shared(); + planner_tester_->declare_parameter( + "GridBased.use_astar", rclcpp::ParameterValue(true)); + planner_tester_->set_parameter( + rclcpp::Parameter(std::string("GridBased.use_astar"), rclcpp::ParameterValue(true))); + planner_tester_->set_parameter( + rclcpp::Parameter(std::string("expected_planner_frequency"), rclcpp::ParameterValue(-1.0))); planner_tester_->onConfigure(state); publishRobotTransform(); map_pub_ = this->create_publisher("map", 1); diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp new file mode 100644 index 00000000000..c99c1c36bbd --- /dev/null +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -0,0 +1,71 @@ +// Copyright (c) 2020 Samsung Research +// +// 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 "rclcpp/rclcpp.hpp" +#include "planner_tester.hpp" +#include "nav2_util/lifecycle_utils.hpp" + +using namespace std::chrono_literals; + +using nav2_system_tests::PlannerTester; +using nav2_util::TestCostmap; + +using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped; +using ComputePathToPoseResult = nav_msgs::msg::Path; + +void callback(const nav_msgs::msg::Path::ConstSharedPtr /*grid*/) +{ +} + +TEST(testPluginMap, Failures) +{ + auto obj = std::make_shared(); + rclcpp_lifecycle::State state; + obj->set_parameter(rclcpp::Parameter("expected_planner_frequency", 100000.0)); + obj->onConfigure(state); + obj->create_subscription( + "plan", rclcpp::SystemDefaultsQoS(), callback); + + geometry_msgs::msg::PoseStamped start; + geometry_msgs::msg::PoseStamped goal; + std::string plugin_fake = "fake"; + std::string plugin_none = ""; + auto path = obj->getPlan(start, goal, plugin_none); + EXPECT_EQ(path.header.frame_id, std::string("map")); + + path = obj->getPlan(start, goal, plugin_fake); + EXPECT_EQ(path.poses.size(), 0ul); + + obj->onCleanup(state); +} + +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/recoveries/backup/backup_recovery_tester.cpp b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp index 6d87fb1bd28..290deff0317 100644 --- a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp @@ -131,7 +131,7 @@ bool BackupRecoveryTester::defaultBackupRecoveryTest( auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); return false; @@ -148,7 +148,7 @@ bool BackupRecoveryTester::defaultBackupRecoveryTest( RCLCPP_INFO(node_->get_logger(), "Waiting for result"); if (rclcpp::spin_until_future_complete(node_, result_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); return false; diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp index 521c6759559..ee9f6b63bc2 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp @@ -129,7 +129,7 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); return false; @@ -146,7 +146,7 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( RCLCPP_INFO(node_->get_logger(), "Waiting for result"); if (rclcpp::spin_until_future_complete(node_, result_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); return false; diff --git a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp index e14ca5c7dcf..39ee9871d29 100644 --- a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp +++ b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp @@ -62,11 +62,16 @@ WaitRecoveryTester * WaitRecoveryTestFixture::wait_recovery_tester = nullptr; TEST_P(WaitRecoveryTestFixture, testSWaitRecovery) { float wait_time = std::get<0>(GetParam()); + float cancel = std::get<1>(GetParam()); bool success = false; int num_tries = 3; for (int i = 0; i != num_tries; i++) { - success = success || wait_recovery_tester->recoveryTest(wait_time); + if (cancel == 1.0) { + success = success || wait_recovery_tester->recoveryTestCancel(wait_time); + } else { + success = success || wait_recovery_tester->recoveryTest(wait_time); + } if (success) { break; } @@ -81,7 +86,8 @@ INSTANTIATE_TEST_CASE_P( ::testing::Values( std::make_tuple(1.0, 0.0), std::make_tuple(2.0, 0.0), - std::make_tuple(5.0, 0.0)), + std::make_tuple(5.0, 0.0), + std::make_tuple(10.0, 1.0)), testNameGenerator); int main(int argc, char ** argv) diff --git a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp index b6bdd62148a..7112289b5af 100644 --- a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp @@ -123,7 +123,7 @@ bool WaitRecoveryTester::recoveryTest( auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); return false; @@ -140,7 +140,7 @@ bool WaitRecoveryTester::recoveryTest( RCLCPP_INFO(node_->get_logger(), "Waiting for result"); if (rclcpp::spin_until_future_complete(node_, result_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); return false; @@ -172,6 +172,83 @@ bool WaitRecoveryTester::recoveryTest( return true; } +bool WaitRecoveryTester::recoveryTestCancel( + const float wait_time) +{ + if (!is_active_) { + RCLCPP_ERROR(node_->get_logger(), "Not activated"); + return false; + } + + // Sleep to let recovery server be ready for serving in multiple runs + std::this_thread::sleep_for(5s); + + auto start_time = node_->now(); + auto goal_msg = Wait::Goal(); + goal_msg.time = rclcpp::Duration(wait_time, 0.0); + + RCLCPP_INFO(this->node_->get_logger(), "Sending goal"); + + auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); + + 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_cancel_all_goals(); + + RCLCPP_INFO(node_->get_logger(), "Waiting for cancellation"); + if (rclcpp::spin_until_future_complete(node_, result_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "get cancel result call failed :("); + return false; + } + + auto status = goal_handle_future.get()->get_status(); + + switch (status) { + case rclcpp_action::GoalStatus::STATUS_SUCCEEDED: RCLCPP_ERROR( + node_->get_logger(), + "Goal succeeded"); + return false; + case rclcpp_action::GoalStatus::STATUS_ABORTED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was aborted"); + return false; + case rclcpp_action::GoalStatus::STATUS_CANCELED: RCLCPP_INFO( + node_->get_logger(), + "Goal was canceled"); + return true; + case rclcpp_action::GoalStatus::STATUS_CANCELING: RCLCPP_INFO( + node_->get_logger(), + "Goal is cancelling"); + return true; + case rclcpp_action::GoalStatus::STATUS_EXECUTING: RCLCPP_ERROR( + node_->get_logger(), + "Goal is executing"); + return false; + case rclcpp_action::GoalStatus::STATUS_ACCEPTED: RCLCPP_ERROR( + node_->get_logger(), + "Goal is processing"); + return false; + default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); + return false; + } + + return false; +} + void WaitRecoveryTester::sendInitialPose() { geometry_msgs::msg::PoseWithCovarianceStamped pose; diff --git a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp index 5daef5025b6..4dfb90c6626 100644 --- a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp +++ b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp @@ -52,6 +52,8 @@ class WaitRecoveryTester bool recoveryTest( float time); + bool recoveryTestCancel(float time); + void activate(); void deactivate(); diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index 09017bfc6ca..b176fcddfc3 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -1,3 +1,6 @@ +# NOTE: The ASTAR=True does not work currently due to remapping not functioning +# All set to false, A* testing of NavFn happens in the planning test portion + ament_add_test(test_bt_navigator GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" @@ -10,6 +13,8 @@ ament_add_test(test_bt_navigator GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml ASTAR=True + CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + PLANNER=nav2_navfn_planner/NavfnPlanner ) ament_add_test(test_bt_navigator_with_dijkstra @@ -24,6 +29,25 @@ ament_add_test(test_bt_navigator_with_dijkstra GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml ASTAR=False + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner/NavfnPlanner +) + +ament_add_test(test_bt_navigator_with_groot_monitoring + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml + ASTAR=False + GROOT_MONITORING=True + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner/NavfnPlanner ) ament_add_test(test_dynamic_obstacle @@ -37,7 +61,9 @@ ament_add_test(test_dynamic_obstacle TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_obstacle.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml - ASTAR=True + ASTAR=False + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner/NavfnPlanner ) # ament_add_test(test_multi_robot @@ -52,4 +78,6 @@ ament_add_test(test_dynamic_obstacle # TEST_URDF=${PROJECT_SOURCE_DIR}/urdf/turtlebot3_waffle.urdf # TEST_SDF=${PROJECT_SOURCE_DIR}/models/turtlebot3_waffle/model.sdf # BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml +# CONTROLLER=dwb_core::DWBLocalPlanner +# PLANNER=nav2_navfn_planner/NavfnPlanner # ) diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index dde6618cdc7..8a15b9a63c7 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 # Copyright (c) 2018 Intel Corporation +# Copyright (c) 2020 Florian Gramss # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -22,6 +23,7 @@ from launch import LaunchDescription from launch import LaunchService from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -40,14 +42,30 @@ def generate_launch_description(): 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 - param_substitutions = {'GridBased.use_astar': os.getenv('ASTAR')} + # Replace the default parameter values for testing special features + # without having multiple params_files inside the nav2 stack + context = LaunchContext() + param_substitutions = {} + + if (os.getenv('ASTAR') == 'True'): + param_substitutions.update({'use_astar': 'True'}) + + if (os.getenv('GROOT_MONITORING') == 'True'): + param_substitutions.update({'enable_groot_monitoring': 'True'}) + + param_substitutions.update( + {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')}) + param_substitutions.update( + {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')}) + configured_params = RewrittenYaml( source_file=params_file, root_key='', param_rewrites=param_substitutions, convert_types=True) + new_yaml = configured_params.perform(context) + return LaunchDescription([ SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), @@ -78,7 +96,7 @@ def generate_launch_description(): 'use_namespace': 'False', 'map': map_yaml_file, 'use_sim_time': 'True', - 'params_file': configured_params, + 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/system/tester_node.py b/nav2_system_tests/src/system/tester_node.py index a66e2f7b89d..d84724ea5c8 100755 --- a/nav2_system_tests/src/system/tester_node.py +++ b/nav2_system_tests/src/system/tester_node.py @@ -1,5 +1,6 @@ #! /usr/bin/env python3 # Copyright 2018 Intel Corporation. +# Copyright 2020 Florian Gramss # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -15,8 +16,10 @@ import argparse import math +import os import sys import time + from typing import Optional from action_msgs.msg import GoalStatus @@ -34,6 +37,8 @@ from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy from rclpy.qos import QoSProfile +import zmq + class NavTester(Node): @@ -94,6 +99,13 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): while not self.action_client.wait_for_server(timeout_sec=1.0): self.info_msg("'NavigateToPose' action server not available, waiting...") + if (os.getenv('GROOT_MONITORING') == 'True'): + if self.grootMonitoringGetStatus(): + self.error_msg('Behavior Tree must not be running already!') + self.error_msg('Are you running multiple goals/bts..?') + return False + self.info_msg('This Error above MUST Fail and is o.k.!') + self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose goal_msg = NavigateToPose.Goal() goal_msg.pose = self.getStampedPoseMsg(self.goal_pose) @@ -111,6 +123,19 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): self.info_msg('Goal accepted') get_result_future = goal_handle.get_result_async() + future_return = True + if (os.getenv('GROOT_MONITORING') == 'True'): + try: + if not self.grootMonitoringReloadTree(): + self.error_msg('Failed GROOT_BT - Reload Tree from ZMQ Server') + future_return = False + if not self.grootMonitoringGetStatus(): + self.error_msg('Failed GROOT_BT - Get Status from ZMQ Publisher') + future_return = False + except Exception as e: + self.error_msg('Failed GROOT_BT - ZMQ Tests: ' + e.__doc__ + e.message) + future_return = False + self.info_msg("Waiting for 'NavigateToPose' action to complete") rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status @@ -118,9 +143,81 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): self.info_msg('Goal failed with status code: {0}'.format(status)) return False + if not future_return: + return False + self.info_msg('Goal succeeded!') return True + def grootMonitoringReloadTree(self): + # ZeroMQ Context + context = zmq.Context() + + sock = context.socket(zmq.REQ) + port = 1667 # default server port for groot monitoring + # # Set a Timeout so we do not spin till infinity + sock.setsockopt(zmq.RCVTIMEO, 1000) + # sock.setsockopt(zmq.LINGER, 0) + + sock.connect('tcp://localhost:' + str(port)) + self.info_msg('ZMQ Server Port: ' + str(port)) + + # this should fail + try: + sock.recv() + self.error_msg('ZMQ Reload Tree Test 1/3 - This should have failed!') + # Only works when ZMQ server receives a request first + sock.close() + return False + except zmq.error.ZMQError: + self.info_msg('ZMQ Reload Tree Test 1/3: Check') + try: + # request tree from server + sock.send_string('') + # receive tree from server as flat_buffer + sock.recv() + self.info_msg('ZMQ Reload Tree Test 2/3: Check') + except zmq.error.Again: + self.info_msg('ZMQ Reload Tree Test 2/3 - Failed to load tree') + sock.close() + return False + + # this should fail + try: + sock.recv() + self.error_msg('ZMQ Reload Tree Test 3/3 - This should have failed!') + # Tree should only be loadable ONCE after ZMQ server received a request + sock.close() + return False + except zmq.error.ZMQError: + self.info_msg('ZMQ Reload Tree Test 3/3: Check') + + return True + + def grootMonitoringGetStatus(self): + # ZeroMQ Context + context = zmq.Context() + # Define the socket using the 'Context' + sock = context.socket(zmq.SUB) + # Set a Timeout so we do not spin till infinity + sock.setsockopt(zmq.RCVTIMEO, 2000) + # sock.setsockopt(zmq.LINGER, 0) + + # Define subscription and messages with prefix to accept. + sock.setsockopt_string(zmq.SUBSCRIBE, '') + port = 1666 # default publishing port for groot monitoring + sock.connect('tcp://127.0.0.1:' + str(port)) + + for request in range(3): + try: + sock.recv() + except zmq.error.Again: + self.error_msg('ZMQ - Did not receive any status') + sock.close() + return False + self.info_msg('ZMQ - Did receive status') + return True + def poseCallback(self, msg): self.info_msg('Received amcl_pose') self.current_pose = msg.pose.pose @@ -228,12 +325,8 @@ def run_all_tests(robot_tester): robot_tester.wait_for_node_active('bt_navigator') result = robot_tester.runNavigateAction() - # TODO(orduno) Test sending the navigation request through the topic interface. - # Need to update tester to receive multiple goal poses. - # Need to fix bug with shutting down while bt_navigator - # is still running. - # if (result): - # result = test_RobotMovesToGoal(robot_tester) + if (result): + result = test_RobotMovesToGoal(robot_tester) # Add more tests here if desired diff --git a/nav2_system_tests/src/system_failure/CMakeLists.txt b/nav2_system_tests/src/system_failure/CMakeLists.txt new file mode 100644 index 00000000000..885234adc21 --- /dev/null +++ b/nav2_system_tests/src/system_failure/CMakeLists.txt @@ -0,0 +1,12 @@ +ament_add_test(test_failure_navigator + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_failure_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/system_failure/README.md b/nav2_system_tests/src/system_failure/README.md new file mode 100644 index 00000000000..4afd4eaac34 --- /dev/null +++ b/nav2_system_tests/src/system_failure/README.md @@ -0,0 +1,3 @@ +# Navigation2 System Tests - Failure + +High level system failures tests diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py new file mode 100755 index 00000000000..c41e78f3048 --- /dev/null +++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# Copyright (c) 2020 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 + param_substitutions = { + 'planner_server.ros__parameters.GridBased.use_astar': 'False'} + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True) + + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '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={'namespace': '', + 'use_namespace': 'False', + '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() + + test1_action = ExecuteProcess( + cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), + '-r', '-2.0', '-0.5', '100.0', '100.0'], + name='tester_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/system_failure/tester_node.py b/nav2_system_tests/src/system_failure/tester_node.py new file mode 100755 index 00000000000..eac29bc6540 --- /dev/null +++ b/nav2_system_tests/src/system_failure/tester_node.py @@ -0,0 +1,307 @@ +#! /usr/bin/env python3 +# Copyright 2018 Intel Corporation. +# Copyright 2020 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 argparse +import math +import sys +import time +from typing import Optional + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseWithCovarianceStamped +from lifecycle_msgs.srv import GetState +from nav2_msgs.action import NavigateToPose +from nav2_msgs.srv import ManageLifecycleNodes + +import rclpy + +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy +from rclpy.qos import QoSProfile + + +class NavTester(Node): + + def __init__( + self, + initial_pose: Pose, + goal_pose: Pose, + namespace: str = '' + ): + super().__init__(node_name='nav2_tester', namespace=namespace) + self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, + 'initialpose', 10) + self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10) + + pose_qos = QoSProfile( + durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE, + history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, + depth=1) + + self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, + 'amcl_pose', self.poseCallback, pose_qos) + self.initial_pose_received = False + self.initial_pose = initial_pose + self.goal_pose = goal_pose + self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') + + def info_msg(self, msg: str): + self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') + + def warn_msg(self, msg: str): + self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m') + + def error_msg(self, msg: str): + self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') + + def setInitialPose(self): + msg = PoseWithCovarianceStamped() + msg.pose.pose = self.initial_pose + msg.header.frame_id = 'map' + self.info_msg('Publishing Initial Pose') + self.initial_pose_pub.publish(msg) + self.currentPose = self.initial_pose + + def getStampedPoseMsg(self, pose: Pose): + msg = PoseStamped() + msg.header.frame_id = 'map' + msg.pose = pose + return msg + + def publishGoalPose(self, goal_pose: Optional[Pose] = None): + self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose + self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose)) + + def runNavigateAction(self, goal_pose: Optional[Pose] = None): + # Sends a `NavToPose` action request and waits for completion + self.info_msg("Waiting for 'NavigateToPose' action server") + while not self.action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'NavigateToPose' action server not available, waiting...") + + self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose + goal_msg = NavigateToPose.Goal() + goal_msg.pose = self.getStampedPoseMsg(self.goal_pose) + + self.info_msg('Sending goal request...') + send_goal_future = self.action_client.send_goal_async(goal_msg) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + get_result_future = goal_handle.get_result_async() + + self.info_msg("Waiting for 'NavigateToPose' action to complete") + rclpy.spin_until_future_complete(self, get_result_future) + status = get_result_future.result().status + if status != GoalStatus.STATUS_ABORTED: + self.info_msg('Goal failed with status code: {0}'.format(status)) + return False + + self.info_msg('Goal failed, as expected!') + return True + + def poseCallback(self, msg): + self.info_msg('Received amcl_pose') + self.current_pose = msg.pose.pose + self.initial_pose_received = True + + def reachesGoal(self, timeout, distance): + goalReached = False + start_time = time.time() + + while not goalReached: + rclpy.spin_once(self, timeout_sec=1) + if self.distanceFromGoal() < distance: + goalReached = True + self.info_msg('*** GOAL REACHED ***') + return True + elif timeout is not None: + if (time.time() - start_time) > timeout: + self.error_msg('Robot timed out reaching its goal!') + return False + + def distanceFromGoal(self): + d_x = self.current_pose.position.x - self.goal_pose.position.x + d_y = self.current_pose.position.y - self.goal_pose.position.y + distance = math.sqrt(d_x * d_x + d_y * d_y) + self.info_msg('Distance from goal is: ' + str(distance)) + return distance + + def wait_for_node_active(self, node_name: str): + # Waits for the node within the tester namespace to become active + self.info_msg('Waiting for ' + node_name + ' to become active') + node_service = node_name + '/get_state' + state_client = self.create_client(GetState, node_service) + while not state_client.wait_for_service(timeout_sec=1.0): + self.info_msg(node_service + ' service not available, waiting...') + req = GetState.Request() # empty request + state = 'UNKNOWN' + while (state != 'active'): + self.info_msg('Getting ' + node_name + ' state...') + future = state_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + state = future.result().current_state.label + self.info_msg('Result of get_state: %s' % state) + else: + self.error_msg('Exception while calling service: %r' % future.exception()) + time.sleep(5) + + def shutdown(self): + self.info_msg('Shutting down') + self.action_client.destroy() + + transition_service = 'lifecycle_manager_navigation/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(transition_service + ' service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down navigation lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg('Shutting down navigation lifecycle manager complete.') + except Exception as e: + self.error_msg('Service call failed %r' % (e,)) + transition_service = 'lifecycle_manager_localization/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(transition_service + ' service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down localization lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg('Shutting down localization lifecycle manager complete') + except Exception as e: + self.error_msg('Service call failed %r' % (e,)) + + def wait_for_initial_pose(self): + self.initial_pose_received = False + while not self.initial_pose_received: + self.info_msg('Setting initial pose') + self.setInitialPose() + self.info_msg('Waiting for amcl_pose to be received') + rclpy.spin_once(self, timeout_sec=1) + + +def run_all_tests(robot_tester): + # set transforms to use_sim_time + result = True + if (result): + robot_tester.wait_for_node_active('amcl') + robot_tester.wait_for_initial_pose() + robot_tester.wait_for_node_active('bt_navigator') + result = robot_tester.runNavigateAction() + + # Add more tests here if desired + + if (result): + robot_tester.info_msg('Test PASSED') + else: + robot_tester.error_msg('Test FAILED') + + return result + + +def fwd_pose(x=0.0, y=0.0, z=0.01): + initial_pose = Pose() + initial_pose.position.x = x + initial_pose.position.y = y + initial_pose.position.z = z + initial_pose.orientation.x = 0.0 + initial_pose.orientation.y = 0.0 + initial_pose.orientation.z = 0.0 + initial_pose.orientation.w = 1.0 + return initial_pose + + +def get_testers(args): + testers = [] + + if args.robot: + # Requested tester for one robot + init_x, init_y, final_x, final_y = args.robot[0] + tester = NavTester( + initial_pose=fwd_pose(float(init_x), float(init_y)), + goal_pose=fwd_pose(float(final_x), float(final_y))) + tester.info_msg( + 'Starting tester, robot going from ' + init_x + ', ' + init_y + + ' to ' + final_x + ', ' + final_y + '.') + testers.append(tester) + return testers + + return testers + + +def main(argv=sys.argv[1:]): + # The robot(s) positions from the input arguments + parser = argparse.ArgumentParser(description='System-level navigation tester node') + group = parser.add_mutually_exclusive_group(required=True) + group.add_argument('-r', '--robot', action='append', nargs=4, + metavar=('init_x', 'init_y', 'final_x', 'final_y'), + help='The robot starting and final positions.') + group.add_argument('-rs', '--robots', action='append', nargs=5, + metavar=('name', 'init_x', 'init_y', 'final_x', 'final_y'), + help="The robot's namespace and starting and final positions. " + + 'Repeating the argument for multiple robots is supported.') + + args, unknown = parser.parse_known_args() + + rclpy.init() + + # Create testers for each robot + testers = get_testers(args) + + # wait a few seconds to make sure entire stacks are up + time.sleep(10) + + for tester in testers: + passed = run_all_tests(tester) + if not passed: + break + + for tester in testers: + # stop and shutdown the nav stack to exit cleanly + tester.shutdown() + + testers[0].info_msg('Done Shutting Down.') + + if not passed: + testers[0].info_msg('Exiting failed') + exit(1) + else: + testers[0].info_msg('Exiting passed') + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_system_tests/src/updown/CMakeLists.txt b/nav2_system_tests/src/updown/CMakeLists.txt index a56bff34b23..273e1828ea5 100644 --- a/nav2_system_tests/src/updown/CMakeLists.txt +++ b/nav2_system_tests/src/updown/CMakeLists.txt @@ -6,6 +6,6 @@ ament_target_dependencies(test_updown ${dependencies} ) -install(TARGETS test_updown RUNTIME DESTINATION share/${PROJECT_NAME}) +install(TARGETS test_updown RUNTIME DESTINATION lib/${PROJECT_NAME}) install(FILES test_updown_launch.py DESTINATION share/${PROJECT_NAME}) diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 31bd9effbf2..7f5db902fba 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -37,6 +37,7 @@ def __init__(self): self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, 'initialpose', 10) self.initial_pose_received = False + self.goal_handle = None pose_qos = QoSProfile( durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, @@ -71,7 +72,7 @@ def setWaypoints(self, waypoints): msg.pose.orientation.w = 1.0 self.waypoints.append(msg) - def run(self): + def run(self, block): if not self.waypoints: rclpy.error_msg('Did not set valid waypoints before running test!') return False @@ -86,16 +87,19 @@ def run(self): send_goal_future = self.action_client.send_goal_async(action_request) try: rclpy.spin_until_future_complete(self, send_goal_future) - goal_handle = send_goal_future.result() + self.goal_handle = send_goal_future.result() except Exception as e: self.error_msg('Service call failed %r' % (e,)) - if not goal_handle.accepted: + if not self.goal_handle.accepted: self.error_msg('Goal rejected') return False self.info_msg('Goal accepted') - get_result_future = goal_handle.get_result_async() + if not block: + return True + + get_result_future = self.goal_handle.get_result_async() self.info_msg("Waiting for 'FollowWaypoints' action to complete") try: @@ -148,14 +152,18 @@ def shutdown(self): except Exception as e: self.error_msg('Service call failed %r' % (e,)) + def cancel_goal(self): + cancel_future = self.goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_future) + def info_msg(self, msg: str): - self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') + self.get_logger().info(msg) def warn_msg(self, msg: str): - self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m') + self.get_logger().warn(msg) def error_msg(self, msg: str): - self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') + self.get_logger().error(msg) def main(argv=sys.argv[1:]): @@ -179,7 +187,27 @@ def main(argv=sys.argv[1:]): test.info_msg('Waiting for amcl_pose to be received') rclpy.spin_once(test, timeout_sec=1.0) # wait for poseCallback - result = test.run() + result = test.run(True) + assert result + + # preempt with new point + test.setWaypoints([starting_pose]) + result = test.run(False) + time.sleep(2) + test.setWaypoints([wps[1]]) + result = test.run(False) + + # cancel + time.sleep(2) + test.cancel_goal() + + # a failure case + time.sleep(2) + test.setWaypoints([[100.0, 100.0]]) + result = test.run(True) + assert not result + result = not result + test.shutdown() test.info_msg('Done Shutting Down.') diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index e97dc6a86a2..de543515a73 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -121,6 +121,14 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode rclcpp_lifecycle::LifecycleNode::shared_from_this()); } + nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & /*state*/) + { + RCLCPP_FATAL( + get_logger(), + "Lifecycle node %s does not have error state implemented", get_name()); + return nav2_util::CallbackReturn::SUCCESS; + } + protected: void print_lifecycle_node_notification(); diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 26f0eedf03d..e1eb6983817 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -70,7 +70,7 @@ class ServiceClient auto future_result = client_->async_send_request(request); if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { throw std::runtime_error(service_name_ + " service client: async_send_request failed"); } @@ -98,7 +98,7 @@ class ServiceClient auto future_result = client_->async_send_request(request); if (rclcpp::spin_until_future_complete(node_, future_result) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { return false; } diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 48f11c8a5f6..9a645cbd706 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 0.3.4 + 0.4.7 TODO Michael Jeronimo Mohammad Haghighipanah @@ -24,13 +24,19 @@ rclcpp_action test_msgs rclcpp_lifecycle + launch + launch_testing_ament_cmake + action_msgs libboost-program-options ament_lint_common ament_lint_auto ament_cmake_gtest + launch + launch_testing_ament_cmake std_srvs + action_msgs ament_cmake diff --git a/nav2_util/src/dump_params.cpp b/nav2_util/src/dump_params.cpp index a29a21c37b0..259479f1f60 100644 --- a/nav2_util/src/dump_params.cpp +++ b/nav2_util/src/dump_params.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 8d6ee2c0fd4..4d3b5511dc8 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -41,9 +41,37 @@ ament_add_gtest(test_robot_utils test_robot_utils.cpp) ament_target_dependencies(test_robot_utils geometry_msgs) target_link_libraries(test_robot_utils ${library_name}) +# This test is disabled due to failing services +# https://github.com/ros-planning/navigation2/issues/1836 + +add_launch_test( + "test_dump_params/test_dump_params_default.test.py" + TARGET "test_dump_params_default" + TIMEOUT 10 + ENV + TEST_EXECUTABLE=$ +) + +add_launch_test( + "test_dump_params/test_dump_params_yaml.test.py" + TARGET "test_dump_params_yaml" + TIMEOUT 10 + ENV + TEST_EXECUTABLE=$ +) + +add_launch_test( + "test_dump_params/test_dump_params_md.test.py" + TARGET "test_dump_params_md" + TIMEOUT 10 + ENV + TEST_EXECUTABLE=$ +) + add_launch_test( - "test_dump_params/test_dump_params.launch.py" - TIMEOUT 30 + "test_dump_params/test_dump_params_multiple.test.py" + TARGET "test_dump_params_multiple" + TIMEOUT 10 ENV TEST_EXECUTABLE=$ ) diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index 11d5c54f118..84cef8576c1 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -245,7 +245,7 @@ TEST_F(ActionTest, test_simple_action) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); auto goal_handle = future_goal_handle.get(); @@ -253,7 +253,7 @@ TEST_F(ActionTest, test_simple_action) auto future_result = node_->action_client_->async_get_result(goal_handle); EXPECT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // The final result rclcpp_action::ClientGoalHandle::WrappedResult result = future_result.get(); @@ -293,7 +293,7 @@ TEST_F(ActionTest, test_simple_action_with_feedback) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); auto goal_handle = future_goal_handle.get(); @@ -302,7 +302,7 @@ TEST_F(ActionTest, test_simple_action_with_feedback) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_result), rclcpp::executor::FutureReturnCode::SUCCESS); + future_result), rclcpp::FutureReturnCode::SUCCESS); // The final result rclcpp_action::ClientGoalHandle::WrappedResult result = future_result.get(); @@ -334,7 +334,7 @@ TEST_F(ActionTest, test_simple_action_activation_cycling) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); // Deactivate while running node_->deactivate_server(); @@ -345,7 +345,7 @@ TEST_F(ActionTest, test_simple_action_activation_cycling) auto future_result = node_->action_client_->async_get_result(goal_handle); EXPECT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // The action should be reported as aborted. EXPECT_EQ(future_result.get().code, rclcpp_action::ResultCode::ABORTED); @@ -361,7 +361,7 @@ TEST_F(ActionTest, test_simple_action_activation_cycling) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); goal_handle = future_goal_handle.get(); @@ -370,7 +370,7 @@ TEST_F(ActionTest, test_simple_action_activation_cycling) std::cout << "Getting result, spinning til complete..." << std::endl; EXPECT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // Now the action should have been successfully executed. EXPECT_EQ(future_result.get().code, rclcpp_action::ResultCode::SUCCEEDED); @@ -391,7 +391,7 @@ TEST_F(ActionTest, test_simple_action_preemption) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); // Preempt the goal auto preemption_goal = Fibonacci::Goal(); @@ -403,7 +403,7 @@ TEST_F(ActionTest, test_simple_action_preemption) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); auto goal_handle = future_goal_handle.get(); @@ -412,7 +412,7 @@ TEST_F(ActionTest, test_simple_action_preemption) std::cout << "Getting result, spinning til complete..." << std::endl; EXPECT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // The final result rclcpp_action::ClientGoalHandle::WrappedResult result = future_result.get(); @@ -442,7 +442,7 @@ TEST_F(ActionTest, test_simple_action_preemption_after_succeeded) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); node_->omit_server_preemptions(); @@ -450,7 +450,7 @@ TEST_F(ActionTest, test_simple_action_preemption_after_succeeded) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); // Get the results auto goal_handle = future_goal_handle.get(); @@ -459,7 +459,7 @@ TEST_F(ActionTest, test_simple_action_preemption_after_succeeded) auto future_result = node_->action_client_->async_get_result(goal_handle); EXPECT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // The final result rclcpp_action::ClientGoalHandle::WrappedResult result = future_result.get(); @@ -480,7 +480,7 @@ TEST_F(ActionTest, test_simple_action_preemption_after_succeeded) future_result = node_->action_client_->async_get_result(goal_handle); ASSERT_EQ( rclcpp::spin_until_future_complete(node_, future_result), - rclcpp::executor::FutureReturnCode::SUCCESS); + rclcpp::FutureReturnCode::SUCCESS); // The final result result = future_result.get(); @@ -507,13 +507,38 @@ TEST_F(ActionTest, test_handle_goal_deactivated) EXPECT_EQ( rclcpp::spin_until_future_complete( node_, - future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); node_->activate_server(); SUCCEED(); } +TEST_F(ActionTest, test_handle_cancel) +{ + auto goal = Fibonacci::Goal(); + goal.order = 14000000; + + // Send the goal + auto future_goal_handle = node_->action_client_->async_send_goal(goal); + EXPECT_EQ( + rclcpp::spin_until_future_complete( + node_, + future_goal_handle), rclcpp::FutureReturnCode::SUCCESS); + + // Cancel the goal + auto cancel_response = node_->action_client_->async_cancel_goal(future_goal_handle.get()); + EXPECT_EQ( + rclcpp::spin_until_future_complete( + node_, + cancel_response), rclcpp::FutureReturnCode::SUCCESS); + + // Check cancelled + EXPECT_EQ(future_goal_handle.get()->get_status(), rclcpp_action::GoalStatus::STATUS_CANCELING); + + SUCCEED(); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/nav2_util/test/test_dump_params/test_dump_params.launch.py b/nav2_util/test/test_dump_params/test_dump_params_default.test.py similarity index 68% rename from nav2_util/test/test_dump_params/test_dump_params.launch.py rename to nav2_util/test/test_dump_params/test_dump_params_default.test.py index 55d28b5fac4..e0849b398fb 100644 --- a/nav2_util/test/test_dump_params/test_dump_params.launch.py +++ b/nav2_util/test/test_dump_params/test_dump_params_default.test.py @@ -37,12 +37,6 @@ def generate_test_description(): cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')], name='test_dump_params') ) - launch_description.add_action( - ExecuteProcess( - cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py'), - 'test_dump_params_copy'], - name='test_dump_params_copy') - ) processes_to_test = [ ExecuteProcess( cmd=[os.getenv('TEST_EXECUTABLE'), '-h'], @@ -52,30 +46,6 @@ def generate_test_description(): cmd=[os.getenv('TEST_EXECUTABLE')], name='test_dump_params_default', output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params'], - name='test_dump_params_yaml', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params'], - name='test_dump_params_markdown', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params', '-v'], - name='test_dump_params_yaml_verbose', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params', '-v'], - name='test_dump_params_markdown_verbose', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'error', '-n', 'test_dump_params'], - name='test_dump_params_bad_format', - output='screen'), - ExecuteProcess( - cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params,test_dump_params_copy'], - name='test_dump_params_multiple', - output='screen'), ExecuteProcess( cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params_error'], name='test_dump_params_error', @@ -114,12 +84,6 @@ def test_processes_output(self, proc_output, processes_to_test): os.path.join(os.path.dirname(__file__), out) for out in ['dump_params_help', 'dump_params_default', - 'dump_params_yaml', - 'dump_params_md', - 'dump_params_yaml_verbose', - 'dump_params_md_verbose', - 'dump_params_yaml', - 'dump_params_multiple', 'dump_params_error'] ] for process, output_file in zip(processes_to_test[:-1], output_files[:-1]): diff --git a/nav2_util/test/test_dump_params/test_dump_params_md.test.py b/nav2_util/test/test_dump_params/test_dump_params_md.test.py new file mode 100644 index 00000000000..61a85c514cd --- /dev/null +++ b/nav2_util/test/test_dump_params/test_dump_params_md.test.py @@ -0,0 +1,90 @@ +#! /usr/bin/env python3 +# Copyright (c) 2020 Sarthak Mittal +# +# 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 unittest + +from launch import LaunchDescription +from launch.actions import ExecuteProcess + +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.util +import launch_testing_ros + +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + launch_description = LaunchDescription() + launch_description.add_action( + ExecuteProcess( + cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')], + name='test_dump_params') + ) + processes_to_test = [ + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params'], + name='test_dump_params_markdown', + output='screen'), + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'md', '-n', 'test_dump_params', '-v'], + name='test_dump_params_markdown_verbose', + output='screen') + ] + for process in processes_to_test: + launch_description.add_action(process) + launch_description.add_action( + launch_testing.actions.ReadyToTest() + ) + return launch_description, {'processes_to_test': processes_to_test} + + +# Tests without a unittest to run concurrently with +# the processes under test throw an exception +# The following is a dummy test to suppress the traceback +# https://github.com/ros2/launch/issues/380 + +class TestLoggingOutputFormat(unittest.TestCase): + + def test_logging_output(self, proc_info, processes_to_test): + for process_name in processes_to_test: + proc_info.assertWaitForShutdown(process=process_name, timeout=10) + + +@launch_testing.post_shutdown_test() +class TestDumpParams(unittest.TestCase): + + def test_processes_output(self, proc_output, processes_to_test): + """Test all processes output against expectations.""" + from launch_testing.tools.output import get_default_filtered_prefixes + output_filter = launch_testing_ros.tools.basic_output_filter( + filtered_prefixes=get_default_filtered_prefixes() + ) + output_files = [ + os.path.join(os.path.dirname(__file__), out) + for out in ['dump_params_md', + 'dump_params_md_verbose'] + ] + for process, output_file in zip(processes_to_test, output_files): + launch_testing.asserts.assertInStdout( + proc_output, + expected_output=launch_testing.tools.expected_output_from_file( + path=output_file), + process=process, output_filter=output_filter + ) diff --git a/nav2_util/test/test_dump_params/test_dump_params_multiple.test.py b/nav2_util/test/test_dump_params/test_dump_params_multiple.test.py new file mode 100644 index 00000000000..4733c19f584 --- /dev/null +++ b/nav2_util/test/test_dump_params/test_dump_params_multiple.test.py @@ -0,0 +1,96 @@ +#! /usr/bin/env python3 +# Copyright (c) 2020 Sarthak Mittal +# +# 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 unittest + +from launch import LaunchDescription +from launch.actions import ExecuteProcess + +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.util +import launch_testing_ros + +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + launch_description = LaunchDescription() + launch_description.add_action( + ExecuteProcess( + cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')], + name='test_dump_params') + ) + launch_description.add_action( + ExecuteProcess( + cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py'), + 'test_dump_params_copy'], + name='test_dump_params_copy') + ) + processes_to_test = [ + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-f', 'error', '-n', 'test_dump_params'], + name='test_dump_params_bad_format', + output='screen'), + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params,test_dump_params_copy'], + name='test_dump_params_multiple', + output='screen') + ] + for process in processes_to_test: + launch_description.add_action(process) + launch_description.add_action( + launch_testing.actions.ReadyToTest() + ) + return launch_description, {'processes_to_test': processes_to_test} + + +# Tests without a unittest to run concurrently with +# the processes under test throw an exception +# The following is a dummy test to suppress the traceback +# https://github.com/ros2/launch/issues/380 + +class TestLoggingOutputFormat(unittest.TestCase): + + def test_logging_output(self, proc_info, processes_to_test): + for process_name in processes_to_test: + proc_info.assertWaitForShutdown(process=process_name, timeout=10) + + +@launch_testing.post_shutdown_test() +class TestDumpParams(unittest.TestCase): + + def test_processes_output(self, proc_output, processes_to_test): + """Test all processes output against expectations.""" + from launch_testing.tools.output import get_default_filtered_prefixes + output_filter = launch_testing_ros.tools.basic_output_filter( + filtered_prefixes=get_default_filtered_prefixes() + ) + output_files = [ + os.path.join(os.path.dirname(__file__), out) + for out in ['dump_params_yaml', + 'dump_params_multiple'] + ] + for process, output_file in zip(processes_to_test[:-1], output_files[:-1]): + launch_testing.asserts.assertInStdout( + proc_output, + expected_output=launch_testing.tools.expected_output_from_file( + path=output_file), + process=process, output_filter=output_filter + ) diff --git a/nav2_util/test/test_dump_params/test_dump_params_yaml.test.py b/nav2_util/test/test_dump_params/test_dump_params_yaml.test.py new file mode 100644 index 00000000000..545bf4934cf --- /dev/null +++ b/nav2_util/test/test_dump_params/test_dump_params_yaml.test.py @@ -0,0 +1,90 @@ +#! /usr/bin/env python3 +# Copyright (c) 2020 Sarthak Mittal +# +# 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 unittest + +from launch import LaunchDescription +from launch.actions import ExecuteProcess + +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.util +import launch_testing_ros + +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + launch_description = LaunchDescription() + launch_description.add_action( + ExecuteProcess( + cmd=[os.path.join(os.path.dirname(__file__), 'test_dump_params_node.py')], + name='test_dump_params') + ) + processes_to_test = [ + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params'], + name='test_dump_params_yaml', + output='screen'), + ExecuteProcess( + cmd=[os.getenv('TEST_EXECUTABLE'), '-n', 'test_dump_params', '-v'], + name='test_dump_params_yaml_verbose', + output='screen') + ] + for process in processes_to_test: + launch_description.add_action(process) + launch_description.add_action( + launch_testing.actions.ReadyToTest() + ) + return launch_description, {'processes_to_test': processes_to_test} + + +# Tests without a unittest to run concurrently with +# the processes under test throw an exception +# The following is a dummy test to suppress the traceback +# https://github.com/ros2/launch/issues/380 + +class TestLoggingOutputFormat(unittest.TestCase): + + def test_logging_output(self, proc_info, processes_to_test): + for process_name in processes_to_test: + proc_info.assertWaitForShutdown(process=process_name, timeout=10) + + +@launch_testing.post_shutdown_test() +class TestDumpParams(unittest.TestCase): + + def test_processes_output(self, proc_output, processes_to_test): + """Test all processes output against expectations.""" + from launch_testing.tools.output import get_default_filtered_prefixes + output_filter = launch_testing_ros.tools.basic_output_filter( + filtered_prefixes=get_default_filtered_prefixes() + ) + output_files = [ + os.path.join(os.path.dirname(__file__), out) + for out in ['dump_params_yaml', + 'dump_params_yaml_verbose'] + ] + for process, output_file in zip(processes_to_test, output_files): + launch_testing.asserts.assertInStdout( + proc_output, + expected_output=launch_testing.tools.expected_output_from_file( + path=output_file), + process=process, output_filter=output_filter + ) diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 7cc1862d4c8..7eaccca6e41 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 0.3.4 + 0.4.7 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_voxel_grid/test/voxel_grid_tests.cpp b/nav2_voxel_grid/test/voxel_grid_tests.cpp index 6ff58812ff8..cfc596f6ae2 100644 --- a/nav2_voxel_grid/test/voxel_grid_tests.cpp +++ b/nav2_voxel_grid/test/voxel_grid_tests.cpp @@ -147,6 +147,46 @@ TEST(voxel_grid, InvalidSize) { EXPECT_TRUE(vg.getVoxelColumn(50, 11, 0, 0) == nav2_voxel_grid::VoxelStatus::UNKNOWN); } +TEST(voxel_grid, MarkAndClear) { + int size_x = 10, size_y = 10, size_z = 10; + nav2_voxel_grid::VoxelGrid vg(size_x, size_y, size_z); + vg.markVoxelInMap(5, 5, 5, 0); + EXPECT_EQ(vg.getVoxel(5, 5, 5), nav2_voxel_grid::MARKED); + vg.clearVoxelColumn(55); + EXPECT_EQ(vg.getVoxel(5, 5, 5), nav2_voxel_grid::FREE); +} + +TEST(voxel_grid, clearVoxelLineInMap) { + int size_x = 10, size_y = 10, size_z = 10; + nav2_voxel_grid::VoxelGrid vg(size_x, size_y, size_z); + vg.markVoxelInMap(0, 0, 5, 0); + EXPECT_EQ(vg.getVoxel(0, 0, 5), nav2_voxel_grid::MARKED); + + unsigned char * map_2d = new unsigned char[100]; + map_2d[0] = 254; + + vg.clearVoxelLineInMap(0, 0, 0, 0, 0, 9, map_2d, 16, 0); + + EXPECT_EQ(map_2d[0], 0); + + vg.markVoxelInMap(0, 0, 5, 0); + vg.clearVoxelLineInMap(0, 0, 0, 0, 0, 9, nullptr, 16, 0); + EXPECT_EQ(vg.getVoxel(0, 0, 5), nav2_voxel_grid::FREE); + delete[] map_2d; +} + +TEST(voxel_grid, GetVoxelData) { + uint32_t * data = new uint32_t[9]; + data[4] = 255; + data[0] = 0; + EXPECT_EQ( + nav2_voxel_grid::VoxelGrid::getVoxel(1, 1, 1, 3, 3, 3, data), nav2_voxel_grid::UNKNOWN); + + EXPECT_EQ( + nav2_voxel_grid::VoxelGrid::getVoxel(0, 0, 0, 3, 3, 3, data), nav2_voxel_grid::FREE); + delete[] data; +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 98d85f932b2..83ffa14f69e 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -92,11 +92,6 @@ class WaypointFollower : public nav2_util::LifecycleNode * @return SUCCESS or FAILURE */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - /** - * @brief Called when in error state - * @param state Reference to LifeCycle node state - */ - nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; /** * @brief Action server callbacks @@ -120,6 +115,7 @@ class WaypointFollower : public nav2_util::LifecycleNode std::unique_ptr action_server_; ActionClient::SharedPtr nav_to_pose_client_; rclcpp::Node::SharedPtr client_node_; + std::shared_future::SharedPtr> future_goal_handle_; bool stop_on_failure_; ActionStatus current_goal_status_; int loop_rate_; diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 95dbfbe1951..a8aa4bb9351 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,13 +2,11 @@ nav2_waypoint_follower - 0.3.4 + 0.4.7 A waypoint follower navigation server Steve Macenski Apache-2.0 - tf2_ros - ament_cmake nav2_common rclcpp @@ -17,6 +15,7 @@ nav_msgs nav2_msgs nav2_util + tf2_ros ament_lint_common ament_lint_auto diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 3a0edabce6e..a86154d9bdd 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -19,8 +19,7 @@ #include #include #include - -// TODO(stevemacenski): Add capability for reading in yaml file and executing +#include namespace nav2_waypoint_follower { @@ -47,9 +46,13 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); loop_rate_ = get_parameter("loop_rate").as_int(); - // use suffix '_rclcpp_node' to keep parameter file consistency #1773 + std::vector new_args = rclcpp::NodeOptions().arguments(); + new_args.push_back("--ros-args"); + new_args.push_back("-r"); + new_args.push_back(std::string("__node:=") + this->get_name() + "_rclcpp_node"); + new_args.push_back("--"); client_node_ = std::make_shared( - std::string(get_name()) + std::string("_rclcpp_node")); + "_", "", rclcpp::NodeOptions().arguments(new_args)); nav_to_pose_client_ = rclcpp_action::create_client( client_node_, "navigate_to_pose"); @@ -95,13 +98,6 @@ WaypointFollower::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -WaypointFollower::on_error(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - nav2_util::CallbackReturn WaypointFollower::on_shutdown(const rclcpp_lifecycle::State & /*state*/) { @@ -126,14 +122,22 @@ WaypointFollower::followWaypoints() get_logger(), "Received follow waypoint request with %i waypoints.", static_cast(goal->poses.size())); - rclcpp::Rate r(loop_rate_); + if (goal->poses.size() == 0) { + action_server_->succeeded_current(result); + return; + } + + rclcpp::WallRate r(loop_rate_); uint32_t goal_index = 0; bool new_goal = true; while (rclcpp::ok()) { // Check if asked to stop processing action if (action_server_->is_cancel_requested()) { - RCLCPP_INFO(get_logger(), "Cancelling action."); + auto cancel_future = nav_to_pose_client_->async_cancel_all_goals(); + rclcpp::spin_until_future_complete(client_node_, cancel_future); + // for result callback processing + spin_some(client_node_); action_server_->terminate_all(); return; } @@ -157,7 +161,7 @@ WaypointFollower::followWaypoints() std::bind(&WaypointFollower::resultCallback, this, std::placeholders::_1); send_goal_options.goal_response_callback = std::bind(&WaypointFollower::goalResponseCallback, this, std::placeholders::_1); - auto future_goal_handle = + future_goal_handle_ = nav_to_pose_client_->async_send_goal(client_goal, send_goal_options); current_goal_status_ = ActionStatus::PROCESSING; } diff --git a/navigation2/package.xml b/navigation2/package.xml index 3c3b35b7320..8b6811af54b 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 0.3.4 + 0.4.7 ROS2 Navigation Stack @@ -30,6 +30,8 @@ nav2_voxel_grid nav2_controller nav2_waypoint_follower + smac_planner + nav2_regulated_pure_pursuit_controller ament_cmake diff --git a/smac_planner/CMakeLists.txt b/smac_planner/CMakeLists.txt new file mode 100644 index 00000000000..d161ba516e8 --- /dev/null +++ b/smac_planner/CMakeLists.txt @@ -0,0 +1,123 @@ +cmake_minimum_required(VERSION 3.5) +project(smac_planner) + +set(CMAKE_BUILD_TYPE Release) #Debug, Release + +find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(std_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(pluginlib REQUIRED) +find_package(Ceres REQUIRED COMPONENTS SuiteSparse) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(ompl REQUIRED) +find_package(OpenMP REQUIRED) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + +add_compile_options(-O3 -Wextra -Wdeprecated -fPIC) + +include_directories( + include + ${CERES_INCLUDES} + ${OMPL_INCLUDE_DIRS} + ${OpenMP_INCLUDE_DIRS} +) +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") +set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") + +set(library_name smac_planner) + +set(dependencies + rclcpp + rclcpp_action + rclcpp_lifecycle + std_msgs + visualization_msgs + nav2_util + nav2_msgs + nav_msgs + geometry_msgs + builtin_interfaces + tf2_ros + nav2_costmap_2d + nav2_core + pluginlib + eigen3_cmake_module +) + +# SE2 plugin +add_library(${library_name} SHARED + src/smac_planner.cpp + src/a_star.cpp + src/node_se2.cpp + src/costmap_downsampler.cpp + src/node_2d.cpp +) + +target_link_libraries(${library_name} ${CERES_LIBRARIES} ${OMPL_LIBRARIES} ${OpenMP_LIBRARIES} OpenMP::OpenMP_CXX) +target_include_directories(${library_name} PUBLIC ${Eigen3_INCLUDE_DIRS}) + +ament_target_dependencies(${library_name} + ${dependencies} +) + +# 2D plugin +add_library(${library_name}_2d SHARED + src/smac_planner_2d.cpp + src/a_star.cpp + src/node_se2.cpp + src/costmap_downsampler.cpp + src/node_2d.cpp +) + +target_link_libraries(${library_name}_2d ${CERES_LIBRARIES} ${OMPL_LIBRARIES}) +target_include_directories(${library_name}_2d PUBLIC ${Eigen3_INCLUDE_DIRS}) + +ament_target_dependencies(${library_name}_2d + ${dependencies} +) + +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +target_compile_definitions(${library_name}_2d PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(nav2_core smac_plugin.xml) +pluginlib_export_plugin_description_file(nav2_core smac_plugin_2d.xml) + +install(TARGETS ${library_name} ${library_name}_2d + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) +endif() + +ament_export_include_directories(include) +ament_export_libraries(${library_name} ${library_name}_2d) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/smac_planner/README.md b/smac_planner/README.md new file mode 100644 index 00000000000..5bf0cd4217b --- /dev/null +++ b/smac_planner/README.md @@ -0,0 +1,173 @@ +# Smac Planner + +The SmacPlanner is a plugin for the Nav2 Planner server. It includes currently 2 distinct plugins: +- `SmacPlanner`: a highly optimized fully reconfigurable Hybrid-A* implementation supporting Dubin and Reeds-Shepp models. +- `SmacPlanner2D`: a highly optimized fully reconfigurable grid-based A* implementation supporting Moore and Von Neumann models. + +It also introduces the following basic building blocks: +- `CostmapDownsampler`: A library to take in a costmap object and downsample it to another resolution. +- `AStar`: A generic and highly optimized A* template library used by the planning plugins to search. Template implementations are provided for grid-A* and SE2 Hybrid-A* planning. Additional template for 3D planning also could be made available. +- `CollisionChecker`: Collision check based on a robot's radius or footprint. +- `Smoother`: A Conjugate-gradient (CG) smoother with several optional cost function implementations for use. This is a cost-aware smoother unlike b-splines or bezier curves. + +We have users reporting using this on: +- Delivery robots +- Industrial robots + +## Introduction + +The `smac_planner` package contains an optimized templated A* search algorithm used to create multiple A\*-based planners for multiple types of robot platforms. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). We support differential-drive and omni-directional drive robots using the `SmacPlanner2D` planner which implements a cost-aware A\* planner. We support cars, car-like, and ackermann vehicles using the `SmacPlanner` plugin which implements a Hybrid-A\* planner. This plugin is also useful for curvature constrained planning, like when planning robot at high speeds to make sure they don't flip over or otherwise skid out of control. It is also applicable to non-round robots (such as large rectangular or arbitrary shaped robots of differential/omnidirectional drivetrains) that need pose-based collision checking. + +The `SmacPlanner` fully-implements the Hybrid-A* planner as proposed in [Practical Search Techniques in Path Planning for Autonomous Driving](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf), including hybrid searching, CG smoothing, analytic expansions and hueristic functions. + +In summary... + +The `SmacPlanner` is designed to work with: +- Ackermann, car, and car-like robots +- High speed or curvature constrained robots (as to not flip over, skid, or dump load at high speeds) +- Arbitrary shaped, non-circular differential or omnidirectional robots requring SE2 collision checking + +The `SmacPlanner2D` is designed to work with: +- Circular, differential or omnidirectional robots +- Relatively small robots with respect to environment size (e.g. RC car in a hallway or large robot in a convention center) that can be approximated by circular footprint. + +## Features + +We further improve on the Hybrid-A\* work in the following ways: +- Remove need for upsampling by searching with 10x smaller motion primitives (same as their upsampling ratio). +- Multi-resolution search allowing planning to occur at a coarser resolution for wider spaces (O(N^2) faster). +- Cost-aware penalty functions in search resulting in far smoother plans (further reducing requirement to smooth). +- Additional cost functions in the CG smoother including path deflection. +- Approximated smoother Voronoi field using costmap inflation function. +- Fixed 3 mathematical issues in the original paper resulting in higher quality smoothing. +- Faster planning than original paper by highly optimizing the template A\* algorithm. +- Automatically adjusted search motion model sizes by motion model, costmap resolution, and bin sizing. +- Closest path on approach within tolerance if exact path cannot be found or in invalid space. +- Multi-model hybrid searching including Dubin and Reeds-Shepp models. More models may be trivially added. +- Time monitoring of planning to dynamically scale the maximum CG smoothing time based on remaining planning duration requested. +- High unit and integration test coverage, doxygen documentation. +- Uses modern C++14 language features and individual components are easily reusable. +- Speed optimizations: Fast approximation of shortest paths with wavefront hueristic, no data structure graph lookups in main loop, near-zero copy main loop, Voronoi field approximation, et al. +- Templated Nodes and A\* implementation to support additional robot extensions. + +All of these features (multi-resolution, models, smoother, etc) are also available in the 2D `SmacPlanner2D` plugin. + +The 2D A\* implementation also does not have any of the weird artifacts introduced by the gradient wavefront-based 2D A\* implementation in the NavFn Planner. While this 2D A\* planner is slightly slower, I believe it's well worth the increased quality in paths. Though the `SmacPlanner2D` is grid-based, any reasonable local trajectory planner - including those supported by Navigation2 - will not have any issue with grid-based plans. + +## Metrics + +The original Hybrid-A\* implementation boasted planning times of 50-300ms for planning across 102,400 cell maps with 72 angular bins. We see much faster results in our evaluations: + +- **2-20ms** for planning across 147,456 (1.4x larger) cell maps with 72 angular bins. +- **30-120ms** for planning across 344,128 (3.3x larger) cell map with 72 angular bins. + +For example, the following path (roughly 85 meters) path took 33ms to compute. + +![alt text](test/path.png) + +## Design + +The basic design centralizes a templated A\* implementation that handles the search of a graph of nodes. The implementation is templated by the nodes, `NodeT`, which contain the methods needed to compute the hueristics, travel costs, and search neighborhoods. The outcome of this design is then a standard A\* implementation that can be used to traverse any type of graph as long as a node template can be created for it. + +We provide by default a 2D node template (`Node2D`) which does 2D grid-search with either 4 or 8-connected neighborhoods, but the smoother can be used to smooth it out. We also provide a SE2 node template (`NodeSE2`) which does SE2 (X, Y, theta) search and collision checking on Dubin or Reeds-Shepp motion models. Additional templates could be easily made and included for 3D grid search and non-grid base searching like routing. + +Additional node templates could be added into the future to better support other types of robot path planning, such as including a state lattice motion primitive node and 3D path planning. Pull requests or discussions aroudn this point is encouraged. + +In the ROS2 facing plugin, we take in the global goals and pre-process the data to feed into the templated A\* used. This includes processing any requests to downsample the costmap to another resolution to speed up search and smoothing the resulting A\* path. For the SE2 and `SmacPlanner` plugins, the path is promised to be kinematically feasible due to the kinematically valid models used in branching search. The 2D A\* is also promised to be feasible for differential and omni-directional robots. + +We isolated the A\*, costmap downsampler, smoother, and Node template objects from ROS2 to allow them to be easily testable independently of ROS or the planner. The only place ROS is used is in the planner plugins themselves. + +## Parameters + +See inline description of parameters in the `SmacPlanner`. This includes comments as specific parameters apply to `SmacPlanner2D` and `SmacPlanner` in SE2 place. + +``` +planner_server: + ros__parameters: + planner_plugins: ["GridBased"] + use_sim_time: True + + GridBased: + plugin: "smac_planner/SmacPlanner" + tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters, for 2D node + 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: -1 # maximum total iterations to search for before failing + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance, 2D only + max_planning_time_ms: 2000.0 # max time in ms for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. + smooth_path: false # Whether to smooth searched path + motion_model_for_search: "DUBIN" # 2D Moore, Von Neumann; SE2 Dubin, Redds-Shepp + angle_quantization_bins: 72 # For SE2 node: Number of angle bins for search, must be 1 for 2D node (no angle search) + minimum_turning_radius: 0.20 # For SE2 node & smoother: minimum turning radius in m of path / vehicle + reverse_penalty: 2.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.20 # For SE2 node: penalty to apply if motion is changing directions, must be >= 0 + non_straight_penalty: 1.05 # For SE2 node: penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 1.3 # For SE2 node: penalty to apply to higher cost zones + + smoother: + smoother: + w_curve: 30.0 # weight to minimize curvature of path + w_dist: 0.0 # weight to bind path to original as optional replacement for cost weight + w_smooth: 30000.0 # weight to maximize smoothness of path + w_cost: 0.025 # weight to steer robot away from collision and cost + cost_scaling_factor: 10.0 # this should match the inflation layer's parameter + + # I do not recommend users mess with this unless they're doing production tuning + optimizer: + max_time: 0.10 # maximum compute time for smoother + max_iterations: 500 # max iterations of smoother + debug_optimizer: false # print debug info + gradient_tol: 1.0e-10 + fn_tol: 1.0e-20 + param_tol: 1.0e-15 + advanced: + min_line_search_step_size: 1.0e-20 + max_num_line_search_step_size_iterations: 50 + line_search_sufficient_function_decrease: 1.0e-20 + max_num_line_search_direction_restarts: 10 + max_line_search_step_expansion: 50 +``` + +## Topics + +| Topic | Type | +|-----------------|-------------------| +| unsmoothed_path | nav_msgs/Path | + + +## Install + +``` +sudo apt-get install ros--smac-planner +``` + +## Etc (Important Side Notes) + +### Potential Fields + +Many users and default navigation configuration files I find are really missing the point of the inflation layer. While it's true that you can simply inflate a small radius around the walls, the _true_ value of the inflation layer is creating a consistent potential field around the entire map. + +Some of the most popular tuning guides for Navigation / Navigation2 even [call this out specifically](https://arxiv.org/pdf/1706.09068.pdf) that there's substantial benefit to creating a gentle potential field across the width of the map - after inscribed costs are applied - yet very few users do this. + +This habit actually results in paths produced by NavFn, Global Planner, and now SmacPlanner to be very suboptimal. They really want to look for a smooth potential field rather than wide open 0-cost spaces in order to stay in the middle of spaces and deal with close-by moving obstacles better. + +So it is my recommendation in using this package, as well as all other cost-aware search planners available in ROS, to increase your inflation layer cost scale in order to adequately produce a smooth potential across the entire map. For very large open spaces, its fine to have 0-cost areas in the middle, but for halls, aisles, and similar; **please create a smooth potential to provide the best performance**. + +### 2D Search and Smoothing + +While the 2D planner has the smoother available (albeit, default parameters are tuned for the Hybrid-A\* planner, so you may need to play with that), my recommendation is not to use it. + +The 2D planner provides a 4-connected or 8-connected neighborhood path. This path may have little zig-zags in order to get at another non-90 or non-45 degree heading. That is totally fine. Your local trajectory planner such as DWB and TEB take these points into account to follow, but you won't see any zig-zag behaviors of your final robot motion after given to a trajectory planner. + +The smoothing is more "pleasing" to human eyes, but you don't want to be owning additional compute when it doesn't largely impact the output. However, if you have a more sensitive local trajectory planner like a carrot follower (e.g. pure pursuit), then you will want to smooth out the paths in order to have something more easily followable. + +Take this advise into account. Some good numbers to potentially start with would be `cost_scaling_factor: 10.0` and `inflation_radius: 5.5`. + +### Costmap Resolutions + +We provide for both the Hybrid-A\* and 2D A\* implementations a costmap downsampler option. This can be **incredible** beneficial when planning very long paths in larger spaces. The motion models for SE2 planning and neighborhood search in 2D planning is proportional to the costmap resolution. By downsampling it, you can N^2 reduce the number of expansions required to achieve a particular goal. However, the lower the resolution, the larger small obstacles appear and you won't be able to get super close to obstacles. This is a trade-off to make and test. Some numbers I've seen are 2-4x drops in planning CPU time for a 2-3x downsample rate. For 60m paths in an office space, I was able to get it << 100ms at a 2-3x downsample rate. + +I recommend users using a 5cm resolution costmap and playing with the different values of downsampling rate until they achieve what they think is optimal performance (lowest number of expansions vs. necessity to achieve fine goal poses). Then, I would recommend to change the global costmap resolution to this new value. That way you don't own the compute of downsampling and maintaining a higher-resolution costmap that isn't used. + +Remember, the global costmap is **only** there to provide an environment for the planner to work in. It is not there for human-viewing even if a more fine resolution costmap is more human "pleasing". If you use multiple planners in the planner server, then you will want to use the highest resolution for the most needed planner and then use the downsampler to downsample to the Hybrid-A* resolution. diff --git a/smac_planner/include/smac_planner/a_star.hpp b/smac_planner/include/smac_planner/a_star.hpp new file mode 100644 index 00000000000..a53b872a913 --- /dev/null +++ b/smac_planner/include/smac_planner/a_star.hpp @@ -0,0 +1,321 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// +// 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 SMAC_PLANNER__A_STAR_HPP_ +#define SMAC_PLANNER__A_STAR_HPP_ + +#include +#include +#include +#include +#include +#include +#include "Eigen/Core" + +#include "nav2_costmap_2d/costmap_2d.hpp" + +#include "smac_planner/node_2d.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/node_basic.hpp" +#include "smac_planner/types.hpp" +#include "smac_planner/constants.hpp" + +namespace smac_planner +{ + +inline double squaredDistance( + const Eigen::Vector2d & p1, + const Eigen::Vector2d & p2) +{ + const double & dx = p1[0] - p2[0]; + const double & dy = p1[1] - p2[1]; + return hypot(dx, dy); +} + +/** + * @class smac_planner::AStarAlgorithm + * @brief An A* implementation for planning in a costmap. Templated based on the Node type. + */ +template +class AStarAlgorithm +{ +public: + typedef NodeT * NodePtr; + typedef std::unordered_map Graph; + typedef std::vector NodeVector; + typedef std::pair> NodeElement; + typedef typename NodeT::Coordinates Coordinates; + typedef typename NodeT::CoordinateVector CoordinateVector; + typedef typename NodeVector::iterator NeighborIterator; + typedef std::function NodeGetter; + + /** + * @struct smac_planner::NodeComparator + * @brief Node comparison for priority queue sorting + */ + struct NodeComparator + { + bool operator()(const NodeElement & a, const NodeElement & b) const + { + return a.first > b.first; + } + }; + + typedef std::priority_queue, NodeComparator> NodeQueue; + + /** + * @brief A constructor for smac_planner::PlannerServer + * @param neighborhood The type of neighborhood to use for search (4 or 8 connected) + */ + explicit AStarAlgorithm(const MotionModel & motion_model, const SearchInfo & search_info); + + /** + * @brief A destructor for smac_planner::AStarAlgorithm + */ + ~AStarAlgorithm(); + + /** + * @brief Initialization of the planner with defaults + * @param allow_unknown Allow search in unknown space, good for navigation while mapping + * @param max_iterations Maximum number of iterations to use while expanding search + * @param max_on_approach_iterations Maximum number of iterations before returning a valid + * path once within thresholds to refine path + * comes at more compute time but smoother paths. + */ + void initialize( + const bool & allow_unknown, + int & max_iterations, + const int & max_on_approach_iterations); + + /** + * @brief Creating path from given costmap, start, and goal + * @param path Reference to a vector of indicies of generated path + * @param num_iterations Reference to number of iterations to create plan + * @param tolerance Reference to tolerance in costmap nodes + * @return if plan was successful + */ + bool createPath(CoordinateVector & path, int & num_iterations, const float & tolerance); + + /** + * @brief Create the graph based on the node type. For 2D nodes, a cost grid. + * For 3D nodes, a SE2 grid without cost info as needs collision detector for footprint. + * @param x The total number of nodes in the X direction + * @param y The total number of nodes in the X direction + * @param dim_3 The total number of nodes in the theta or Z direction + * @param costmap Costmap to convert into the graph + */ + void createGraph( + const unsigned int & x, + const unsigned int & y, + const unsigned int & dim_3, + nav2_costmap_2d::Costmap2D * & costmap); + + /** + * @brief Set the goal for planning, as a node index + * @param mx The node X index of the goal + * @param my The node Y index of the goal + * @param dim_3 The node dim_3 index of the goal + */ + void setGoal( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3); + + /** + * @brief Set the starting pose for planning, as a node index + * @param mx The node X index of the goal + * @param my The node Y index of the goal + * @param dim_3 The node dim_3 index of the goal + */ + void setStart( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3); + + /** + * @brief Set the footprint + * @param footprint footprint of robot + * @param use_radius Whether this footprint is a circle with radius + */ + void setFootprint(nav2_costmap_2d::Footprint footprint, bool use_radius); + + /** + * @brief Perform an analytic path expansion to the goal + * @param node The node to start the analytic path from + * @param getter The function object that gets valid nodes from the graph + * @return Node pointer to goal node if successful, else return nullptr + */ + NodePtr getAnalyticPath(const NodePtr & node, const NodeGetter & getter); + + /** + * @brief Set the starting pose for planning, as a node index + * @param node Node pointer to the goal node to backtrace + * @param path Reference to a vector of indicies of generated path + * @return whether the path was able to be backtraced + */ + bool backtracePath(NodePtr & node, CoordinateVector & path); + + /** + * @brief Get maximum number of iterations to plan + * @return Reference to Maximum iterations parameter + */ + int & getMaxIterations(); + + /** + * @brief Get pointer reference to starting node + * @return Node pointer reference to starting node + */ + NodePtr & getStart(); + + /** + * @brief Get pointer reference to goal node + * @return Node pointer reference to goal node + */ + NodePtr & getGoal(); + + /** + * @brief Get maximum number of on-approach iterations after within threshold + * @return Reference to Maximum on-appraoch iterations parameter + */ + int & getOnApproachMaxIterations(); + + /** + * @brief Get tolerance, in node nodes + * @return Reference to tolerance parameter + */ + float & getToleranceHeuristic(); + + /** + * @brief Get size of graph in X + * @return Size in X + */ + unsigned int & getSizeX(); + + /** + * @brief Get size of graph in Y + * @return Size in Y + */ + unsigned int & getSizeY(); + + /** + * @brief Get number of angle quantization bins (SE2) or Z coordinate (XYZ) + * @return Number of angle bins / Z dimension + */ + unsigned int & getSizeDim3(); + +protected: + /** + * @brief Get pointer to next goal in open set + * @return Node pointer reference to next heuristically scored node + */ + inline NodePtr getNextNode(); + + /** + * @brief Get pointer to next goal in open set + * @param cost The cost to sort into the open set of the node + * @param node Node pointer reference to add to open set + */ + inline void addNode(const float cost, NodePtr & node); + + /** + * @brief Adds node to graph + * @param cost The cost to sort into the open set of the node + * @param node Node pointer reference to add to open set + */ + inline NodePtr addToGraph(const unsigned int & index); + + /** + * @brief Check if this node is the goal node + * @param node Node pointer to check if its the goal node + * @return if node is goal + */ + inline bool isGoal(NodePtr & node); + + /** + * @brief Get cost of traversal between nodes + * @param current_node Pointer to current node + * @param new_node Pointer to new node + * @return Reference traversal cost between the nodes + */ + inline float getTraversalCost(NodePtr & current_node, NodePtr & new_node); + + /** + * @brief Get total cost of traversal for a node + * @param node Pointer to current node + * @return Reference accumulated cost between the nodes + */ + inline float & getAccumulatedCost(NodePtr & node); + + /** + * @brief Get cost of heuristic of node + * @param node Node index current + * @param node Node index of new + * @return Heuristic cost between the nodes + */ + inline float getHeuristicCost(const NodePtr & node); + + /** + * @brief Check if inputs to planner are valid + * @return Are valid + */ + inline bool areInputsValid(); + + /** + * @brief Clear hueristic queue of nodes to search + */ + inline void clearQueue(); + + /** + * @brief Clear graph of nodes searched + */ + inline void clearGraph(); + + /** + * @brief Attempt an analytic path completion + * @return Node pointer reference to goal node if successful, else + * return nullptr + */ + inline NodePtr tryAnalyticExpansion( + const NodePtr & current_node, + const NodeGetter & getter, int & iterations, int & best_cost); + + bool _traverse_unknown; + int _max_iterations; + int _max_on_approach_iterations; + float _tolerance; + unsigned int _x_size; + unsigned int _y_size; + unsigned int _dim3_size; + SearchInfo _search_info; + + Coordinates _goal_coordinates; + NodePtr _start; + NodePtr _goal; + + Graph _graph; + NodeQueue _queue; + + MotionModel _motion_model; + NodeHeuristicPair _best_heuristic_node; + + GridCollisionChecker _collision_checker; + nav2_costmap_2d::Footprint _footprint; + bool _is_radius_footprint; + nav2_costmap_2d::Costmap2D * _costmap; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__A_STAR_HPP_ diff --git a/smac_planner/include/smac_planner/collision_checker.hpp b/smac_planner/include/smac_planner/collision_checker.hpp new file mode 100644 index 00000000000..588514901ff --- /dev/null +++ b/smac_planner/include/smac_planner/collision_checker.hpp @@ -0,0 +1,113 @@ +// Copyright (c) 2020, 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 "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "smac_planner/constants.hpp" + +#ifndef SMAC_PLANNER__COLLISION_CHECKER_HPP_ +#define SMAC_PLANNER__COLLISION_CHECKER_HPP_ + +namespace smac_planner +{ + +/** + * @class smac_planner::GridCollisionChecker + * @brief A costmap grid collision checker + */ +class GridCollisionChecker + : public nav2_costmap_2d::FootprintCollisionChecker +{ +public: + /** + * @brief A constructor for smac_planner::GridCollisionChecker + * @param costmap The costmap to collision check against + */ + GridCollisionChecker( + nav2_costmap_2d::Costmap2D * costmap) + : FootprintCollisionChecker(costmap) + { + } + + /** + * @brief Set the footprint to use with collision checker + * @param footprint The footprint to collision check against + * @param radius Whether or not the footprint is a circle and use radius collision checking + */ + void setFootprint(const nav2_costmap_2d::Footprint & footprint, const bool & radius) + { + unoriented_footprint_ = footprint; + footprint_is_radius_ = radius; + } + + /** + * @brief Check if in collision with costmap and footprint at pose + * @param x X coordinate of pose to check against + * @param y Y coordinate of pose to check against + * @param theta Angle of pose to check against + * @param traverse_unknown Whether or not to traverse in unknown space + * @return boolean if in collision or not. + */ + bool inCollision( + const float & x, + const float & y, + const float & theta, + const bool & traverse_unknown) + { + // Assumes setFootprint already set + double wx, wy; + costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); + + if (!footprint_is_radius_) { + // if footprint, then we check for the footprint's points + footprint_cost_ = footprintCostAtPose( + wx, wy, static_cast(theta), unoriented_footprint_); + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= OCCUPIED; + } else { + // if radius, then we can check the center of the cost assuming inflation is used + footprint_cost_ = costmap_->getCost( + static_cast(x), static_cast(y)); + + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= INSCRIBED; + } + } + + /** + * @brief Get cost at footprint pose in costmap + * @return the cost at the pose in costmap + */ + float getCost() + { + // Assumes inCollision called prior + return static_cast(footprint_cost_); + } + +protected: + nav2_costmap_2d::Footprint unoriented_footprint_; + double footprint_cost_; + bool footprint_is_radius_; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__COLLISION_CHECKER_HPP_ diff --git a/smac_planner/include/smac_planner/constants.hpp b/smac_planner/include/smac_planner/constants.hpp new file mode 100644 index 00000000000..bc2ccfd8c63 --- /dev/null +++ b/smac_planner/include/smac_planner/constants.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2020, 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 SMAC_PLANNER__CONSTANTS_HPP_ +#define SMAC_PLANNER__CONSTANTS_HPP_ + +#include + +namespace smac_planner +{ +enum class MotionModel +{ + UNKNOWN = 0, + VON_NEUMANN = 1, + MOORE = 2, + DUBIN = 3, + REEDS_SHEPP = 4, +}; + +inline std::string toString(const MotionModel & n) +{ + switch (n) { + case MotionModel::VON_NEUMANN: + return "Von Neumann"; + case MotionModel::MOORE: + return "Moore"; + case MotionModel::DUBIN: + return "Dubin"; + case MotionModel::REEDS_SHEPP: + return "Reeds-Shepp"; + default: + return "Unknown"; + } +} + +inline MotionModel fromString(const std::string & n) +{ + if (n == "VON_NEUMANN") { + return MotionModel::VON_NEUMANN; + } else if (n == "MOORE") { + return MotionModel::MOORE; + } else if (n == "DUBIN") { + return MotionModel::DUBIN; + } else if (n == "REEDS_SHEPP") { + return MotionModel::REEDS_SHEPP; + } else { + return MotionModel::UNKNOWN; + } +} + +const float UNKNOWN = 255; +const float OCCUPIED = 254; +const float INSCRIBED = 253; +const float MAX_NON_OBSTACLE = 252; +const float FREE = 0; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__CONSTANTS_HPP_ diff --git a/smac_planner/include/smac_planner/costmap_downsampler.hpp b/smac_planner/include/smac_planner/costmap_downsampler.hpp new file mode 100644 index 00000000000..bdfb5cdf33c --- /dev/null +++ b/smac_planner/include/smac_planner/costmap_downsampler.hpp @@ -0,0 +1,115 @@ +// Copyright (c) 2020, Carlos Luis +// +// 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 SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ +#define SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ + +#include +#include +#include + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "smac_planner/constants.hpp" + +namespace smac_planner +{ + +/** + * @class smac_planner::CostmapDownsampler + * @brief A costmap downsampler for more efficient path planning + */ +class CostmapDownsampler +{ +public: + /** + * @brief A constructor for CostmapDownsampler + */ + CostmapDownsampler(); + + /** + * @brief A destructor for CostmapDownsampler + */ + ~CostmapDownsampler(); + + /** + * @brief Configure the downsampled costmap object and the ROS publisher + * @param node Lifecycle node pointer + * @param global_frame The ID of the global frame used by the costmap + * @param topic_name The name of the topic to publish the downsampled costmap + * @param costmap The costmap we want to downsample + * @param downsampling_factor Multiplier for the costmap resolution + */ + void on_configure( + const nav2_util::LifecycleNode::SharedPtr & node, + const std::string & global_frame, + const std::string & topic_name, + nav2_costmap_2d::Costmap2D * const costmap, + const unsigned int & downsampling_factor); + + /** + * @brief Activate the publisher of the downsampled costmap + */ + void on_activate(); + + /** + * @brief Deactivate the publisher of the downsampled costmap + */ + void on_deactivate(); + + /** + * @brief Cleanup the publisher of the downsampled costmap + */ + void on_cleanup(); + + /** + * @brief Downsample the given costmap by the downsampling factor, and publish the downsampled costmap + * @param downsampling_factor Multiplier for the costmap resolution + * @return A ptr to the downsampled costmap + */ + nav2_costmap_2d::Costmap2D * downsample(const unsigned int & downsampling_factor); + + /** + * @brief Resize the downsampled costmap. Used in case the costmap changes and we need to update the downsampled version + */ + void resizeCostmap(); + +protected: + /** + * @brief Update the sizes X-Y of the costmap and its downsampled version + */ + void updateCostmapSize(); + + /** + * @brief Explore all subcells of the original costmap and assign the max cost to the new (downsampled) cell + * @param new_mx The X-coordinate of the cell in the new costmap + * @param new_my The Y-coordinate of the cell in the new costmap + */ + void setCostOfCell( + const unsigned int & new_mx, + const unsigned int & new_my); + + unsigned int _size_x; + unsigned int _size_y; + unsigned int _downsampled_size_x; + unsigned int _downsampled_size_y; + unsigned int _downsampling_factor; + float _downsampled_resolution; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr _downsampled_costmap; + std::unique_ptr _downsampled_costmap_pub; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ diff --git a/smac_planner/include/smac_planner/node_2d.hpp b/smac_planner/include/smac_planner/node_2d.hpp new file mode 100644 index 00000000000..b80a316cbb1 --- /dev/null +++ b/smac_planner/include/smac_planner/node_2d.hpp @@ -0,0 +1,248 @@ +// Copyright (c) 2020, 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 SMAC_PLANNER__NODE_2D_HPP_ +#define SMAC_PLANNER__NODE_2D_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "smac_planner/constants.hpp" +#include "smac_planner/collision_checker.hpp" + +namespace smac_planner +{ + +/** + * @class smac_planner::Node2D + * @brief Node2D implementation for graph + */ +class Node2D +{ +public: + typedef Node2D * NodePtr; + typedef std::unique_ptr> Graph; + typedef std::vector NodeVector; + + /** + * @class smac_planner::Node2D::Coordinates + * @brief Node2D implementation of coordinate structure + */ + struct Coordinates + { + Coordinates() {} + Coordinates(const float & x_in, const float & y_in) + : x(x_in), y(y_in) + {} + + float x, y; + }; + typedef std::vector CoordinateVector; + + /** + * @brief A constructor for smac_planner::Node2D + * @param cost_in The costmap cost at this node + * @param index The index of this node for self-reference + */ + explicit Node2D(unsigned char & cost_in, const unsigned int index); + + /** + * @brief A destructor for smac_planner::Node2D + */ + ~Node2D(); + + /** + * @brief operator== for comparisons + * @param Node2D right hand side node reference + * @return If cell indicies are equal + */ + bool operator==(const Node2D & rhs) + { + return this->_index == rhs._index; + } + + /** + * @brief Reset method for new search + * @param cost_in The costmap cost at this node + */ + void reset(const unsigned char & cost); + /** + * @brief Gets the accumulated cost at this node + * @return accumulated cost + */ + inline float & getAccumulatedCost() + { + return _accumulated_cost; + } + + /** + * @brief Sets the accumulated cost at this node + * @param reference to accumulated cost + */ + inline void setAccumulatedCost(const float cost_in) + { + _accumulated_cost = cost_in; + } + + /** + * @brief Gets the costmap cost at this node + * @return costmap cost + */ + inline float & getCost() + { + return _cell_cost; + } + + /** + * @brief Gets if cell has been visited in search + * @param If cell was visited + */ + inline bool & wasVisited() + { + return _was_visited; + } + + /** + * @brief Sets if cell has been visited in search + */ + inline void visited() + { + _was_visited = true; + _is_queued = false; + } + + /** + * @brief Gets if cell is currently queued in search + * @param If cell was queued + */ + inline bool & isQueued() + { + return _is_queued; + } + + /** + * @brief Sets if cell is currently queued in search + */ + inline void queued() + { + _is_queued = true; + } + + /** + * @brief Gets cell index + * @return Reference to cell index + */ + inline unsigned int & getIndex() + { + return _index; + } + + /** + * @brief Check if this node is valid + * @param traverse_unknown If we can explore unknown nodes on the graph + * @param collision_checker Pointer to collision checker object + * @return whether this node is valid and collision free + */ + bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker collision_checker); + + /** + * @brief get traversal cost from this node to child node + * @param child Node pointer to this node's child + * @return traversal cost + */ + float getTraversalCost(const NodePtr & child); + + /** + * @brief Get index + * @param x x coordinate of point to get index of + * @param y y coordinate of point to get index of + * @param width width of costmap + * @return index + */ + static inline unsigned int getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & width) + { + return x + y * width; + } + + /** + * @brief Get index + * @param Index Index of point + * @param width width of costmap + * @param angles angle bins to use (must be 1 or throws exception) + * @return coordinates of point + */ + static inline Coordinates getCoords( + const unsigned int & index, const unsigned int & width, const unsigned int & angles) + { + if (angles != 1) { + throw std::runtime_error("Node type Node2D does not have a valid angle quantization."); + } + + return Coordinates(index % width, index / width); + } + + /** + * @brief Get cost of heuristic of node + * @param node Node index current + * @param node Node index of new + * @return Heuristic cost between the nodes + */ + static float getHeuristicCost( + const Coordinates & node_coords, + const Coordinates & goal_coordinates); + + /** + * @brief Initialize the neighborhood to be used in A* + * We support 4-connect (VON_NEUMANN) and 8-connect (MOORE) + * @param x_size_uint The total x size to find neighbors + * @param neighborhood The desired neighborhood type + */ + static void initNeighborhood( + const unsigned int & x_size_uint, + const MotionModel & neighborhood); + /** + * @brief Retrieve all valid neighbors of a node. + * @param node Pointer to the node we are currently exploring in A* + * @param graph Reference to graph to discover new nodes + * @param neighbors Vector of neighbors to be filled + */ + static void getNeighbors( + NodePtr & node, + std::function & validity_checker, + GridCollisionChecker collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors); + + Node2D * parent; + static double neutral_cost; + static std::vector _neighbors_grid_offsets; + +private: + float _cell_cost; + float _accumulated_cost; + unsigned int _index; + bool _was_visited; + bool _is_queued; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__NODE_2D_HPP_ diff --git a/smac_planner/include/smac_planner/node_basic.hpp b/smac_planner/include/smac_planner/node_basic.hpp new file mode 100644 index 00000000000..3f3941717d1 --- /dev/null +++ b/smac_planner/include/smac_planner/node_basic.hpp @@ -0,0 +1,68 @@ +// Copyright (c) 2020, 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 SMAC_PLANNER__NODE_BASIC_HPP_ +#define SMAC_PLANNER__NODE_BASIC_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ompl/base/StateSpace.h" + +#include "smac_planner/constants.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/node_2d.hpp" +#include "smac_planner/types.hpp" +#include "smac_planner/collision_checker.hpp" + +namespace smac_planner +{ + +/** + * @class smac_planner::NodeBasic + * @brief NodeBasic implementation for priority queue insertion + */ +template +class NodeBasic +{ +public: + /** + * @brief A constructor for smac_planner::NodeBasic + * @param cost_in The costmap cost at this node + * @param index The index of this node for self-reference + */ + explicit NodeBasic(const unsigned int index) + : index(index), + graph_node_ptr(nullptr) + { + } + + typename NodeT::Coordinates pose; // Used by NodeSE2 + NodeT * graph_node_ptr; + unsigned int index; +}; + +template class NodeBasic; +template class NodeBasic; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__NODE_BASIC_HPP_ diff --git a/smac_planner/include/smac_planner/node_se2.hpp b/smac_planner/include/smac_planner/node_se2.hpp new file mode 100644 index 00000000000..4fb1ae579dc --- /dev/null +++ b/smac_planner/include/smac_planner/node_se2.hpp @@ -0,0 +1,423 @@ +// Copyright (c) 2020, 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 SMAC_PLANNER__NODE_SE2_HPP_ +#define SMAC_PLANNER__NODE_SE2_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ompl/base/StateSpace.h" + +#include "smac_planner/constants.hpp" +#include "smac_planner/types.hpp" +#include "smac_planner/collision_checker.hpp" + +namespace smac_planner +{ + +// Need seperate pose struct for motion table operations + +/** + * @struct smac_planner::MotionPose + * @brief A struct for poses in motion primitives + */ +struct MotionPose +{ + /** + * @brief A constructor for smac_planner::MotionPose + */ + MotionPose() {} + + /** + * @brief A constructor for smac_planner::MotionPose + * @param x X pose + * @param y Y pose + * @param theta Angle of pose + */ + MotionPose(const float & x, const float & y, const float & theta) + : _x(x), _y(y), _theta(theta) + {} + + float _x; + float _y; + float _theta; +}; + +typedef std::vector MotionPoses; + +// Must forward declare +class NodeSE2; + +/** + * @struct smac_planner::MotionTable + * @brief A table of motion primitives and related functions + */ +struct MotionTable +{ + /** + * @brief A constructor for smac_planner::MotionTable + */ + MotionTable() {} + + /** + * @brief Initializing using Dubin model + * @param size_x_in Size of costmap in X + * @param size_y_in Size of costmap in Y + * @param angle_quantization_in Size of costmap in bin sizes + * @param search_info Parameters for searching + */ + void initDubin( + unsigned int & size_x_in, + unsigned int & size_y_in, + unsigned int & angle_quantization_in, + SearchInfo & search_info); + + /** + * @brief Initializing using Reeds-Shepp model + * @param size_x_in Size of costmap in X + * @param size_y_in Size of costmap in Y + * @param angle_quantization_in Size of costmap in bin sizes + * @param search_info Parameters for searching + */ + void initReedsShepp( + unsigned int & size_x_in, + unsigned int & size_y_in, + unsigned int & angle_quantization_in, + SearchInfo & search_info); + + /** + * @brief Get projections of motion models + * @param node Ptr to SE2 node + * @return A set of motion poses + */ + MotionPoses getProjections(const NodeSE2 * node); + + /** + * @brief Get a projection of motion model + * @param node Ptr to SE2 node + * @return A motion pose + */ + MotionPose getProjection(const NodeSE2 * node, const unsigned int & motion_index); + + MotionPoses projections; + unsigned int size_x; + unsigned int num_angle_quantization; + float num_angle_quantization_float; + float bin_size; + float change_penalty; + float non_straight_penalty; + float cost_penalty; + float reverse_penalty; + ompl::base::StateSpacePtr state_space; +}; + +/** + * @class smac_planner::NodeSE2 + * @brief NodeSE2 implementation for graph + */ +class NodeSE2 +{ +public: + typedef NodeSE2 * NodePtr; + typedef std::unique_ptr> Graph; + typedef std::vector NodeVector; + /** + * @class smac_planner::NodeSE2::Coordinates + * @brief NodeSE2 implementation of coordinate structure + */ + struct Coordinates + { + /** + * @brief A constructor for smac_planner::NodeSE2::Coordinates + */ + Coordinates() {} + + /** + * @brief A constructor for smac_planner::NodeSE2::Coordinates + * @param x_in X coordinate + * @param y_in Y coordinate + * @param theta_in Theta coordinate + */ + Coordinates(const float & x_in, const float & y_in, const float & theta_in) + : x(x_in), y(y_in), theta(theta_in) + {} + + float x, y, theta; + }; + + typedef std::vector CoordinateVector; + + /** + * @brief A constructor for smac_planner::NodeSE2 + * @param index The index of this node for self-reference + */ + explicit NodeSE2(const unsigned int index); + + /** + * @brief A destructor for smac_planner::NodeSE2 + */ + ~NodeSE2(); + + /** + * @brief operator== for comparisons + * @param NodeSE2 right hand side node reference + * @return If cell indicies are equal + */ + bool operator==(const NodeSE2 & rhs) + { + return this->_index == rhs._index; + } + + /** + * @brief setting continuous coordinate search poses (in partial-cells) + * @param Pose pose + */ + inline void setPose(const Coordinates & pose_in) + { + pose = pose_in; + } + + /** + * @brief Reset method for new search + */ + void reset(); + + /** + * @brief Gets the accumulated cost at this node + * @return accumulated cost + */ + inline float & getAccumulatedCost() + { + return _accumulated_cost; + } + + /** + * @brief Sets the accumulated cost at this node + * @param reference to accumulated cost + */ + inline void setAccumulatedCost(const float cost_in) + { + _accumulated_cost = cost_in; + } + + /** + * @brief Sets the motion primitive index used to achieve node in search + * @param reference to motion primitive idx + */ + inline void setMotionPrimitiveIndex(const unsigned int & idx) + { + _motion_primitive_index = idx; + } + + /** + * @brief Gets the motion primitive index used to achieve node in search + * @return reference to motion primitive idx + */ + inline unsigned int & getMotionPrimitiveIndex() + { + return _motion_primitive_index; + } + + /** + * @brief Gets the costmap cost at this node + * @return costmap cost + */ + inline float & getCost() + { + return _cell_cost; + } + + /** + * @brief Gets if cell has been visited in search + * @param If cell was visited + */ + inline bool & wasVisited() + { + return _was_visited; + } + + /** + * @brief Sets if cell has been visited in search + */ + inline void visited() + { + _was_visited = true; + _is_queued = false; + } + + /** + * @brief Gets if cell is currently queued in search + * @param If cell was queued + */ + inline bool & isQueued() + { + return _is_queued; + } + + /** + * @brief Sets if cell is currently queued in search + */ + inline void queued() + { + _is_queued = true; + } + + /** + * @brief Gets cell index + * @return Reference to cell index + */ + inline unsigned int & getIndex() + { + return _index; + } + + /** + * @brief Check if this node is valid + * @param traverse_unknown If we can explore unknown nodes on the graph + * @return whether this node is valid and collision free + */ + bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker collision_checker); + + /** + * @brief Get traversal cost of parent node to child node + * @param child Node pointer to child + * @return traversal cost + */ + float getTraversalCost(const NodePtr & child); + + /** + * @brief Get index at coordinates + * @param x X coordinate of point + * @param y Y coordinate of point + * @param angle Theta coordinate of point + * @param width Width of costmap + * @param angle_quantization Number of theta bins + * @return Index + */ + static inline unsigned int getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & angle, + const unsigned int & width, const unsigned int angle_quantization) + { + return angle + x * angle_quantization + y * width * angle_quantization; + } + + /** + * @brief Get index at coordinates + * @param x X coordinate of point + * @param y Y coordinate of point + * @param angle Theta coordinate of point + * @return Index + */ + static inline unsigned int getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & angle) + { + return getIndex( + x, y, angle, motion_table.size_x, + motion_table.num_angle_quantization); + } + + /** + * @brief Get coordinates at index + * @param index Index of point + * @param width Width of costmap + * @param angle_quantization Theta size of costmap + * @return Coordinates + */ + static inline Coordinates getCoords( + const unsigned int & index, + const unsigned int & width, const unsigned int angle_quantization) + { + return Coordinates( + (index / angle_quantization) % width, // x + index / (angle_quantization * width), // y + index % angle_quantization); // theta + } + + /** + * @brief Get cost of heuristic of node + * @param node Node index current + * @param node Node index of new + * @return Heuristic cost between the nodes + */ + static float getHeuristicCost( + const Coordinates & node_coords, + const Coordinates & goal_coordinates); + + /** + * @brief Initialize motion models + * @param motion_model Motion model enum to use + * @param size_x Size of X of graph + * @param size_y Size of y of graph + * @param angle_quantization Size of theta bins of graph + * @param search_info Search info to use + */ + static void initMotionModel( + const MotionModel & motion_model, + unsigned int & size_x, + unsigned int & size_y, + unsigned int & angle_quantization, + SearchInfo & search_info); + + /** + * @brief Compute the wavefront heuristic + * @param costmap Costmap to use to compute heuristic + * @param start_x Coordinate of Start X + * @param start_y Coordinate of Start Y + * @param goal_x Coordinate of Goal X + * @param goal_y Coordinate of Goal Y + */ + static void computeWavefrontHeuristic( + nav2_costmap_2d::Costmap2D * & costmap, + const unsigned int & start_x, const unsigned int & start_y, + const unsigned int & goal_x, const unsigned int & goal_y); + + /** + * @brief Retrieve all valid neighbors of a node. + * @param node Pointer to the node we are currently exploring in A* + * @param validity_checker Functor for state validity checking + * @param neighbors Vector of neighbors to be filled + */ + static void getNeighbors( + const NodePtr & node, + std::function & validity_checker, + GridCollisionChecker collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors); + + NodeSE2 * parent; + Coordinates pose; + static double neutral_cost; + static MotionTable motion_table; + +private: + float _cell_cost; + float _accumulated_cost; + unsigned int _index; + bool _was_visited; + bool _is_queued; + unsigned int _motion_primitive_index; + static std::vector _wavefront_heuristic; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__NODE_SE2_HPP_ diff --git a/smac_planner/include/smac_planner/options.hpp b/smac_planner/include/smac_planner/options.hpp new file mode 100644 index 00000000000..01951d8950a --- /dev/null +++ b/smac_planner/include/smac_planner/options.hpp @@ -0,0 +1,207 @@ +// Copyright (c) 2020, 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 SMAC_PLANNER__OPTIONS_HPP_ +#define SMAC_PLANNER__OPTIONS_HPP_ + +#include +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" + +namespace smac_planner +{ + +/** + * @struct smac_planner::SmootherParams + * @brief Parameters for the smoother cost function + */ +struct SmootherParams +{ + /** + * @brief A constructor for smac_planner::SmootherParams + */ + SmootherParams() + { + } + + /** + * @brief Get params from ROS parameter + * @param node_ Ptr to node + * @param name Name of plugin + */ + void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name) + { + std::string local_name = name + std::string(".smoother.smoother."); + + // Smoother params + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_curve", rclcpp::ParameterValue(1.5)); + node->get_parameter(local_name + "w_curve", curvature_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_cost", rclcpp::ParameterValue(0.0)); + node->get_parameter(local_name + "w_cost", costmap_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_dist", rclcpp::ParameterValue(0.0)); + node->get_parameter(local_name + "w_dist", distance_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_smooth", rclcpp::ParameterValue(15000.0)); + node->get_parameter(local_name + "w_smooth", smooth_weight); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "cost_scaling_factor", rclcpp::ParameterValue(10.0)); + node->get_parameter(local_name + "cost_scaling_factor", costmap_factor); + } + + double smooth_weight{0.0}; + double costmap_weight{0.0}; + double distance_weight{0.0}; + double curvature_weight{0.0}; + double max_curvature{0.0}; + double costmap_factor{0.0}; + double max_time; +}; + +/** + * @struct smac_planner::OptimizerParams + * @brief Parameters for the ceres optimizer + */ +struct OptimizerParams +{ + OptimizerParams() + : debug(false), + max_iterations(50), + max_time(1e4), + param_tol(1e-8), + fn_tol(1e-6), + gradient_tol(1e-10) + { + } + + /** + * @struct AdvancedParams + * @brief Advanced parameters for the ceres optimizer + */ + struct AdvancedParams + { + AdvancedParams() + : min_line_search_step_size(1e-9), + max_num_line_search_step_size_iterations(20), + line_search_sufficient_function_decrease(1e-4), + max_num_line_search_direction_restarts(20), + max_line_search_step_contraction(1e-3), + min_line_search_step_contraction(0.6), + line_search_sufficient_curvature_decrease(0.9), + max_line_search_step_expansion(10) + { + } + + /** + * @brief Get advanced params from ROS parameter + * @param node_ Ptr to node + * @param name Name of plugin + */ + void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name) + { + std::string local_name = name + std::string(".smoother.optimizer.advanced."); + + // Optimizer advanced params + nav2_util::declare_parameter_if_not_declared( + node, local_name + "min_line_search_step_size", + rclcpp::ParameterValue(1e-20)); + node->get_parameter( + local_name + "min_line_search_step_size", + min_line_search_step_size); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_num_line_search_step_size_iterations", + rclcpp::ParameterValue(50)); + node->get_parameter( + local_name + "max_num_line_search_step_size_iterations", + max_num_line_search_step_size_iterations); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "line_search_sufficient_function_decrease", + rclcpp::ParameterValue(1e-20)); + node->get_parameter( + local_name + "line_search_sufficient_function_decrease", + line_search_sufficient_function_decrease); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_num_line_search_direction_restarts", + rclcpp::ParameterValue(10)); + node->get_parameter( + local_name + "max_num_line_search_direction_restarts", + max_num_line_search_direction_restarts); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_line_search_step_expansion", + rclcpp::ParameterValue(50)); + node->get_parameter( + local_name + "max_line_search_step_expansion", + max_line_search_step_expansion); + } + + + double min_line_search_step_size; // Ceres default: 1e-9 + int max_num_line_search_step_size_iterations; // Ceres default: 20 + double line_search_sufficient_function_decrease; // Ceres default: 1e-4 + int max_num_line_search_direction_restarts; // Ceres default: 5 + + double max_line_search_step_contraction; // Ceres default: 1e-3 + double min_line_search_step_contraction; // Ceres default: 0.6 + double line_search_sufficient_curvature_decrease; // Ceres default: 0.9 + int max_line_search_step_expansion; // Ceres default: 10 + }; + + /** + * @brief Get params from ROS parameter + * @param node_ Ptr to node + * @param name Name of plugin + */ + void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name) + { + std::string local_name = name + std::string(".smoother.optimizer."); + + // Optimizer params + nav2_util::declare_parameter_if_not_declared( + node, local_name + "param_tol", rclcpp::ParameterValue(1e-15)); + node->get_parameter(local_name + "param_tol", param_tol); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "fn_tol", rclcpp::ParameterValue(1e-7)); + node->get_parameter(local_name + "fn_tol", fn_tol); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "gradient_tol", rclcpp::ParameterValue(1e-10)); + node->get_parameter(local_name + "gradient_tol", gradient_tol); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_iterations", rclcpp::ParameterValue(500)); + node->get_parameter(local_name + "max_iterations", max_iterations); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_time", rclcpp::ParameterValue(0.100)); + node->get_parameter(local_name + "max_time", max_time); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "debug_optimizer", rclcpp::ParameterValue(false)); + node->get_parameter(local_name + "debug_optimizer", debug); + + advanced.get(node, name); + } + + bool debug; + int max_iterations; // Ceres default: 50 + double max_time; // Ceres default: 1e4 + + double param_tol; // Ceres default: 1e-8 + double fn_tol; // Ceres default: 1e-6 + double gradient_tol; // Ceres default: 1e-10 + + AdvancedParams advanced; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__OPTIONS_HPP_ diff --git a/smac_planner/include/smac_planner/smac_planner.hpp b/smac_planner/include/smac_planner/smac_planner.hpp new file mode 100644 index 00000000000..347bd562c92 --- /dev/null +++ b/smac_planner/include/smac_planner/smac_planner.hpp @@ -0,0 +1,132 @@ +// Copyright (c) 2020, 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 SMAC_PLANNER__SMAC_PLANNER_HPP_ +#define SMAC_PLANNER__SMAC_PLANNER_HPP_ + +#include +#include +#include + +#include "smac_planner/a_star.hpp" +#include "smac_planner/smoother.hpp" +#include "smac_planner/costmap_downsampler.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "tf2/utils.h" + +namespace smac_planner +{ + +class SmacPlanner : public nav2_core::GlobalPlanner +{ +public: + /** + * @brief constructor + */ + SmacPlanner(); + + /** + * @brief destructor + */ + ~SmacPlanner(); + + /** + * @brief Configuring plugin + * @param parent Lifecycle node pointer + * @param name Name of plugin map + * @param tf Shared ptr of TF2 buffer + * @param costmap_ros Costmap2DROS object + */ + void configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; + + /** + * @brief Cleanup lifecycle node + */ + void cleanup() override; + + /** + * @brief Activate lifecycle node + */ + void activate() override; + + /** + * @brief Deactivate lifecycle node + */ + void deactivate() override; + + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @return nav2_msgs::Path of the generated path + */ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) override; + + /** + * @brief Create an Eigen Vector2D of world poses from continuous map coords + * @param mx float of map X coordinate + * @param my float of map Y coordinate + * @param costmap Costmap pointer + * @return Eigen::Vector2d eigen vector of the generated path + */ + Eigen::Vector2d getWorldCoords( + const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap); + + /** + * @brief Create quaternion from A* coord bins + * @param theta continuous bin coordinates angle + * @return quaternion orientation in map frame + */ + geometry_msgs::msg::Quaternion getWorldOrientation(const float & theta); + + /** + * @brief Remove hooking at end of paths + * @param path Path to remove hooking from + */ + void removeHook(std::vector & path); + +protected: + std::unique_ptr> _a_star; + std::unique_ptr _smoother; + rclcpp::Clock::SharedPtr _clock; + rclcpp::Logger _logger{rclcpp::get_logger("SmacPlanner")}; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr _costmap_downsampler; + std::string _global_frame, _name; + float _tolerance; + int _downsampling_factor; + unsigned int _angle_quantizations; + double _angle_bin_size; + bool _downsample_costmap; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + SmootherParams _smoother_params; + OptimizerParams _optimizer_params; + double _max_planning_time; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__SMAC_PLANNER_HPP_ diff --git a/smac_planner/include/smac_planner/smac_planner_2d.hpp b/smac_planner/include/smac_planner/smac_planner_2d.hpp new file mode 100644 index 00000000000..7cf18a673c4 --- /dev/null +++ b/smac_planner/include/smac_planner/smac_planner_2d.hpp @@ -0,0 +1,123 @@ +// Copyright (c) 2020, 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 SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ +#define SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ + +#include +#include +#include + +#include "smac_planner/a_star.hpp" +#include "smac_planner/smoother.hpp" +#include "smac_planner/costmap_downsampler.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "tf2/utils.h" + +namespace smac_planner +{ + +class SmacPlanner2D : public nav2_core::GlobalPlanner +{ +public: + /** + * @brief constructor + */ + SmacPlanner2D(); + + /** + * @brief destructor + */ + ~SmacPlanner2D(); + + /** + * @brief Configuring plugin + * @param parent Lifecycle node pointer + * @param name Name of plugin map + * @param tf Shared ptr of TF2 buffer + * @param costmap_ros Costmap2DROS object + */ + void configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; + + /** + * @brief Cleanup lifecycle node + */ + void cleanup() override; + + /** + * @brief Activate lifecycle node + */ + void activate() override; + + /** + * @brief Deactivate lifecycle node + */ + void deactivate() override; + + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @return nav2_msgs::Path of the generated path + */ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) override; + + /** + * @brief Create an Eigen Vector2D of world poses from continuous map coords + * @param mx float of map X coordinate + * @param my float of map Y coordinate + * @param costmap Costmap pointer + * @return Eigen::Vector2d eigen vector of the generated path + */ + Eigen::Vector2d getWorldCoords( + const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap); + + /** + * @brief Remove hooking at end of paths + * @param path Path to remove hooking from + */ + void removeHook(std::vector & path); + +protected: + std::unique_ptr> _a_star; + std::unique_ptr _smoother; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr _costmap_downsampler; + rclcpp::Clock::SharedPtr _clock; + rclcpp::Logger _logger{rclcpp::get_logger("SmacPlanner2D")}; + std::string _global_frame, _name; + float _tolerance; + int _downsampling_factor; + bool _downsample_costmap; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + SmootherParams _smoother_params; + OptimizerParams _optimizer_params; + double _max_planning_time; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ diff --git a/smac_planner/include/smac_planner/smoother.hpp b/smac_planner/include/smac_planner/smoother.hpp new file mode 100644 index 00000000000..756e6b57939 --- /dev/null +++ b/smac_planner/include/smac_planner/smoother.hpp @@ -0,0 +1,141 @@ +// Copyright (c) 2020, 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 SMAC_PLANNER__SMOOTHER_HPP_ +#define SMAC_PLANNER__SMOOTHER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "smac_planner/types.hpp" +#include "smac_planner/smoother_cost_function.hpp" + +#include "ceres/ceres.h" +#include "Eigen/Core" + +namespace smac_planner +{ + +/** + * @class smac_planner::Smoother + * @brief A Conjugate Gradient 2D path smoother implementation + */ +class Smoother +{ +public: + /** + * @brief A constructor for smac_planner::Smoother + */ + Smoother() {} + + /** + * @brief A destructor for smac_planner::Smoother + */ + ~Smoother() {} + + /** + * @brief Initialization of the smoother + * @param params OptimizerParam struct + */ + void initialize(const OptimizerParams params) + { + _debug = params.debug; + + // General Params + + // 2 most valid options: STEEPEST_DESCENT, NONLINEAR_CONJUGATE_GRADIENT + _options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT; + _options.line_search_type = ceres::WOLFE; + _options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE; + _options.line_search_interpolation_type = ceres::CUBIC; + + _options.max_num_iterations = params.max_iterations; + _options.max_solver_time_in_seconds = params.max_time; + + _options.function_tolerance = params.fn_tol; + _options.gradient_tolerance = params.gradient_tol; + _options.parameter_tolerance = params.param_tol; + + _options.min_line_search_step_size = params.advanced.min_line_search_step_size; + _options.max_num_line_search_step_size_iterations = + params.advanced.max_num_line_search_step_size_iterations; + _options.line_search_sufficient_function_decrease = + params.advanced.line_search_sufficient_function_decrease; + _options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction; + _options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction; + _options.max_num_line_search_direction_restarts = + params.advanced.max_num_line_search_direction_restarts; + _options.line_search_sufficient_curvature_decrease = + params.advanced.line_search_sufficient_curvature_decrease; + _options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion; + + if (_debug) { + _options.minimizer_progress_to_stdout = true; + } else { + _options.logging_type = ceres::SILENT; + } + } + + /** + * @brief Smoother method + * @param path Reference to path + * @param costmap Pointer to minimal costmap + * @param smoother parameters weights + * @return If smoothing was successful + */ + bool smooth( + std::vector & path, + nav2_costmap_2d::Costmap2D * costmap, + const SmootherParams & params) + { + _options.max_solver_time_in_seconds = params.max_time; + + double parameters[path.size() * 2]; // NOLINT + for (uint i = 0; i != path.size(); i++) { + parameters[2 * i] = path[i][0]; + parameters[2 * i + 1] = path[i][1]; + } + + ceres::GradientProblemSolver::Summary summary; + ceres::GradientProblem problem(new UnconstrainedSmootherCostFunction(&path, costmap, params)); + ceres::Solve(_options, problem, parameters, &summary); + + if (_debug) { + std::cout << summary.FullReport() << '\n'; + } + + if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost <= 0.0) { + return false; + } + + for (uint i = 0; i != path.size(); i++) { + path[i][0] = parameters[2 * i]; + path[i][1] = parameters[2 * i + 1]; + } + + return true; + } + +private: + bool _debug; + ceres::GradientProblemSolver::Options _options; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__SMOOTHER_HPP_ diff --git a/smac_planner/include/smac_planner/smoother_cost_function.hpp b/smac_planner/include/smac_planner/smoother_cost_function.hpp new file mode 100644 index 00000000000..82d1c88a6dc --- /dev/null +++ b/smac_planner/include/smac_planner/smoother_cost_function.hpp @@ -0,0 +1,515 @@ +// Copyright (c) 2020, 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 SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ +#define SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ceres/ceres.h" +#include "Eigen/Core" +#include "smac_planner/types.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "smac_planner/options.hpp" + +#define EPSILON 0.0001 + +namespace smac_planner +{ + +/** + * @struct smac_planner::UnconstrainedSmootherCostFunction + * @brief Cost function for path smoothing with multiple terms + * including curvature, smoothness, collision, and avoid obstacles. + */ +class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction +{ +public: + /** + * @brief A constructor for smac_planner::UnconstrainedSmootherCostFunction + * @param original_path Original unsmoothed path to smooth + * @param costmap A costmap to get values for collision and obstacle avoidance + */ + UnconstrainedSmootherCostFunction( + std::vector * original_path, + nav2_costmap_2d::Costmap2D * costmap, + const SmootherParams & params) + : _original_path(original_path), + _num_params(2 * original_path->size()), + _costmap(costmap), + _params(params) + { + } + + /** + * @struct CurvatureComputations + * @brief Cache common computations between the curvature terms to minimize recomputations + */ + struct CurvatureComputations + { + /** + * @brief A constructor for smac_planner::CurvatureComputations + */ + CurvatureComputations() + { + valid = true; + } + + bool valid; + /** + * @brief Check if result is valid for penalty + * @return is valid (non-nan, non-inf, and turning angle > max) + */ + bool isValid() + { + return valid; + } + + Eigen::Vector2d delta_xi{0.0, 0.0}; + Eigen::Vector2d delta_xi_p{0.0, 0.0}; + double delta_xi_norm{0}; + double delta_xi_p_norm{0}; + double delta_phi_i{0}; + double turning_rad{0}; + double ki_minus_kmax{0}; + }; + + /** + * @brief Smoother cost function evaluation + * @param parameters X,Y pairs of points + * @param cost total cost of path + * @param gradient of path at each X,Y pair from cost function derived analytically + * @return if successful in computing values + */ + virtual bool Evaluate( + const double * parameters, + double * cost, + double * gradient) const + { + Eigen::Vector2d xi; + Eigen::Vector2d xi_p1; + Eigen::Vector2d xi_m1; + uint x_index, y_index; + cost[0] = 0.0; + double cost_raw = 0.0; + double grad_x_raw = 0.0; + double grad_y_raw = 0.0; + unsigned int mx, my; + bool valid_coords = true; + double costmap_cost = 0.0; + + // cache some computations between the residual and jacobian + CurvatureComputations curvature_params; + + for (int i = 0; i != NumParameters() / 2; i++) { + x_index = 2 * i; + y_index = 2 * i + 1; + gradient[x_index] = 0.0; + gradient[y_index] = 0.0; + if (i < 1 || i >= NumParameters() / 2 - 1) { + continue; + } + + xi = Eigen::Vector2d(parameters[x_index], parameters[y_index]); + xi_p1 = Eigen::Vector2d(parameters[x_index + 2], parameters[y_index + 2]); + xi_m1 = Eigen::Vector2d(parameters[x_index - 2], parameters[y_index - 2]); + + // compute cost + addSmoothingResidual(_params.smooth_weight, xi, xi_p1, xi_m1, cost_raw); + addCurvatureResidual(_params.curvature_weight, xi, xi_p1, xi_m1, curvature_params, cost_raw); + addDistanceResidual(_params.distance_weight, xi, _original_path->at(i), cost_raw); + + if (valid_coords = _costmap->worldToMap(xi[0], xi[1], mx, my)) { + costmap_cost = _costmap->getCost(mx, my); + addCostResidual(_params.costmap_weight, costmap_cost, cost_raw); + } + + if (gradient != NULL) { + // compute gradient + gradient[x_index] = 0.0; + gradient[y_index] = 0.0; + addSmoothingJacobian(_params.smooth_weight, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw); + addCurvatureJacobian( + _params.curvature_weight, xi, xi_p1, xi_m1, curvature_params, + grad_x_raw, grad_y_raw); + addDistanceJacobian( + _params.distance_weight, xi, _original_path->at( + i), grad_x_raw, grad_y_raw); + + if (valid_coords) { + addCostJacobian(_params.costmap_weight, mx, my, costmap_cost, grad_x_raw, grad_y_raw); + } + + gradient[x_index] = grad_x_raw; + gradient[y_index] = grad_y_raw; + } + } + + cost[0] = cost_raw; + + return true; + } + + /** + * @brief Get number of parameter blocks + * @return Number of parameters in cost function + */ + virtual int NumParameters() const {return _num_params;} + +protected: + /** + * @brief Cost function term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param r Residual (cost) of term + */ + inline void addSmoothingResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & r) const + { + r += weight * ( + pt_p.dot(pt_p) - + 4 * pt_p.dot(pt) + + 2 * pt_p.dot(pt_m) + + 4 * pt.dot(pt) - + 4 * pt.dot(pt_m) + + pt_m.dot(pt_m)); // objective function value + } + + /** + * @brief Cost function derivative term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addSmoothingJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & j0, + double & j1) const + { + j0 += weight * + (-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative + j1 += weight * + (-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative + } + + /** + * @brief Cost function term for maximum curved paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + * @param r Residual (cost) of term + */ + inline void addCurvatureResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params, + double & r) const + { + curvature_params.valid = true; + curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]); + curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]); + curvature_params.delta_xi_norm = curvature_params.delta_xi.norm(); + curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm(); + + if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON || + std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) || + std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm)) + { + // ensure we have non-nan values returned + curvature_params.valid = false; + return; + } + + const double & delta_xi_by_xi_p = + curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm; + double projection = + curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p; + if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) { + projection = 1.0; + } + + curvature_params.delta_phi_i = std::acos(projection); + curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm; + + curvature_params.ki_minus_kmax = curvature_params.turning_rad - _params.max_curvature; + + if (curvature_params.ki_minus_kmax <= EPSILON) { + // Quadratic penalty need not apply + curvature_params.valid = false; + return; + } + + r += weight * + curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; // objective function value + } + + /** + * @brief Cost function derivative term for maximum curvature paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct with cached values to speed up Jacobian computation + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addCurvatureJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & /*pt_m*/, + CurvatureComputations & curvature_params, + double & j0, + double & j1) const + { + if (!curvature_params.isValid()) { + return; + } + + const double & partial_delta_phi_i_wrt_cost_delta_phi_i = + -1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2)); + // const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0); + auto neg_pt_plus = -1 * pt_p; + Eigen::Vector2d p1 = normalizedOrthogonalComplement( + pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm); + Eigen::Vector2d p2 = normalizedOrthogonalComplement( + neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm); + + const double & u = 2 * curvature_params.ki_minus_kmax; + const double & common_prefix = + (1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i; + const double & common_suffix = curvature_params.delta_phi_i / + (curvature_params.delta_xi_norm * curvature_params.delta_xi_norm); + + const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi / + curvature_params.delta_xi_norm; + + const Eigen::Vector2d jacobian = u * + (common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_im1 = u * + (common_prefix * p2 + (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1); + + // Old formulation we may require again. + // j0 += weight * + // (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]); + // j1 += weight * + // (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]); + + j0 += weight * jacobian[0]; // xi x component of partial-derivative + j1 += weight * jacobian[1]; // xi x component of partial-derivative + } + + /** + * @brief Cost function derivative term for steering away changes in pose + * @param weight Weight to apply to function + * @param xi Point Xi for evaluation + * @param xi_original original point Xi for evaluation + * @param r Residual (cost) of term + */ + inline void addDistanceResidual( + const double & weight, + const Eigen::Vector2d & xi, + const Eigen::Vector2d & xi_original, + double & r) const + { + r += weight * (xi - xi_original).dot(xi - xi_original); // objective function value + } + + /** + * @brief Cost function derivative term for steering away changes in pose + * @param weight Weight to apply to function + * @param xi Point Xi for evaluation + * @param xi_original original point Xi for evaluation + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addDistanceJacobian( + const double & weight, + const Eigen::Vector2d & xi, + const Eigen::Vector2d & xi_original, + double & j0, + double & j1) const + { + j0 += weight * 2 * (xi[0] - xi_original[0]); // xi y component of partial-derivative + j1 += weight * 2 * (xi[1] - xi_original[1]); // xi y component of partial-derivative + } + + + /** + * @brief Cost function term for steering away from costs + * @param weight Weight to apply to function + * @param value Point Xi's cost' + * @param params computed values to reduce overhead + * @param r Residual (cost) of term + */ + inline void addCostResidual( + const double & weight, + const double & value, + double & r) const + { + if (value == FREE) { + return; + } + + r += weight * value * value; // objective function value + } + + /** + * @brief Cost function derivative term for steering away from costs + * @param weight Weight to apply to function + * @param mx Point Xi's x coordinate in map frame + * @param mx Point Xi's y coordinate in map frame + * @param value Point Xi's cost' + * @param params computed values to reduce overhead + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addCostJacobian( + const double & weight, + const unsigned int & mx, + const unsigned int & my, + const double & value, + double & j0, + double & j1) const + { + if (value == FREE) { + return; + } + + const Eigen::Vector2d grad = getCostmapGradient(mx, my); + const double common_prefix = -2.0 * _params.costmap_factor * weight * value * value; + + j0 += common_prefix * grad[0]; // xi x component of partial-derivative + j1 += common_prefix * grad[1]; // xi y component of partial-derivative + } + + /** + * @brief Computing the gradient of the costmap using + * the 2 point numerical differentiation method + * @param mx Point Xi's x coordinate in map frame + * @param mx Point Xi's y coordinate in map frame + * @param params Params reference to store gradients + */ + inline Eigen::Vector2d getCostmapGradient( + const unsigned int mx, + const unsigned int my) const + { + // find unit vector that describes that direction + // via 7 point taylor series approximation for gradient at Xi + Eigen::Vector2d gradient; + + double l_1 = 0.0; + double l_2 = 0.0; + double l_3 = 0.0; + double r_1 = 0.0; + double r_2 = 0.0; + double r_3 = 0.0; + + if (mx < _costmap->getSizeInCellsX()) { + r_1 = static_cast(_costmap->getCost(mx + 1, my)); + } + if (mx + 1 < _costmap->getSizeInCellsX()) { + r_2 = static_cast(_costmap->getCost(mx + 2, my)); + } + if (mx + 2 < _costmap->getSizeInCellsX()) { + r_3 = static_cast(_costmap->getCost(mx + 3, my)); + } + + if (mx > 0) { + l_1 = static_cast(_costmap->getCost(mx - 1, my)); + } + if (mx - 1 > 0) { + l_2 = static_cast(_costmap->getCost(mx - 2, my)); + } + if (mx - 2 > 0) { + l_3 = static_cast(_costmap->getCost(mx - 3, my)); + } + + gradient[1] = (45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3) / 60; + + if (my < _costmap->getSizeInCellsY()) { + r_1 = static_cast(_costmap->getCost(mx, my + 1)); + } + if (my + 1 < _costmap->getSizeInCellsY()) { + r_2 = static_cast(_costmap->getCost(mx, my + 2)); + } + if (my + 2 < _costmap->getSizeInCellsY()) { + r_3 = static_cast(_costmap->getCost(mx, my + 3)); + } + + if (my > 0) { + l_1 = static_cast(_costmap->getCost(mx, my - 1)); + } + if (my - 1 > 0) { + l_2 = static_cast(_costmap->getCost(mx, my - 2)); + } + if (my - 2 > 0) { + l_3 = static_cast(_costmap->getCost(mx, my - 3)); + } + + gradient[0] = (45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3) / 60; + + gradient.normalize(); + return gradient; + } + + /** + * @brief Computing the normalized orthogonal component of 2 vectors + * @param a Vector + * @param b Vector + * @param norm a Vector's norm + * @param norm b Vector's norm + * @return Normalized vector of orthogonal components + */ + inline Eigen::Vector2d normalizedOrthogonalComplement( + const Eigen::Vector2d & a, + const Eigen::Vector2d & b, + const double & a_norm, + const double & b_norm) const + { + return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm); + } + + std::vector * _original_path{nullptr}; + int _num_params; + nav2_costmap_2d::Costmap2D * _costmap{nullptr}; + SmootherParams _params; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ diff --git a/smac_planner/include/smac_planner/types.hpp b/smac_planner/include/smac_planner/types.hpp new file mode 100644 index 00000000000..e39564d333b --- /dev/null +++ b/smac_planner/include/smac_planner/types.hpp @@ -0,0 +1,42 @@ +// Copyright (c) 2020, 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 SMAC_PLANNER__TYPES_HPP_ +#define SMAC_PLANNER__TYPES_HPP_ + +#include +#include + +namespace smac_planner +{ + +typedef std::pair NodeHeuristicPair; + +/** + * @struct smac_planner::SearchInfo + * @brief Search properties and penalties + */ +struct SearchInfo +{ + float minimum_turning_radius; + float non_straight_penalty; + float change_penalty; + float reverse_penalty; + float cost_penalty; + float analytic_expansion_ratio; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__TYPES_HPP_ diff --git a/smac_planner/package.xml b/smac_planner/package.xml new file mode 100644 index 00000000000..34d0e42ec97 --- /dev/null +++ b/smac_planner/package.xml @@ -0,0 +1,38 @@ + + + + smac_planner + 0.4.7 + Smac global planning plugin + Steve Macenski + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_lifecycle + visualization_msgs + nav2_util + nav2_msgs + nav_msgs + geometry_msgs + builtin_interfaces + nav2_common + tf2_ros + nav2_costmap_2d + nav2_core + pluginlib + libceres-dev + eigen3_cmake_module + eigen + ompl + + ament_lint_common + ament_lint_auto + + + ament_cmake + + + diff --git a/smac_planner/smac_plugin.xml b/smac_planner/smac_plugin.xml new file mode 100644 index 00000000000..09f17b666fc --- /dev/null +++ b/smac_planner/smac_plugin.xml @@ -0,0 +1,5 @@ + + + SE2 version of the SMAC planner + + diff --git a/smac_planner/smac_plugin_2d.xml b/smac_planner/smac_plugin_2d.xml new file mode 100644 index 00000000000..3845a0ffe2a --- /dev/null +++ b/smac_planner/smac_plugin_2d.xml @@ -0,0 +1,5 @@ + + + 2D version of the SMAC Planner + + diff --git a/smac_planner/src/a_star.cpp b/smac_planner/src/a_star.cpp new file mode 100644 index 00000000000..a8432b6c470 --- /dev/null +++ b/smac_planner/src/a_star.cpp @@ -0,0 +1,651 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// +// 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 +#include +#include +#include +#include +#include + +#include "smac_planner/a_star.hpp" +using namespace std::chrono; // NOLINT + +namespace smac_planner +{ + +template +AStarAlgorithm::AStarAlgorithm( + const MotionModel & motion_model, + const SearchInfo & search_info) +: _traverse_unknown(true), + _max_iterations(0), + _x_size(0), + _y_size(0), + _search_info(search_info), + _goal_coordinates(Coordinates()), + _start(nullptr), + _goal(nullptr), + _motion_model(motion_model), + _collision_checker(nullptr) +{ + _graph.reserve(100000); +} + +template +AStarAlgorithm::~AStarAlgorithm() +{ +} + +template +void AStarAlgorithm::initialize( + const bool & allow_unknown, + int & max_iterations, + const int & max_on_approach_iterations) +{ + _traverse_unknown = allow_unknown; + _max_iterations = max_iterations; + _max_on_approach_iterations = max_on_approach_iterations; +} + +template<> +void AStarAlgorithm::createGraph( + const unsigned int & x_size, + const unsigned int & y_size, + const unsigned int & dim_3_size, + nav2_costmap_2d::Costmap2D * & costmap) +{ + if (dim_3_size != 1) { + throw std::runtime_error("Node type Node2D cannot be given non-1 dim 3 quantization."); + } + _costmap = costmap; + _dim3_size = dim_3_size; // 2D search MUST be 2D, not 3D or SE2. + clearGraph(); + + if (getSizeX() != x_size || getSizeY() != y_size) { + _x_size = x_size; + _y_size = y_size; + Node2D::initNeighborhood(_x_size, _motion_model); + } +} + +template<> +void AStarAlgorithm::createGraph( + const unsigned int & x_size, + const unsigned int & y_size, + const unsigned int & dim_3_size, + nav2_costmap_2d::Costmap2D * & costmap) +{ + _costmap = costmap; + _collision_checker = GridCollisionChecker(costmap); + _collision_checker.setFootprint(_footprint, _is_radius_footprint); + + _dim3_size = dim_3_size; + unsigned int index; + clearGraph(); + + if (getSizeX() != x_size || getSizeY() != y_size) { + _x_size = x_size; + _y_size = y_size; + NodeSE2::initMotionModel(_motion_model, _x_size, _y_size, _dim3_size, _search_info); + } +} + +template +void AStarAlgorithm::setFootprint(nav2_costmap_2d::Footprint footprint, bool use_radius) +{ + _footprint = footprint; + _is_radius_footprint = use_radius; +} + +template<> +typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( + const unsigned int & index) +{ + return &(_graph.emplace(index, Node2D(_costmap->getCharMap()[index], index)).first->second); +} + +template<> +typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( + const unsigned int & index) +{ + return &(_graph.emplace(index, NodeSE2(index)).first->second); +} + +template<> +void AStarAlgorithm::setStart( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3) +{ + if (dim_3 != 0) { + throw std::runtime_error("Node type Node2D cannot be given non-zero starting dim 3."); + } + _start = addToGraph(Node2D::getIndex(mx, my, getSizeX())); +} + +template<> +void AStarAlgorithm::setStart( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3) +{ + _start = addToGraph(NodeSE2::getIndex(mx, my, dim_3, getSizeX(), getSizeDim3())); + _start->setPose( + Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3))); +} + +template<> +void AStarAlgorithm::setGoal( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3) +{ + if (dim_3 != 0) { + throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); + } + + _goal = addToGraph(Node2D::getIndex(mx, my, getSizeX())); + _goal_coordinates = Node2D::Coordinates(mx, my); +} + +template<> +void AStarAlgorithm::setGoal( + const unsigned int & mx, + const unsigned int & my, + const unsigned int & dim_3) +{ + _goal = addToGraph(NodeSE2::getIndex(mx, my, dim_3, getSizeX(), getSizeDim3())); + _goal_coordinates = NodeSE2::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3)); + _goal->setPose(_goal_coordinates); + + NodeSE2::computeWavefrontHeuristic( + _costmap, + static_cast(getStart()->pose.x), + static_cast(getStart()->pose.y), + mx, my); +} + +template +bool AStarAlgorithm::areInputsValid() +{ + // Check if graph was filled in + if (_graph.empty()) { + throw std::runtime_error("Failed to compute path, no costmap given."); + } + + // Check if points were filled in + if (!_start || !_goal) { + throw std::runtime_error("Failed to compute path, no valid start or goal given."); + } + + // Check if ending point is valid + if (getToleranceHeuristic() < 0.001 && + !_goal->isNodeValid(_traverse_unknown, _collision_checker)) + { + throw std::runtime_error("Failed to compute path, goal is occupied with no tolerance."); + } + + // Check if starting point is valid + if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) { + throw std::runtime_error("Starting point in lethal space! Cannot create feasible plan."); + } + + return true; +} + +template +bool AStarAlgorithm::createPath( + CoordinateVector & path, int & iterations, + const float & tolerance) +{ + _tolerance = tolerance * NodeT::neutral_cost; + _best_heuristic_node = {std::numeric_limits::max(), 0}; + clearQueue(); + + if (!areInputsValid()) { + return false; + } + + // 0) Add starting point to the open set + addNode(0.0, getStart()); + getStart()->setAccumulatedCost(0.0); + + // Optimization: preallocate all variables + NodePtr current_node = nullptr; + NodePtr neighbor = nullptr; + float g_cost = 0.0; + NodeVector neighbors; + int approach_iterations = 0; + NeighborIterator neighbor_iterator; + int analytic_iterations = 0; + int closest_distance = std::numeric_limits::max(); + + // Given an index, return a node ptr reference if its collision-free and valid + const unsigned int max_index = getSizeX() * getSizeY() * getSizeDim3(); + NodeGetter neighborGetter = + [&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool + { + if (index < 0 || index >= max_index) { + return false; + } + + neighbor_rtn = addToGraph(index); + return true; + }; + + while (iterations < getMaxIterations() && !_queue.empty()) { + // 1) Pick Nbest from O s.t. min(f(Nbest)), remove from queue + current_node = getNextNode(); + + // We allow for nodes to be queued multiple times in case + // shorter paths result in it, but we can visit only once + if (current_node->wasVisited()) { + continue; + } + + iterations++; + + // 2) Mark Nbest as visited + current_node->visited(); + + // 2.a) Use an analytic expansion (if available) to generate a path + // to the goal. + NodePtr result = tryAnalyticExpansion( + current_node, neighborGetter, analytic_iterations, + closest_distance); + if (result != nullptr) { + current_node = result; + } + + // 3) Check if we're at the goal, backtrace if required + if (isGoal(current_node)) { + return backtracePath(current_node, path); + } else if (_best_heuristic_node.first < getToleranceHeuristic()) { + // Optimization: Let us find when in tolerance and refine within reason + approach_iterations++; + if (approach_iterations > getOnApproachMaxIterations() || + iterations + 1 == getMaxIterations()) + { + NodePtr node = &_graph.at(_best_heuristic_node.second); + return backtracePath(node, path); + } + } + + // 4) Expand neighbors of Nbest not visited + neighbors.clear(); + NodeT::getNeighbors( + current_node, neighborGetter, _collision_checker, _traverse_unknown, neighbors); + + for (neighbor_iterator = neighbors.begin(); + neighbor_iterator != neighbors.end(); ++neighbor_iterator) + { + neighbor = *neighbor_iterator; + + // 4.1) Compute the cost to go to this node + g_cost = getAccumulatedCost(current_node) + getTraversalCost(current_node, neighbor); + + // 4.2) If this is a lower cost than prior, we set this as the new cost and new approach + if (g_cost < getAccumulatedCost(neighbor)) { + neighbor->setAccumulatedCost(g_cost); + neighbor->parent = current_node; + + // 4.3) If not in queue or visited, add it, `getNeighbors()` handles + neighbor->queued(); + addNode(g_cost + getHeuristicCost(neighbor), neighbor); + } + } + } + + return false; +} + +template +bool AStarAlgorithm::isGoal(NodePtr & node) +{ + return node == getGoal(); +} + +template<> +AStarAlgorithm::NodePtr AStarAlgorithm::getAnalyticPath( + const NodePtr & node, + const NodeGetter & node_getter) +{ + ompl::base::ScopedState<> from(node->motion_table.state_space), to( + node->motion_table.state_space), s(node->motion_table.state_space); + const NodeSE2::Coordinates & node_coords = node->pose; + from[0] = node_coords.x; + from[1] = node_coords.y; + from[2] = node_coords.theta * node->motion_table.bin_size; + to[0] = _goal_coordinates.x; + to[1] = _goal_coordinates.y; + to[2] = _goal_coordinates.theta * node->motion_table.bin_size; + + float d = node->motion_table.state_space->distance(from(), to()); + NodePtr prev(node); + // A move of sqrt(2) is guaranteed to be in a new cell + static const float sqrt_2 = std::sqrt(2.); + unsigned int num_intervals = std::floor(d / sqrt_2); + + using PossibleNode = std::pair; + std::vector possible_nodes; + possible_nodes.reserve(num_intervals - 1); // We won't store this node or the goal + std::vector reals; + // Pre-allocate + unsigned int index = 0; + NodePtr next(nullptr); + float angle = 0.0; + Coordinates proposed_coordinates; + // Don't generate the first point because we are already there! + // And the last point is the goal, so ignore it too! + for (float i = 1; i < num_intervals; i++) { + node->motion_table.state_space->interpolate(from(), to(), i / num_intervals, s()); + reals = s.reals(); + angle = reals[2] / node->motion_table.bin_size; + while (angle >= node->motion_table.num_angle_quantization_float) { + angle -= node->motion_table.num_angle_quantization_float; + } + while (angle < 0.0) { + angle += node->motion_table.num_angle_quantization_float; + } + // Turn the pose into a node, and check if it is valid + index = NodeSE2::getIndex( + static_cast(reals[0]), + static_cast(reals[1]), + static_cast(angle)); + // Get the node from the graph + if (node_getter(index, next)) { + Coordinates initial_node_coords = next->pose; + proposed_coordinates = {static_cast(reals[0]), static_cast(reals[1]), angle}; + next->setPose(proposed_coordinates); + if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) { + // Save the node, and its previous coordinates in case we need to abort + possible_nodes.emplace_back(next, initial_node_coords); + prev = next; + } else { + next->setPose(initial_node_coords); + for (const auto & node_pose : possible_nodes) { + const auto & n = node_pose.first; + n->setPose(node_pose.second); + } + return NodePtr(nullptr); + } + } else { + // Abort + for (const auto & node_pose : possible_nodes) { + const auto & n = node_pose.first; + n->setPose(node_pose.second); + } + return NodePtr(nullptr); + } + } + // Legitimate path - set the parent relationships - poses already set + prev = node; + for (const auto & node_pose : possible_nodes) { + const auto & n = node_pose.first; + if (!n->wasVisited() && n->getIndex() != _goal->getIndex()) { + // Make sure this node has not been visited by the regular algorithm. + // If it has been, there is the (slight) chance that it is in the path we are expanding + // from, so we should skip it. + // Skipping to the next node will still create a kinematically feasible path. + n->parent = prev; + n->visited(); + prev = n; + } + } + if (_goal != prev) { + _goal->parent = prev; + _goal->visited(); + } + return _goal; +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::getAnalyticPath( + const NodePtr & node, + const NodeGetter & node_getter) +{ + return NodePtr(nullptr); +} + +template<> +bool AStarAlgorithm::backtracePath(NodePtr & node, CoordinateVector & path) +{ + if (!node->parent) { + return false; + } + + NodePtr current_node = node; + + while (current_node->parent) { + path.push_back( + Node2D::getCoords( + current_node->getIndex(), getSizeX(), getSizeDim3())); + current_node = current_node->parent; + } + + return path.size() > 1; +} + +template<> +bool AStarAlgorithm::backtracePath(NodePtr & node, CoordinateVector & path) +{ + if (!node->parent) { + return false; + } + + NodePtr current_node = node; + + while (current_node->parent) { + path.push_back(current_node->pose); + current_node = current_node->parent; + } + + return path.size() > 1; +} + +template +typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() +{ + return _start; +} + +template +typename AStarAlgorithm::NodePtr & AStarAlgorithm::getGoal() +{ + return _goal; +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() +{ + NodeBasic node = _queue.top().second; + _queue.pop(); + return node.graph_node_ptr; +} + +template<> +typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() +{ + NodeBasic node = _queue.top().second; + _queue.pop(); + + if (!node.graph_node_ptr->wasVisited()) { + node.graph_node_ptr->pose = node.pose; + } + + return node.graph_node_ptr; +} + +template +void AStarAlgorithm::addNode(const float cost, NodePtr & node) +{ + NodeBasic queued_node(node->getIndex()); + queued_node.graph_node_ptr = node; + _queue.emplace(cost, queued_node); +} + +template<> +void AStarAlgorithm::addNode(const float cost, NodePtr & node) +{ + NodeBasic queued_node(node->getIndex()); + queued_node.pose = node->pose; + queued_node.graph_node_ptr = node; + _queue.emplace(cost, queued_node); +} + +template +float AStarAlgorithm::getTraversalCost( + NodePtr & current_node, + NodePtr & new_node) +{ + return current_node->getTraversalCost(new_node); +} + +template +float & AStarAlgorithm::getAccumulatedCost(NodePtr & node) +{ + return node->getAccumulatedCost(); +} + +template +float AStarAlgorithm::getHeuristicCost(const NodePtr & node) +{ + const Coordinates node_coords = + NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); + float heuristic = NodeT::getHeuristicCost( + node_coords, _goal_coordinates); + + if (heuristic < _best_heuristic_node.first) { + _best_heuristic_node = {heuristic, node->getIndex()}; + } + + return heuristic; +} + +template +void AStarAlgorithm::clearQueue() +{ + NodeQueue q; + std::swap(_queue, q); +} + +template +void AStarAlgorithm::clearGraph() +{ + Graph g; + g.reserve(100000); + std::swap(_graph, g); +} + +template +int & AStarAlgorithm::getMaxIterations() +{ + return _max_iterations; +} + +template +int & AStarAlgorithm::getOnApproachMaxIterations() +{ + return _max_on_approach_iterations; +} + +template +float & AStarAlgorithm::getToleranceHeuristic() +{ + return _tolerance; +} + +template +unsigned int & AStarAlgorithm::getSizeX() +{ + return _x_size; +} + +template +unsigned int & AStarAlgorithm::getSizeY() +{ + return _y_size; +} + +template +unsigned int & AStarAlgorithm::getSizeDim3() +{ + return _dim3_size; +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::tryAnalyticExpansion( + const NodePtr & current_node, const NodeGetter & getter, int & analytic_iterations, + int & closest_distance) +{ + if (_motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP) { + // This must be a NodeSE2 node if we are using these motion models + + // See if we are closer and should be expanding more often + const Coordinates node_coords = + NodeT::getCoords(current_node->getIndex(), getSizeX(), getSizeDim3()); + closest_distance = + std::min( + closest_distance, + static_cast(NodeT::getHeuristicCost( + node_coords, + _goal_coordinates) / NodeT::neutral_cost) + ); + // We want to expand at a rate of d/expansion_ratio, + // but check to see if we are so close that we would be expanding every iteration + // If so, limit it to the expansion ratio (rounded up) + int desired_iterations = std::max( + static_cast(closest_distance / _search_info.analytic_expansion_ratio), + static_cast(std::ceil(_search_info.analytic_expansion_ratio)) + ); + // If we are closer now, we should update the target number of iterations to go + analytic_iterations = + std::min(analytic_iterations, desired_iterations); + + // Always run the expansion on the first run in case there is a + // trivial path to be found + if (analytic_iterations <= 0) { + // Reset the counter, and try the analytic path expansion + analytic_iterations = desired_iterations; + return getAnalyticPath(current_node, getter); + } + analytic_iterations--; + } + // No valid motion model - return nullptr + return NodePtr(nullptr); +} + +// Instantiate algorithm for the supported template types +template class AStarAlgorithm; +template class AStarAlgorithm; + +} // namespace smac_planner diff --git a/smac_planner/src/costmap_downsampler.cpp b/smac_planner/src/costmap_downsampler.cpp new file mode 100644 index 00000000000..025a30b351a --- /dev/null +++ b/smac_planner/src/costmap_downsampler.cpp @@ -0,0 +1,142 @@ +// Copyright (c) 2020, Carlos Luis +// Copyright (c) 2020, 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 "smac_planner/costmap_downsampler.hpp" + +#include +#include +#include + +namespace smac_planner +{ + +CostmapDownsampler::CostmapDownsampler() +: _costmap(nullptr), + _downsampled_costmap(nullptr), + _downsampled_costmap_pub(nullptr) +{ +} + +CostmapDownsampler::~CostmapDownsampler() +{ +} + +void CostmapDownsampler::on_configure( + const nav2_util::LifecycleNode::SharedPtr & node, + const std::string & global_frame, + const std::string & topic_name, + nav2_costmap_2d::Costmap2D * const costmap, + const unsigned int & downsampling_factor) +{ + _costmap = costmap; + _downsampling_factor = downsampling_factor; + updateCostmapSize(); + + _downsampled_costmap = std::make_unique( + _downsampled_size_x, _downsampled_size_y, _downsampled_resolution, + _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN); + + _downsampled_costmap_pub = std::make_unique( + node, _downsampled_costmap.get(), global_frame, topic_name, false); +} + +void CostmapDownsampler::on_activate() +{ + _downsampled_costmap_pub->on_activate(); +} + +void CostmapDownsampler::on_deactivate() +{ + _downsampled_costmap_pub->on_deactivate(); +} + +void CostmapDownsampler::on_cleanup() +{ + _costmap = nullptr; + _downsampled_costmap.reset(); + _downsampled_costmap_pub.reset(); +} + +nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample( + const unsigned int & downsampling_factor) +{ + _downsampling_factor = downsampling_factor; + updateCostmapSize(); + + // Adjust costmap size if needed + if (_downsampled_costmap->getSizeInCellsX() != _downsampled_size_x || + _downsampled_costmap->getSizeInCellsY() != _downsampled_size_y || + _downsampled_costmap->getResolution() != _downsampled_resolution) + { + resizeCostmap(); + } + + // Assign costs + for (uint i = 0; i < _downsampled_size_x; ++i) { + for (uint j = 0; j < _downsampled_size_y; ++j) { + setCostOfCell(i, j); + } + } + + _downsampled_costmap_pub->publishCostmap(); + return _downsampled_costmap.get(); +} + +void CostmapDownsampler::updateCostmapSize() +{ + _size_x = _costmap->getSizeInCellsX(); + _size_y = _costmap->getSizeInCellsY(); + _downsampled_size_x = ceil(static_cast(_size_x) / _downsampling_factor); + _downsampled_size_y = ceil(static_cast(_size_y) / _downsampling_factor); + _downsampled_resolution = _downsampling_factor * _costmap->getResolution(); +} + +void CostmapDownsampler::resizeCostmap() +{ + _downsampled_costmap->resizeMap( + _downsampled_size_x, + _downsampled_size_y, + _downsampled_resolution, + _costmap->getOriginX(), + _costmap->getOriginY()); +} + +void CostmapDownsampler::setCostOfCell( + const unsigned int & new_mx, + const unsigned int & new_my) +{ + unsigned int mx, my; + unsigned char cost = 0; + unsigned int x_offset = new_mx * _downsampling_factor; + unsigned int y_offset = new_my * _downsampling_factor; + + for (uint i = 0; i < _downsampling_factor; ++i) { + mx = x_offset + i; + if (mx >= _size_x) { + continue; + } + for (uint j = 0; j < _downsampling_factor; ++j) { + my = y_offset + j; + if (my >= _size_y) { + continue; + } + cost = std::max(cost, _costmap->getCost(mx, my)); + } + } + + _downsampled_costmap->setCost(new_mx, new_my, cost); +} + +} // namespace smac_planner diff --git a/smac_planner/src/node_2d.cpp b/smac_planner/src/node_2d.cpp new file mode 100644 index 00000000000..b6afd5b944b --- /dev/null +++ b/smac_planner/src/node_2d.cpp @@ -0,0 +1,151 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// +// 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 "smac_planner/node_2d.hpp" + +#include +#include + +namespace smac_planner +{ + +// defining static member for all instance to share +std::vector Node2D::_neighbors_grid_offsets; +double Node2D::neutral_cost = 50.0; + +Node2D::Node2D(unsigned char & cost_in, const unsigned int index) +: parent(nullptr), + _cell_cost(static_cast(cost_in)), + _accumulated_cost(std::numeric_limits::max()), + _index(index), + _was_visited(false), + _is_queued(false) +{ +} + +Node2D::~Node2D() +{ + parent = nullptr; +} + +void Node2D::reset(const unsigned char & cost) +{ + parent = nullptr; + _cell_cost = static_cast(cost); + _accumulated_cost = std::numeric_limits::max(); + _was_visited = false; + _is_queued = false; +} + +bool Node2D::isNodeValid( + const bool & traverse_unknown, + GridCollisionChecker /*collision_checker*/) +{ + // NOTE(stevemacenski): Right now, we do not check if the node has wrapped around + // the regular grid (e.g. your node is on the edge of the costmap and i+1 + // goes to the other side). This check would add compute time and my assertion is + // that if you do wrap around, the heuristic will be so high it'll be added far + // in the queue that it will never be called if a valid path exists. + // This is intentionally un-included to increase speed, but be aware. If this causes + // trouble, please file a ticket and we can address it then. + + auto & cost = this->getCost(); + + // occupied node + if (cost == OCCUPIED || cost == INSCRIBED) { + return false; + } + + // unknown node + if (cost == UNKNOWN && !traverse_unknown) { + return false; + } + + return true; +} + +float Node2D::getTraversalCost(const NodePtr & child) +{ + // cost to travel will be the cost of the cell's code + + // neutral_cost is neutral cost for cost just to travel anywhere (50) + // 0.8 is a scale factor to remap costs [0, 252] evenly from [50, 252] + return Node2D::neutral_cost + 0.8 * child->getCost(); +} + +float Node2D::getHeuristicCost( + const Coordinates & node_coords, + const Coordinates & goal_coordinates) +{ + return hypotf( + goal_coordinates.x - node_coords.x, + goal_coordinates.y - node_coords.y) * Node2D::neutral_cost; +} + +void Node2D::initNeighborhood( + const unsigned int & x_size_uint, + const MotionModel & neighborhood) +{ + int x_size = static_cast(x_size_uint); + switch (neighborhood) { + case MotionModel::UNKNOWN: + throw std::runtime_error("Unknown neighborhood type selected."); + case MotionModel::VON_NEUMANN: + _neighbors_grid_offsets = {-1, +1, -x_size, +x_size}; + break; + case MotionModel::MOORE: + _neighbors_grid_offsets = {-1, +1, -x_size, +x_size, -x_size - 1, + -x_size + 1, +x_size - 1, +x_size + 1}; + break; + default: + throw std::runtime_error( + "Invalid neighborhood type selected. " + "Von-Neumann and Moore are valid for Node2D."); + } +} + +void Node2D::getNeighbors( + NodePtr & node, + std::function & NeighborGetter, + GridCollisionChecker collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors) +{ + // NOTE(stevemacenski): Irritatingly, the order here matters. If you start in free + // space and then expand 8-connected, the first set of neighbors will be all cost + // _neutral_cost. Then its expansion will all be 2 * _neutral_cost but now multiple + // nodes are touching that node so the last cell to update the back pointer wins. + // Thusly, the ordering ends with the cardinal directions for both sets such that + // behavior is consistent in large free spaces between them. + // 100 50 0 + // 100 50 50 + // 100 100 100 where lower-middle '100' is visited with same cost by both bottom '50' nodes + // Therefore, it is valuable to have some low-potential across the entire map + // rather than a small inflation around the obstacles + int index; + NodePtr neighbor; + int node_i = node->getIndex(); + + for (unsigned int i = 0; i != _neighbors_grid_offsets.size(); ++i) { + index = node_i + _neighbors_grid_offsets[i]; + if (NeighborGetter(index, neighbor)) { + if (neighbor->isNodeValid(traverse_unknown, collision_checker) && !neighbor->wasVisited()) { + neighbors.push_back(neighbor); + } + } + } +} + +} // namespace smac_planner diff --git a/smac_planner/src/node_se2.cpp b/smac_planner/src/node_se2.cpp new file mode 100644 index 00000000000..9764df0dbd2 --- /dev/null +++ b/smac_planner/src/node_se2.cpp @@ -0,0 +1,432 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// +// 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 "ompl/base/ScopedState.h" +#include "ompl/base/spaces/DubinsStateSpace.h" +#include "ompl/base/spaces/ReedsSheppStateSpace.h" + +#include "smac_planner/node_se2.hpp" + +using namespace std::chrono; // NOLINT + +namespace smac_planner +{ + +// defining static member for all instance to share +std::vector NodeSE2::_wavefront_heuristic; +double NodeSE2::neutral_cost = sqrt(2); +MotionTable NodeSE2::motion_table; + +// Each of these tables are the projected motion models through +// time and space applied to the search on the current node in +// continuous map-coordinates (e.g. not meters but partial map cells) +// Currently, these are set to project *at minimum* into a neighboring +// cell. Though this could be later modified to project a certain +// amount of time or particular distance forward. + +// http://planning.cs.uiuc.edu/node821.html +// Model for ackermann style vehicle with minimum radius restriction +void MotionTable::initDubin( + unsigned int & size_x_in, + unsigned int & /*size_y_in*/, + unsigned int & num_angle_quantization_in, + SearchInfo & search_info) +{ + size_x = size_x_in; + num_angle_quantization = num_angle_quantization_in; + num_angle_quantization_float = static_cast(num_angle_quantization); + change_penalty = search_info.change_penalty; + non_straight_penalty = search_info.non_straight_penalty; + cost_penalty = search_info.cost_penalty; + reverse_penalty = search_info.reverse_penalty; + + // angle must meet 3 requirements: + // 1) be increment of quantized bin size + // 2) chord length must be greater than sqrt(2) to leave current cell + // 3) maximum curvature must be respected, represented by minimum turning angle + // Thusly: + // On circle of radius minimum turning angle, we need select motion primatives + // with chord length > sqrt(2) and be an increment of our bin size + // + // chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size + // Thusly: angle <= 2.0 * asin(sqrt(2) / (2 * R)) + float angle = 2.0 * asin(sqrt(2.0) / (2 * search_info.minimum_turning_radius)); + // Now make sure angle is an increment of the quantized bin size + // And since its based on the minimum chord, we need to make sure its always larger + bin_size = + 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); + float increments; + if (angle < bin_size) { + increments = 1.0f; + } else { + // Search dimensions are clean multiples of quantization - this prevents + // paths with loops in them + increments = ceil(angle / bin_size); + } + angle = increments * bin_size; + + // find deflections + // If we make a right triangle out of the chord in circle of radius + // min turning angle, we can see that delta X = R * sin (angle) + float delta_x = search_info.minimum_turning_radius * sin(angle); + // Using that same right triangle, we can see that the complement + // to delta Y is R * cos (angle). If we subtract R, we get the actual value + float delta_y = search_info.minimum_turning_radius - + (search_info.minimum_turning_radius * cos(angle)); + + projections.clear(); + projections.reserve(3); + projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward + projections.emplace_back(delta_x, delta_y, increments); // Left + projections.emplace_back(delta_x, -delta_y, -increments); // Right + + // Create the correct OMPL state space + state_space = std::make_unique(search_info.minimum_turning_radius); +} + +// http://planning.cs.uiuc.edu/node822.html +// Same as Dubin model but now reverse is valid +// See notes in Dubin for explanation +void MotionTable::initReedsShepp( + unsigned int & size_x_in, + unsigned int & /*size_y_in*/, + unsigned int & num_angle_quantization_in, + SearchInfo & search_info) +{ + size_x = size_x_in; + num_angle_quantization = num_angle_quantization_in; + num_angle_quantization_float = static_cast(num_angle_quantization); + change_penalty = search_info.change_penalty; + non_straight_penalty = search_info.non_straight_penalty; + cost_penalty = search_info.cost_penalty; + reverse_penalty = search_info.reverse_penalty; + + float angle = 2.0 * asin(sqrt(2.0) / (2 * search_info.minimum_turning_radius)); + bin_size = + 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); + float increments; + if (angle < bin_size) { + increments = 1.0f; + } else { + increments = ceil(angle / bin_size); + } + angle = increments * bin_size; + + float delta_x = search_info.minimum_turning_radius * sin(angle); + float delta_y = search_info.minimum_turning_radius - + (search_info.minimum_turning_radius * cos(angle)); + + projections.clear(); + projections.reserve(6); + projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward + projections.emplace_back(delta_x, delta_y, increments); // Forward + Left + projections.emplace_back(delta_x, -delta_y, -increments); // Forward + Right + projections.emplace_back(-hypotf(delta_x, delta_y), 0.0, 0.0); // Backward + projections.emplace_back(-delta_x, delta_y, -increments); // Backward + Left + projections.emplace_back(-delta_x, -delta_y, increments); // Backward + Right + + // Create the correct OMPL state space + state_space = std::make_unique( + search_info.minimum_turning_radius); +} + +MotionPoses MotionTable::getProjections(const NodeSE2 * node) +{ + MotionPoses projection_list; + for (unsigned int i = 0; i != projections.size(); i++) { + projection_list.push_back(getProjection(node, i)); + } + + return projection_list; +} + +MotionPose MotionTable::getProjection(const NodeSE2 * node, const unsigned int & motion_index) +{ + const MotionPose & motion_model = projections[motion_index]; + + // transform delta X, Y, and Theta into local coordinates + const float & node_heading = node->pose.theta; + const float cos_theta = cos(node_heading * bin_size); // needs actual angle [0, 2PI] + const float sin_theta = sin(node_heading * bin_size); + const float delta_x = motion_model._x * cos_theta - motion_model._y * sin_theta; + const float delta_y = motion_model._x * sin_theta + motion_model._y * cos_theta; + float new_heading = node_heading + motion_model._theta; + + // normalize theta + while (new_heading >= num_angle_quantization_float) { + new_heading -= num_angle_quantization_float; + } + while (new_heading < 0.0) { + new_heading += num_angle_quantization_float; + } + + return MotionPose(delta_x + node->pose.x, delta_y + node->pose.y, new_heading); +} + +NodeSE2::NodeSE2(const unsigned int index) +: parent(nullptr), + pose(0.0f, 0.0f, 0.0f), + _cell_cost(std::numeric_limits::quiet_NaN()), + _accumulated_cost(std::numeric_limits::max()), + _index(index), + _was_visited(false), + _is_queued(false), + _motion_primitive_index(std::numeric_limits::max()) +{ +} + +NodeSE2::~NodeSE2() +{ + parent = nullptr; +} + +void NodeSE2::reset() +{ + parent = nullptr; + _cell_cost = std::numeric_limits::quiet_NaN(); + _accumulated_cost = std::numeric_limits::max(); + _was_visited = false; + _is_queued = false; + _motion_primitive_index = std::numeric_limits::max(); + pose.x = 0.0f; + pose.y = 0.0f; + pose.theta = 0.0f; +} + +bool NodeSE2::isNodeValid(const bool & traverse_unknown, GridCollisionChecker collision_checker) +{ + if (collision_checker.inCollision( + this->pose.x, this->pose.y, this->pose.theta * motion_table.bin_size, traverse_unknown)) + { + return false; + } + + _cell_cost = collision_checker.getCost(); + return true; +} + +float NodeSE2::getTraversalCost(const NodePtr & child) +{ + const float normalized_cost = child->getCost() / 252.0; + if (std::isnan(normalized_cost)) { + throw std::runtime_error( + "Node attempted to get traversal " + "cost without a known SE2 collision cost!"); + } + + // this is the first node + if (getMotionPrimitiveIndex() == std::numeric_limits::max()) { + return NodeSE2::neutral_cost; + } + + float travel_cost = 0.0; + float travel_cost_raw = NodeSE2::neutral_cost + motion_table.cost_penalty * normalized_cost; + + if (child->getMotionPrimitiveIndex() == 0 || child->getMotionPrimitiveIndex() == 3) { + // straight motion, no additional costs to be applied + travel_cost = travel_cost_raw; + } else { + if (getMotionPrimitiveIndex() == child->getMotionPrimitiveIndex()) { + // Turning motion but keeps in same direction: encourages to commit to turning if starting it + travel_cost = travel_cost_raw * motion_table.non_straight_penalty; + } else { + // Turning motion and changing direction: penalizes wiggling + travel_cost = travel_cost_raw * motion_table.change_penalty; + travel_cost += travel_cost_raw * motion_table.non_straight_penalty; + } + } + + if (getMotionPrimitiveIndex() > 2) { + // reverse direction + travel_cost *= motion_table.reverse_penalty; + } + + return travel_cost; +} + +float NodeSE2::getHeuristicCost( + const Coordinates & node_coords, + const Coordinates & goal_coords) +{ + // Dubin or Reeds-Shepp shortest distances + // Create OMPL states for checking + ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); + from[0] = node_coords.x; + from[1] = node_coords.y; + from[2] = node_coords.theta * motion_table.bin_size; + to[0] = goal_coords.x; + to[1] = goal_coords.y; + to[2] = goal_coords.theta * motion_table.bin_size; + + const float motion_heuristic = motion_table.state_space->distance(from(), to()); + + const unsigned int & wavefront_idx = static_cast(node_coords.y) * + motion_table.size_x + static_cast(node_coords.x); + const unsigned int & wavefront_value = _wavefront_heuristic[wavefront_idx]; + + // if lethal or didn't visit, use the motion heuristic instead. + if (wavefront_value == 0) { + return NodeSE2::neutral_cost * motion_heuristic; + } + + // -2 because wavefront starts at 2 + const float wavefront_heuristic = static_cast(wavefront_value - 2); + + return NodeSE2::neutral_cost * std::max(wavefront_heuristic, motion_heuristic); +} + +void NodeSE2::initMotionModel( + const MotionModel & motion_model, + unsigned int & size_x, + unsigned int & size_y, + unsigned int & num_angle_quantization, + SearchInfo & search_info) +{ + // find the motion model selected + switch (motion_model) { + case MotionModel::DUBIN: + motion_table.initDubin(size_x, size_y, num_angle_quantization, search_info); + break; + case MotionModel::REEDS_SHEPP: + motion_table.initReedsShepp(size_x, size_y, num_angle_quantization, search_info); + break; + default: + throw std::runtime_error( + "Invalid motion model for SE2 node. Please select between" + " Dubin (Ackermann forward only)," + " Reeds-Shepp (Ackermann forward and back)."); + } +} + +void NodeSE2::computeWavefrontHeuristic( + nav2_costmap_2d::Costmap2D * & costmap, + const unsigned int & start_x, const unsigned int & start_y, + const unsigned int & goal_x, const unsigned int & goal_y) +{ + unsigned int size = costmap->getSizeInCellsX() * costmap->getSizeInCellsY(); + if (_wavefront_heuristic.size() == size) { + // must reset all values + for (unsigned int i = 0; i != _wavefront_heuristic.size(); i++) { + _wavefront_heuristic[i] = 0; + } + } else { + unsigned int wavefront_size = _wavefront_heuristic.size(); + _wavefront_heuristic.resize(size, 0); + // must reset values for non-constructed indices + for (unsigned int i = 0; i != wavefront_size; i++) { + _wavefront_heuristic[i] = 0; + } + } + + const unsigned int & size_x = motion_table.size_x; + const int size_x_int = static_cast(size_x); + const unsigned int size_y = costmap->getSizeInCellsY(); + const unsigned int goal_index = goal_y * size_x + goal_x; + const unsigned int start_index = start_y * size_x + start_x; + unsigned int mx, my, mx_idx, my_idx; + + std::queue q; + q.emplace(goal_index); + + unsigned int idx = goal_index; + _wavefront_heuristic[idx] = 2; + + static const std::vector neighborhood = {1, -1, // left right + size_x_int, -size_x_int, // up down + size_x_int + 1, size_x_int - 1, // upper diagonals + -size_x_int + 1, -size_x_int - 1}; // lower diagonals + + while (!q.empty() || idx == start_index) { + // get next one + idx = q.front(); + q.pop(); + + my_idx = idx / size_x; + mx_idx = idx - (my_idx * size_x); + + // find neighbors + for (unsigned int i = 0; i != neighborhood.size(); i++) { + unsigned int new_idx = static_cast(static_cast(idx) + neighborhood[i]); + unsigned int last_wave_cost = _wavefront_heuristic[idx]; + + // if neighbor is unvisited and non-lethal, set N and add to queue + if (new_idx > 0 && new_idx < size_x * size_y && + _wavefront_heuristic[new_idx] == 0 && + static_cast(costmap->getCost(idx)) < INSCRIBED) + { + my = new_idx / size_x; + mx = new_idx - (my * size_x); + + if (mx == 0 && mx_idx >= size_x - 1 || mx >= size_x - 1 && mx_idx == 0) { + continue; + } + if (my == 0 && my_idx >= size_y - 1 || my >= size_y - 1 && my_idx == 0) { + continue; + } + + _wavefront_heuristic[new_idx] = last_wave_cost + 1; + q.emplace(idx + neighborhood[i]); + } + } + } +} + +void NodeSE2::getNeighbors( + const NodePtr & node, + std::function & NeighborGetter, + GridCollisionChecker collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors) +{ + unsigned int index = 0; + NodePtr neighbor = nullptr; + Coordinates initial_node_coords; + const MotionPoses motion_projections = motion_table.getProjections(node); + + for (unsigned int i = 0; i != motion_projections.size(); i++) { + index = NodeSE2::getIndex( + static_cast(motion_projections[i]._x), + static_cast(motion_projections[i]._y), + static_cast(motion_projections[i]._theta), + motion_table.size_x, motion_table.num_angle_quantization); + + if (NeighborGetter(index, neighbor) && !neighbor->wasVisited()) { + // Cache the initial pose in case it was visited but valid + // don't want to disrupt continuous coordinate expansion + initial_node_coords = neighbor->pose; + neighbor->setPose( + Coordinates( + motion_projections[i]._x, + motion_projections[i]._y, + motion_projections[i]._theta)); + if (neighbor->isNodeValid(traverse_unknown, collision_checker)) { + neighbor->setMotionPrimitiveIndex(i); + neighbors.push_back(neighbor); + } else { + neighbor->setPose(initial_node_coords); + } + } + } +} + +} // namespace smac_planner diff --git a/smac_planner/src/smac_planner.cpp b/smac_planner/src/smac_planner.cpp new file mode 100644 index 00000000000..71ba1d3333a --- /dev/null +++ b/smac_planner/src/smac_planner.cpp @@ -0,0 +1,387 @@ +// Copyright (c) 2020, 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 "Eigen/Core" +#include "smac_planner/smac_planner.hpp" + +#define BENCHMARK_TESTING + +namespace smac_planner +{ + +using namespace std::chrono; // NOLINT + +SmacPlanner::SmacPlanner() +: _a_star(nullptr), + _smoother(nullptr), + _costmap(nullptr), + _costmap_downsampler(nullptr) +{ +} + +SmacPlanner::~SmacPlanner() +{ + RCLCPP_INFO( + _logger, "Destroying plugin %s of type SmacPlanner", + _name.c_str()); +} + +void SmacPlanner::configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::string name, std::shared_ptr/*tf*/, + std::shared_ptr costmap_ros) +{ + _logger = node->get_logger(); + _clock = node->get_clock(); + _costmap = costmap_ros->getCostmap(); + _name = name; + _global_frame = costmap_ros->getGlobalFrameID(); + + bool allow_unknown; + int max_iterations; + int max_on_approach_iterations = std::numeric_limits::max(); + int angle_quantizations; + SearchInfo search_info; + bool smooth_path; + std::string motion_model_for_search; + + // General planner params + nav2_util::declare_parameter_if_not_declared( + node, name + ".tolerance", rclcpp::ParameterValue(0.125)); + _tolerance = static_cast(node->get_parameter(name + ".tolerance").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, name + ".downsample_costmap", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".downsample_costmap", _downsample_costmap); + nav2_util::declare_parameter_if_not_declared( + node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); + node->get_parameter(name + ".downsampling_factor", _downsampling_factor); + + nav2_util::declare_parameter_if_not_declared( + node, name + ".angle_quantization_bins", rclcpp::ParameterValue(72)); + node->get_parameter(name + ".angle_quantization_bins", angle_quantizations); + _angle_bin_size = 2.0 * M_PI / angle_quantizations; + _angle_quantizations = static_cast(angle_quantizations); + + 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(-1)); + node->get_parameter(name + ".max_iterations", max_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".smooth_path", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".smooth_path", smooth_path); + + nav2_util::declare_parameter_if_not_declared( + node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2)); + node->get_parameter(name + ".minimum_turning_radius", search_info.minimum_turning_radius); + + nav2_util::declare_parameter_if_not_declared( + node, name + ".reverse_penalty", rclcpp::ParameterValue(2.0)); + node->get_parameter(name + ".reverse_penalty", search_info.reverse_penalty); + nav2_util::declare_parameter_if_not_declared( + node, name + ".change_penalty", rclcpp::ParameterValue(0.5)); + node->get_parameter(name + ".change_penalty", search_info.change_penalty); + nav2_util::declare_parameter_if_not_declared( + node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.05)); + node->get_parameter(name + ".non_straight_penalty", search_info.non_straight_penalty); + nav2_util::declare_parameter_if_not_declared( + node, name + ".cost_penalty", rclcpp::ParameterValue(1.2)); + node->get_parameter(name + ".cost_penalty", search_info.cost_penalty); + nav2_util::declare_parameter_if_not_declared( + node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(2.0)); + node->get_parameter(name + ".analytic_expansion_ratio", search_info.analytic_expansion_ratio); + + nav2_util::declare_parameter_if_not_declared( + node, name + ".max_planning_time_ms", rclcpp::ParameterValue(5000.0)); + node->get_parameter(name + ".max_planning_time_ms", _max_planning_time); + + nav2_util::declare_parameter_if_not_declared( + node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN"))); + node->get_parameter(name + ".motion_model_for_search", motion_model_for_search); + MotionModel motion_model = fromString(motion_model_for_search); + if (motion_model == MotionModel::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get MotionModel search type. Given '%s', " + "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", + 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, " + "disabling maximum iterations."); + max_iterations = std::numeric_limits::max(); + } + + // convert to grid coordinates + const double minimum_turning_radius_global_coords = search_info.minimum_turning_radius; + search_info.minimum_turning_radius = + search_info.minimum_turning_radius / (_costmap->getResolution() * _downsampling_factor); + + _a_star = std::make_unique>(motion_model, search_info); + _a_star->initialize( + allow_unknown, + max_iterations, + max_on_approach_iterations); + _a_star->setFootprint(costmap_ros->getRobotFootprint(), costmap_ros->getUseRadius()); + + if (smooth_path) { + _smoother = std::make_unique(); + _optimizer_params.get(node.get(), name); + _smoother_params.get(node.get(), name); + _smoother_params.max_curvature = 1.0f / minimum_turning_radius_global_coords; + _smoother->initialize(_optimizer_params); + } + + if (_downsample_costmap && _downsampling_factor > 1) { + std::string topic_name = "downsampled_costmap"; + _costmap_downsampler = std::make_unique(); + _costmap_downsampler->on_configure( + node, _global_frame, topic_name, _costmap, _downsampling_factor); + } + + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + + RCLCPP_INFO( + _logger, "Configured plugin %s of type SmacPlanner with " + "tolerance %.2f, maximum iterations %i, " + "max on approach iterations %i, and %s. Using motion model: %s.", + _name.c_str(), _tolerance, max_iterations, max_on_approach_iterations, + allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", + toString(motion_model).c_str()); +} + +void SmacPlanner::activate() +{ + RCLCPP_INFO( + _logger, "Activating plugin %s of type SmacPlanner", + _name.c_str()); + _raw_plan_publisher->on_activate(); + if (_costmap_downsampler) { + _costmap_downsampler->on_activate(); + } +} + +void SmacPlanner::deactivate() +{ + RCLCPP_INFO( + _logger, "Deactivating plugin %s of type SmacPlanner", + _name.c_str()); + _raw_plan_publisher->on_deactivate(); + if (_costmap_downsampler) { + _costmap_downsampler->on_deactivate(); + } +} + +void SmacPlanner::cleanup() +{ + RCLCPP_INFO( + _logger, "Cleaning up plugin %s of type SmacPlanner", + _name.c_str()); + _a_star.reset(); + _smoother.reset(); + _costmap_downsampler->on_cleanup(); + _costmap_downsampler.reset(); + _raw_plan_publisher.reset(); +} + +nav_msgs::msg::Path SmacPlanner::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + steady_clock::time_point a = steady_clock::now(); + + std::unique_lock lock(*(_costmap->getMutex())); + + // Downsample costmap, if required + nav2_costmap_2d::Costmap2D * costmap = _costmap; + if (_costmap_downsampler) { + costmap = _costmap_downsampler->downsample(_downsampling_factor); + } + + // Set Costmap + _a_star->createGraph( + costmap->getSizeInCellsX(), + costmap->getSizeInCellsY(), + _angle_quantizations, + costmap); + + // Set starting point, in A* bin search coordinates + unsigned int mx, my; + costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + double orientation_bin = tf2::getYaw(start.pose.orientation) / _angle_bin_size; + while (orientation_bin < 0.0) { + orientation_bin += static_cast(_angle_quantizations); + } + unsigned int orientation_bin_id = static_cast(floor(orientation_bin)); + _a_star->setStart(mx, my, orientation_bin_id); + + // Set goal point, in A* bin search coordinates + costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); + orientation_bin = tf2::getYaw(goal.pose.orientation) / _angle_bin_size; + while (orientation_bin < 0.0) { + orientation_bin += static_cast(_angle_quantizations); + } + orientation_bin_id = static_cast(floor(orientation_bin)); + _a_star->setGoal(mx, my, orientation_bin_id); + + // Setup message + nav_msgs::msg::Path plan; + plan.header.stamp = _clock->now(); + plan.header.frame_id = _global_frame; + geometry_msgs::msg::PoseStamped pose; + pose.header = plan.header; + pose.pose.position.z = 0.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + pose.pose.orientation.w = 1.0; + + // Compute plan + NodeSE2::CoordinateVector path; + int num_iterations = 0; + std::string error; + try { + 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 { + error = std::string("exceeded maximum iterations"); + } + } + } catch (const std::runtime_error & e) { + error = "invalid use: "; + error += e.what(); + } + + if (!error.empty()) { + RCLCPP_WARN( + _logger, + "%s: failed to create plan, %s.", + _name.c_str(), error.c_str()); + return plan; + } + + // Convert to world coordinates and downsample path for smoothing if necesssary + // We're going to downsample by 4x to give terms room to move. + const int downsample_ratio = 4; + std::vector path_world; + path_world.reserve(path.size()); + plan.poses.reserve(path.size()); + + for (int i = path.size() - 1; i >= 0; --i) { + path_world.push_back(getWorldCoords(path[i].x, path[i].y, costmap)); + pose.pose.position.x = path_world.back().x(); + pose.pose.position.y = path_world.back().y(); + pose.pose.orientation = getWorldOrientation(path[i].theta); + plan.poses.push_back(pose); + } + + // Publish raw path for debug + if (_raw_plan_publisher->get_subscription_count() > 0) { + _raw_plan_publisher->publish(plan); + } + + // If not smoothing or too short to smooth, return path + if (!_smoother || path_world.size() < 4) { +#ifdef BENCHMARK_TESTING + steady_clock::time_point b = steady_clock::now(); + duration time_span = duration_cast>(b - a); + std::cout << "It took " << time_span.count() * 1000 << + " milliseconds with " << num_iterations << " iterations." << std::endl; +#endif + return plan; + } + + // Find how much time we have left to do smoothing + steady_clock::time_point b = steady_clock::now(); + duration time_span = duration_cast>(b - a); + double time_remaining = _max_planning_time - static_cast(time_span.count()); + _smoother_params.max_time = std::min(time_remaining, _optimizer_params.max_time); + + // Smooth plan + if (!_smoother->smooth(path_world, costmap, _smoother_params)) { + RCLCPP_WARN( + _logger, + "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", + _name.c_str()); + return plan; + } + + removeHook(path_world); + + // populate final path + // TODO(stevemacenski): set orientation to tangent of path + for (uint i = 0; i != path_world.size(); i++) { + pose.pose.position.x = path_world[i][0]; + pose.pose.position.y = path_world[i][1]; + plan.poses[i] = pose; + } + + return plan; +} + +void SmacPlanner::removeHook(std::vector & path) +{ + // Removes the end "hooking" since goal is locked in place + Eigen::Vector2d interpolated_second_to_last_point; + interpolated_second_to_last_point = (path.end()[-3] + path.end()[-1]) / 2.0; + if ( + squaredDistance(path.end()[-2], path.end()[-1]) > + squaredDistance(interpolated_second_to_last_point, path.end()[-1])) + { + path.end()[-2] = interpolated_second_to_last_point; + } +} + +Eigen::Vector2d SmacPlanner::getWorldCoords( + const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap) +{ + // mx, my are in continuous grid coordinates, must convert to world coordinates + double world_x = + static_cast(costmap->getOriginX()) + (mx + 0.5) * costmap->getResolution(); + double world_y = + static_cast(costmap->getOriginY()) + (my + 0.5) * costmap->getResolution(); + return Eigen::Vector2d(world_x, world_y); +} + +geometry_msgs::msg::Quaternion SmacPlanner::getWorldOrientation(const float & theta) +{ + // theta is in continuous bin coordinates, must convert to world orientation + tf2::Quaternion q; + q.setEuler(0.0, 0.0, theta * static_cast(_angle_bin_size)); + return tf2::toMsg(q); +} + +} // namespace smac_planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(smac_planner::SmacPlanner, nav2_core::GlobalPlanner) diff --git a/smac_planner/src/smac_planner_2d.cpp b/smac_planner/src/smac_planner_2d.cpp new file mode 100644 index 00000000000..643ffe41a69 --- /dev/null +++ b/smac_planner/src/smac_planner_2d.cpp @@ -0,0 +1,341 @@ +// Copyright (c) 2020, 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 "smac_planner/smac_planner_2d.hpp" + +// #define BENCHMARK_TESTING + +namespace smac_planner +{ +using namespace std::chrono; // NOLINT + +SmacPlanner2D::SmacPlanner2D() +: _a_star(nullptr), + _smoother(nullptr), + _costmap(nullptr), + _costmap_downsampler(nullptr) +{ +} + +SmacPlanner2D::~SmacPlanner2D() +{ + RCLCPP_INFO( + _logger, "Destroying plugin %s of type SmacPlanner2D", + _name.c_str()); +} + +void SmacPlanner2D::configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::string name, std::shared_ptr/*tf*/, + std::shared_ptr costmap_ros) +{ + _logger = node->get_logger(); + _clock = node->get_clock(); + _costmap = costmap_ros->getCostmap(); + _name = name; + _global_frame = costmap_ros->getGlobalFrameID(); + + bool allow_unknown; + int max_iterations; + int max_on_approach_iterations; + bool smooth_path; + double minimum_turning_radius; + std::string motion_model_for_search; + + // General planner params + nav2_util::declare_parameter_if_not_declared( + node, name + ".tolerance", rclcpp::ParameterValue(0.125)); + _tolerance = static_cast(node->get_parameter(name + ".tolerance").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, name + ".downsample_costmap", rclcpp::ParameterValue(true)); + node->get_parameter(name + ".downsample_costmap", _downsample_costmap); + nav2_util::declare_parameter_if_not_declared( + node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); + node->get_parameter(name + ".downsampling_factor", _downsampling_factor); + + 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(-1)); + 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(false)); + node->get_parameter(name + ".smooth_path", smooth_path); + nav2_util::declare_parameter_if_not_declared( + node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2)); + node->get_parameter(name + ".minimum_turning_radius", minimum_turning_radius); + + nav2_util::declare_parameter_if_not_declared( + node, name + ".max_planning_time_ms", rclcpp::ParameterValue(1000.0)); + node->get_parameter(name + ".max_planning_time_ms", _max_planning_time); + + nav2_util::declare_parameter_if_not_declared( + node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("MOORE"))); + node->get_parameter(name + ".motion_model_for_search", motion_model_for_search); + MotionModel motion_model = fromString(motion_model_for_search); + if (motion_model == MotionModel::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get MotionModel search type. Given '%s', " + "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", + 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, " + "disabling maximum iterations."); + max_iterations = std::numeric_limits::max(); + } + + _a_star = std::make_unique>(motion_model, SearchInfo()); + _a_star->initialize( + allow_unknown, + max_iterations, + max_on_approach_iterations); + + if (smooth_path) { + _smoother = std::make_unique(); + _optimizer_params.get(node.get(), name); + _smoother_params.get(node.get(), name); + _smoother_params.max_curvature = 1.0f / minimum_turning_radius; + _smoother->initialize(_optimizer_params); + } + + if (_downsample_costmap && _downsampling_factor > 1) { + std::string topic_name = "downsampled_costmap"; + _costmap_downsampler = std::make_unique(); + _costmap_downsampler->on_configure( + node, _global_frame, topic_name, _costmap, _downsampling_factor); + } + + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + + RCLCPP_INFO( + _logger, "Configured plugin %s of type SmacPlanner2D with " + "tolerance %.2f, maximum iterations %i, " + "max on approach iterations %i, and %s. Using motion model: %s.", + _name.c_str(), _tolerance, max_iterations, max_on_approach_iterations, + allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", + toString(motion_model).c_str()); +} + +void SmacPlanner2D::activate() +{ + RCLCPP_INFO( + _logger, "Activating plugin %s of type SmacPlanner2D", + _name.c_str()); + _raw_plan_publisher->on_activate(); + if (_costmap_downsampler) { + _costmap_downsampler->on_activate(); + } +} + +void SmacPlanner2D::deactivate() +{ + RCLCPP_INFO( + _logger, "Deactivating plugin %s of type SmacPlanner2D", + _name.c_str()); + _raw_plan_publisher->on_deactivate(); + if (_costmap_downsampler) { + _costmap_downsampler->on_deactivate(); + } +} + +void SmacPlanner2D::cleanup() +{ + RCLCPP_INFO( + _logger, "Cleaning up plugin %s of type SmacPlanner2D", + _name.c_str()); + _a_star.reset(); + _smoother.reset(); + _costmap_downsampler->on_cleanup(); + _costmap_downsampler.reset(); + _raw_plan_publisher.reset(); +} + +nav_msgs::msg::Path SmacPlanner2D::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + steady_clock::time_point a = steady_clock::now(); + + std::unique_lock lock(*(_costmap->getMutex())); + + // Downsample costmap, if required + nav2_costmap_2d::Costmap2D * costmap = _costmap; + if (_costmap_downsampler) { + costmap = _costmap_downsampler->downsample(_downsampling_factor); + } + + // Set Costmap + _a_star->createGraph( + costmap->getSizeInCellsX(), + costmap->getSizeInCellsY(), + 1, + costmap); + + // Set starting point + unsigned int mx, my; + costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + _a_star->setStart(mx, my, 0); + + // Set goal point + costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); + _a_star->setGoal(mx, my, 0); + + // Setup message + nav_msgs::msg::Path plan; + plan.header.stamp = _clock->now(); + plan.header.frame_id = _global_frame; + geometry_msgs::msg::PoseStamped pose; + pose.header = plan.header; + pose.pose.position.z = 0.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + pose.pose.orientation.w = 1.0; + + // Compute plan + Node2D::CoordinateVector path; + int num_iterations = 0; + std::string error; + try { + 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 { + error = std::string("exceeded maximum iterations"); + } + } + } catch (const std::runtime_error & e) { + error = "invalid use: "; + error += e.what(); + } + + if (!error.empty()) { + RCLCPP_WARN( + _logger, + "%s: failed to create plan, %s.", + _name.c_str(), error.c_str()); + return plan; + } + + // Convert to world coordinates and downsample path for smoothing if necesssary + // We're going to downsample by 4x to give terms room to move. + const int downsample_ratio = 4; + std::vector path_world; + path_world.reserve(_smoother ? path.size() / downsample_ratio : path.size()); + plan.poses.reserve(_smoother ? path.size() / downsample_ratio : path.size()); + + for (int i = path.size() - 1; i >= 0; --i) { + if (_smoother && i % downsample_ratio != 0) { + continue; + } + + path_world.push_back(getWorldCoords(path[i].x, path[i].y, costmap)); + pose.pose.position.x = path_world.back().x(); + pose.pose.position.y = path_world.back().y(); + plan.poses.push_back(pose); + } + + // Publish raw path for debug + if (_raw_plan_publisher->get_subscription_count() > 0) { + _raw_plan_publisher->publish(plan); + } + + // If not smoothing or too short to smooth, return path + if (!_smoother || path_world.size() < 4) { +#ifdef BENCHMARK_TESTING + steady_clock::time_point b = steady_clock::now(); + duration time_span = duration_cast>(b - a); + std::cout << "It took " << time_span.count() * 1000 << + " milliseconds with " << num_iterations << " iterations." << std::endl; +#endif + return plan; + } + + // Find how much time we have left to do smoothing + steady_clock::time_point b = steady_clock::now(); + duration time_span = duration_cast>(b - a); + double time_remaining = _max_planning_time - static_cast(time_span.count()); + _smoother_params.max_time = std::min(time_remaining, _optimizer_params.max_time); + + // Smooth plan + if (!_smoother->smooth(path_world, costmap, _smoother_params)) { + RCLCPP_WARN( + _logger, + "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", + _name.c_str()); + return plan; + } + + removeHook(path_world); + + // populate final path + for (uint i = 0; i != path_world.size(); i++) { + pose.pose.position.x = path_world[i][0]; + pose.pose.position.y = path_world[i][1]; + plan.poses[i] = pose; + } + + return plan; +} + +void SmacPlanner2D::removeHook(std::vector & path) +{ + // Removes the end "hooking" since goal is locked in place + Eigen::Vector2d interpolated_second_to_last_point; + interpolated_second_to_last_point = (path.end()[-3] + path.end()[-1]) / 2.0; + if ( + squaredDistance(path.end()[-2], path.end()[-1]) > + squaredDistance(interpolated_second_to_last_point, path.end()[-1])) + { + path.end()[-2] = interpolated_second_to_last_point; + } +} + +Eigen::Vector2d SmacPlanner2D::getWorldCoords( + const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap) +{ + float world_x = + static_cast(costmap->getOriginX()) + (mx + 0.5) * costmap->getResolution(); + float world_y = + static_cast(costmap->getOriginY()) + (my + 0.5) * costmap->getResolution(); + return Eigen::Vector2d(world_x, world_y); +} + +} // namespace smac_planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(smac_planner::SmacPlanner2D, nav2_core::GlobalPlanner) diff --git a/smac_planner/test/CMakeLists.txt b/smac_planner/test/CMakeLists.txt new file mode 100644 index 00000000000..c5ff757d1c8 --- /dev/null +++ b/smac_planner/test/CMakeLists.txt @@ -0,0 +1,98 @@ +# Test costmap downsampler +ament_add_gtest(test_costmap_downsampler + test_costmap_downsampler.cpp +) +ament_target_dependencies(test_costmap_downsampler + ${dependencies} +) +target_link_libraries(test_costmap_downsampler + ${library_name} +) + +# Test Node2D +ament_add_gtest(test_node2d + test_node2d.cpp +) +ament_target_dependencies(test_node2d + ${dependencies} +) +target_link_libraries(test_node2d + ${library_name} +) + +# Test NodeSE2 +ament_add_gtest(test_nodese2 + test_nodese2.cpp +) +ament_target_dependencies(test_nodese2 + ${dependencies} +) +target_link_libraries(test_nodese2 + ${library_name} +) + +# Test NodeBasic +ament_add_gtest(test_nodebasic + test_nodebasic.cpp +) +ament_target_dependencies(test_nodebasic + ${dependencies} +) +target_link_libraries(test_nodebasic + ${library_name} +) + +# Test collision checker +ament_add_gtest(test_collision_checker + test_collision_checker.cpp +) +ament_target_dependencies(test_collision_checker + ${dependencies} +) +target_link_libraries(test_collision_checker + ${library_name} +) + +# Test A* +ament_add_gtest(test_a_star + test_a_star.cpp +) +ament_target_dependencies(test_a_star + ${dependencies} +) +target_link_libraries(test_a_star + ${library_name} +) + +# Test SMAC SE2 +ament_add_gtest(test_smac_se2 + test_smac_se2.cpp +) +ament_target_dependencies(test_smac_se2 + ${dependencies} +) +target_link_libraries(test_smac_se2 + ${library_name} +) + +# Test SMAC 2D +ament_add_gtest(test_smac_2d + test_smac_2d.cpp +) +ament_target_dependencies(test_smac_2d + ${dependencies} +) +target_link_libraries(test_smac_2d + ${library_name}_2d +) + +# Test smoother +ament_add_gtest(test_smoother + test_smoother.cpp +) +ament_target_dependencies(test_smoother + ${dependencies} +) +target_link_libraries(test_smoother + ${library_name}_2d +) diff --git a/smac_planner/test/deprecated_upsampler/upsampler.hpp b/smac_planner/test/deprecated_upsampler/upsampler.hpp new file mode 100644 index 00000000000..8bfc36c00e5 --- /dev/null +++ b/smac_planner/test/deprecated_upsampler/upsampler.hpp @@ -0,0 +1,213 @@ +// Copyright (c) 2020, 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 DEPRECATED_UPSAMPLER__UPSAMPLER_HPP_ +#define DEPRECATED_UPSAMPLER__UPSAMPLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "smac_planner/types.hpp" +#include "smac_planner/upsampler_cost_function.hpp" +#include "smac_planner/upsampler_cost_function_nlls.hpp" + +#include "ceres/ceres.h" +#include "Eigen/Core" + +namespace smac_planner +{ + +/** + * @class smac_planner::Upsampler + * @brief A Conjugate Gradient 2D path upsampler implementation + */ +class Upsampler +{ +public: + /** + * @brief A constructor for smac_planner::Upsampler + */ + Upsampler() {} + + /** + * @brief A destructor for smac_planner::Upsampler + */ + ~Upsampler() {} + + /** + * @brief Initialization of the Upsampler + */ + void initialize(const OptimizerParams params) + { + _debug = params.debug; + + // General Params + + // 2 most valid options: STEEPEST_DESCENT, NONLINEAR_CONJUGATE_GRADIENT + _options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT; + _options.line_search_type = ceres::WOLFE; + _options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE; + _options.line_search_interpolation_type = ceres::CUBIC; + + _options.max_num_iterations = params.max_iterations; // 5000 + _options.max_solver_time_in_seconds = params.max_time; // 5.0; // TODO + + _options.function_tolerance = params.fn_tol; + _options.gradient_tolerance = params.gradient_tol; + _options.parameter_tolerance = params.param_tol; // 1e-20; + + _options.min_line_search_step_size = params.advanced.min_line_search_step_size; // 1e-30; + _options.max_num_line_search_step_size_iterations = + params.advanced.max_num_line_search_step_size_iterations; + _options.line_search_sufficient_function_decrease = + params.advanced.line_search_sufficient_function_decrease; // 1e-30; + _options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction; + _options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction; + _options.max_num_line_search_direction_restarts = + params.advanced.max_num_line_search_direction_restarts; + _options.line_search_sufficient_curvature_decrease = + params.advanced.line_search_sufficient_curvature_decrease; + _options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion; + + if (_debug) { + _options.minimizer_progress_to_stdout = true; + } else { + _options.logging_type = ceres::SILENT; + } + } + + /** + * @brief Upsampling method + * @param path Reference to path + * @param upsample parameters weights + * @param upsample_ratio upsample ratio + * @return If Upsampler was successful + */ + bool upsample( + std::vector & path, + const SmootherParams & params, + const int & upsample_ratio) + { + _options.max_solver_time_in_seconds = params.max_time; + + if (upsample_ratio != 2 && upsample_ratio != 4) { + // invalid inputs + return false; + } + + const int param_ratio = upsample_ratio * 2.0; + const int total_size = 2 * (path.size() * upsample_ratio - upsample_ratio + 1); + double parameters[total_size]; // NOLINT + + // 20-4hz regularly, but dosnt work in faster cases + // Linearly distribute initial poses for optimization + // TODO(stevemacenski) generalize for 2x and 4x + unsigned int next_pt; + Eigen::Vector2d interpolated; + std::vector temp_path; + for (unsigned int pt = 0; pt != path.size() - 1; pt++) { + next_pt = pt + 1; + interpolated = (path[next_pt] + path[pt]) / 2.0; + + parameters[param_ratio * pt] = path[pt][0]; + parameters[param_ratio * pt + 1] = path[pt][1]; + temp_path.push_back(path[pt]); + + parameters[param_ratio * pt + 2] = interpolated[0]; + parameters[param_ratio * pt + 3] = interpolated[1]; + temp_path.push_back(interpolated); + } + + parameters[total_size - 2] = path.back()[0]; + parameters[total_size - 1] = path.back()[1]; + temp_path.push_back(path.back()); + + // Solve the upsampling problem + ceres::GradientProblemSolver::Summary summary; + ceres::GradientProblem problem(new UpsamplerCostFunction(temp_path, params, upsample_ratio)); + ceres::Solve(_options, problem, parameters, &summary); + + + path.resize(total_size / 2); + for (int i = 0; i != total_size / 2; i++) { + path[i][0] = parameters[2 * i]; + path[i][1] = parameters[2 * i + 1]; + } + + // 10-15 hz, regularly + // std::vector path_double_sampled; + // for (int i = 0; i != path.size() - 1; i++) { // last term should not be upsampled + // path_double_sampled.push_back(path[i]); + // path_double_sampled.push_back((path[i+1] + path[i]) / 2); + // } + + // std::unique_ptr problem = std::make_unique(); + // for (uint i = 1; i != path_double_sampled.size() - 1; i++) { + // ceres::CostFunction * cost_fn = + // new UpsamplerConstrainedCostFunction(path_double_sampled, params, 2, i); + // problem->AddResidualBlock( + // cost_fn, nullptr, &path_double_sampled[i][0], &path_double_sampled[i][1]); + // // locking initial coordinates unnecessary since there's no update between terms in NLLS + // } + + // ceres::Solver::Summary summary; + // _options.minimizer_type = ceres::LINE_SEARCH; + // ceres::Solve(_options, problem.get(), &summary); + + // if (upsample_ratio == 4) { + // std::vector path_quad_sampled; + // for (int i = 0; i != path_double_sampled.size() - 1; i++) { + // path_quad_sampled.push_back(path_double_sampled[i]); + // path_quad_sampled.push_back((path_double_sampled[i+1] + path_double_sampled[i]) / 2.0); + // } + + // std::unique_ptr problem2 = std::make_unique(); + // for (uint i = 1; i != path_quad_sampled.size() - 1; i++) { + // ceres::CostFunction * cost_fn = + // new UpsamplerConstrainedCostFunction(path_quad_sampled, params, 4, i); + // problem2->AddResidualBlock( + // cost_fn, nullptr, &path_quad_sampled[i][0], &path_quad_sampled[i][1]); + // } + + // ceres::Solve(_options, problem2.get(), &summary); + + // path = path_quad_sampled; + // } else { + // path = path_double_sampled; + // } + + if (_debug) { + std::cout << summary.FullReport() << '\n'; + } + + if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost <= 0.0) { + return false; + } + + return true; + } + +private: + bool _debug; + ceres::GradientProblemSolver::Options _options; +}; + +} // namespace smac_planner + +#endif // DEPRECATED_UPSAMPLER__UPSAMPLER_HPP_ diff --git a/smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp b/smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp new file mode 100644 index 00000000000..cf84acf6234 --- /dev/null +++ b/smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp @@ -0,0 +1,366 @@ +// Copyright (c) 2020, 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 DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_ +#define DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ceres/ceres.h" +#include "Eigen/Core" +#include "smac_planner/types.hpp" +#include "smac_planner/options.hpp" + +#define EPSILON 0.0001 + +namespace smac_planner +{ +/** + * @struct smac_planner::UpsamplerCostFunction + * @brief Cost function for path upsampling with multiple terms using unconstrained + * optimization including curvature, smoothness, collision, and avoid obstacles. + */ +class UpsamplerCostFunction : public ceres::FirstOrderFunction +{ +public: + /** + * @brief A constructor for smac_planner::UpsamplerCostFunction + * @param num_points Number of path points to consider + */ + UpsamplerCostFunction( + const std::vector & path, + const SmootherParams & params, + const int & upsample_ratio) + : _num_params(2 * path.size()), + _params(params), + _upsample_ratio(upsample_ratio), + _path(path) + { + } + // TODO(stevemacenski) removed upsample_ratio because temp upsampling on path size + + /** + * @struct CurvatureComputations + * @brief Cache common computations between the curvature terms to minimize recomputations + */ + struct CurvatureComputations + { + /** + * @brief A constructor for smac_planner::CurvatureComputations + */ + CurvatureComputations() + { + valid = false; + } + + bool valid; + /** + * @brief Check if result is valid for penalty + * @return is valid (non-nan, non-inf, and turning angle > max) + */ + bool isValid() + { + return valid; + } + + Eigen::Vector2d delta_xi{0, 0}; + Eigen::Vector2d delta_xi_p{0, 0}; + double delta_xi_norm{0}; + double delta_xi_p_norm{0}; + double delta_phi_i{0}; + double turning_rad{0}; + double ki_minus_kmax{0}; + }; + + /** + * @brief Smoother cost function evaluation + * @param parameters X,Y pairs of points + * @param cost total cost of path + * @param gradient of path at each X,Y pair from cost function derived analytically + * @return if successful in computing values + */ + virtual bool Evaluate( + const double * parameters, + double * cost, + double * gradient) const + { + Eigen::Vector2d xi; + Eigen::Vector2d xi_p1; + Eigen::Vector2d xi_m1; + uint x_index, y_index; + cost[0] = 0.0; + double cost_raw = 0.0; + double grad_x_raw = 0.0; + double grad_y_raw = 0.0; + + // cache some computations between the residual and jacobian + CurvatureComputations curvature_params; + + for (int i = 0; i != NumParameters() / 2; i++) { + x_index = 2 * i; + y_index = 2 * i + 1; + gradient[x_index] = 0.0; + gradient[y_index] = 0.0; + if (i < 1 || i >= NumParameters() / 2 - 1) { + continue; + } + + // if original point's neighbors TODO + if (i % _upsample_ratio == 1) { + continue; + } + + xi = Eigen::Vector2d(parameters[x_index], parameters[y_index]); + + // TODO(stevemacenski): from deep copy to make sure no feedback _path + xi_p1 = _path.at(i + 1); + xi_m1 = _path.at(i - 1); + // xi_p1 = Eigen::Vector2d(parameters[x_index + 2], parameters[y_index + 2]); + // xi_m1 = Eigen::Vector2d(parameters[x_index - 2], parameters[y_index - 2]); + + // compute cost + addSmoothingResidual(15000, xi, xi_p1, xi_m1, cost_raw); + addCurvatureResidual(60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw); + + if (gradient != NULL) { + // compute gradient + addSmoothingJacobian(15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw); + addCurvatureJacobian(60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw); + + gradient[x_index] = grad_x_raw; + gradient[y_index] = grad_y_raw; + } + } + + cost[0] = cost_raw; + return true; + } + + /** + * @brief Get number of parameter blocks + * @return Number of parameters in cost function + */ + virtual int NumParameters() const {return _num_params;} + +protected: + /** + * @brief Cost function term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param r Residual (cost) of term + */ + inline void addSmoothingResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & r) const + { + r += weight * ( + pt_p.dot(pt_p) - + 4 * pt_p.dot(pt) + + 2 * pt_p.dot(pt_m) + + 4 * pt.dot(pt) - + 4 * pt.dot(pt_m) + + pt_m.dot(pt_m)); // objective function value + } + + /** + * @brief Cost function derivative term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addSmoothingJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & j0, + double & j1) const + { + j0 += weight * + (-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative + j1 += weight * + (-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative + } + + /** + * @brief Get path curvature information + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + */ + inline void getCurvatureParams( + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params) const + { + curvature_params.valid = true; + curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]); + curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]); + curvature_params.delta_xi_norm = curvature_params.delta_xi.norm(); + curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm(); + + if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON || + std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) || + std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm)) + { + // ensure we have non-nan values returned + curvature_params.valid = false; + return; + } + + const double & delta_xi_by_xi_p = + curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm; + double projection = + curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p; + if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) { + projection = 1.0; + } + + curvature_params.delta_phi_i = std::acos(projection); + curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm; + + curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio * + _params.max_curvature; + // TODO(stevemacenski) is use of upsample_ratio correct here? small number? + // TODO(stevemacenski) can remove the subtraction with a + // lower weight value, does have direction issue, maybe just tuning? + + if (curvature_params.ki_minus_kmax <= EPSILON) { + // Quadratic penalty need not apply + curvature_params.valid = false; + } + } + + /** + * @brief Cost function term for maximum curved paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + * @param r Residual (cost) of term + */ + inline void addCurvatureResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params, + double & r) const + { + getCurvatureParams(pt, pt_p, pt_m, curvature_params); + + if (!curvature_params.isValid()) { + return; + } + + r += weight * + curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; // objective function value + } + + /** + * @brief Cost function derivative term for maximum curvature paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct with cached values to speed up Jacobian computation + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addCurvatureJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & /*pt_m*/, + CurvatureComputations & curvature_params, + double & j0, + double & j1) const + { + if (!curvature_params.isValid()) { + return; + } + + const double & partial_delta_phi_i_wrt_cost_delta_phi_i = + -1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2)); + // const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0); + auto neg_pt_plus = -1 * pt_p; + Eigen::Vector2d p1 = normalizedOrthogonalComplement( + pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm); + Eigen::Vector2d p2 = normalizedOrthogonalComplement( + neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm); + + const double & u = 2 * curvature_params.ki_minus_kmax; + const double & common_prefix = + (1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i; + const double & common_suffix = curvature_params.delta_phi_i / + (curvature_params.delta_xi_norm * curvature_params.delta_xi_norm); + const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi / + curvature_params.delta_xi_norm; + + const Eigen::Vector2d jacobian = u * + (common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_im1 = u * + (common_prefix * p2 + (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1); + j0 += weight * jacobian[0]; // xi y component of partial-derivative + j1 += weight * jacobian[1]; // xi x component of partial-derivative + // j0 += weight * + // (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]); + // j1 += weight * + // (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]); + } + + /** + * @brief Computing the normalized orthogonal component of 2 vectors + * @param a Vector + * @param b Vector + * @param norm a Vector's norm + * @param norm b Vector's norm + * @return Normalized vector of orthogonal components + */ + inline Eigen::Vector2d normalizedOrthogonalComplement( + const Eigen::Vector2d & a, + const Eigen::Vector2d & b, + const double & a_norm, + const double & b_norm) const + { + return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm); + } + + int _num_params; + SmootherParams _params; + int _upsample_ratio; + std::vector _path; +}; + +} // namespace smac_planner + +#endif // DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_ diff --git a/smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp b/smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp new file mode 100644 index 00000000000..a6fd595f969 --- /dev/null +++ b/smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp @@ -0,0 +1,334 @@ +// Copyright (c) 2020, 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 DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ +#define DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ceres/ceres.h" +#include "Eigen/Core" +#include "smac_planner/types.hpp" +#include "smac_planner/options.hpp" + +#define EPSILON 0.0001 + +namespace smac_planner +{ +/** + * @struct smac_planner::UpsamplerConstrainedCostFunction + * @brief Cost function for path upsampling with multiple terms using NLLS + * including curvature, smoothness, collision, and avoid obstacles. + */ +class UpsamplerConstrainedCostFunction : public ceres::SizedCostFunction<1, 1, 1> +{ +public: + /** + * @brief A constructor for smac_planner::UpsamplerConstrainedCostFunction + * @param num_points Number of path points to consider + */ + UpsamplerConstrainedCostFunction( + const std::vector & path, + const SmootherParams & params, + const int & upsample_ratio, + const int & i) + : _path(path), + _params(params), + _upsample_ratio(upsample_ratio), + index(i) + { + } + + /** + * @struct CurvatureComputations + * @brief Cache common computations between the curvature terms to minimize recomputations + */ + struct CurvatureComputations + { + /** + * @brief A constructor for smac_planner::CurvatureComputations + */ + CurvatureComputations() + { + valid = true; + } + + bool valid; + /** + * @brief Check if result is valid for penalty + * @return is valid (non-nan, non-inf, and turning angle > max) + */ + bool isValid() + { + return valid; + } + + Eigen::Vector2d delta_xi{0, 0}; + Eigen::Vector2d delta_xi_p{0, 0}; + double delta_xi_norm{0}; + double delta_xi_p_norm{0}; + double delta_phi_i{0}; + double turning_rad{0}; + double ki_minus_kmax{0}; + }; + + /** + * @brief Smoother cost function evaluation + * @param parameters X,Y pairs of points + * @param cost total cost of path + * @param gradient of path at each X,Y pair from cost function derived analytically + * @return if successful in computing values + */ + + bool Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const override + { + Eigen::Vector2d xi = Eigen::Vector2d(parameters[0][0], parameters[1][0]); + Eigen::Vector2d xi_p1 = _path.at(index + 1); + Eigen::Vector2d xi_m1 = _path.at(index - 1); + CurvatureComputations curvature_params; + double grad_x_raw = 0, grad_y_raw = 0, cost_raw = 0; + + // compute cost + addSmoothingResidual(15000, xi, xi_p1, xi_m1, cost_raw); + addCurvatureResidual(60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw); + + residuals[0] = 0; + residuals[0] = cost_raw; // objective function value x + + if (jacobians != NULL && jacobians[0] != NULL) { + addSmoothingJacobian(15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw); + addCurvatureJacobian(60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw); + + jacobians[0][0] = 0; + jacobians[1][0] = 0; + jacobians[0][0] = grad_x_raw; // x derivative + jacobians[1][0] = grad_y_raw; // y derivative + jacobians[0][1] = 0.0; + jacobians[1][1] = 0.0; + } + + return true; + } + +protected: + /** + * @brief Cost function term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param r Residual (cost) of term + */ + inline void addSmoothingResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & r) const + { + r += weight * ( + pt_p.dot(pt_p) - + 4 * pt_p.dot(pt) + + 2 * pt_p.dot(pt_m) + + 4 * pt.dot(pt) - + 4 * pt.dot(pt_m) + + pt_m.dot(pt_m)); // objective function value + } + + /** + * @brief Cost function derivative term for smooth paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addSmoothingJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + double & j0, + double & j1) const + { + j0 += weight * + (-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]); // xi x component of partial-derivative + j1 += weight * + (-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]); // xi y component of partial-derivative + } + + /** + * @brief Get path curvature information + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + */ + inline void getCurvatureParams( + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params) const + { + curvature_params.valid = true; + curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]); + curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]); + curvature_params.delta_xi_norm = curvature_params.delta_xi.norm(); + curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm(); + + if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON || + std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) || + std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm)) + { + // ensure we have non-nan values returned + curvature_params.valid = false; + return; + } + + const double & delta_xi_by_xi_p = + curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm; + double projection = + curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p; + if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) { + projection = 1.0; + } + + curvature_params.delta_phi_i = std::acos(projection); + curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm; + + curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio * + _params.max_curvature; + + if (curvature_params.ki_minus_kmax <= EPSILON) { + // Quadratic penalty need not apply + curvature_params.valid = false; + } + } + + /** + * @brief Cost function term for maximum curved paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct to cache computations for the jacobian to use + * @param r Residual (cost) of term + */ + inline void addCurvatureResidual( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & pt_m, + CurvatureComputations & curvature_params, + double & r) const + { + getCurvatureParams(pt, pt_p, pt_m, curvature_params); + + if (!curvature_params.isValid()) { + return; + } + + // objective function value + r += weight * + curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; + } + + /** + * @brief Cost function derivative term for maximum curvature paths + * @param weight Weight to apply to function + * @param pt Point Xi for evaluation + * @param pt Point Xi+1 for calculating Xi's cost + * @param pt Point Xi-1 for calculating Xi's cost + * @param curvature_params A struct with cached values to speed up Jacobian computation + * @param j0 Gradient of X term + * @param j1 Gradient of Y term + */ + inline void addCurvatureJacobian( + const double & weight, + const Eigen::Vector2d & pt, + const Eigen::Vector2d & pt_p, + const Eigen::Vector2d & /*pt_m*/, + CurvatureComputations & curvature_params, + double & j0, + double & j1) const + { + if (!curvature_params.isValid()) { + return; + } + + const double & partial_delta_phi_i_wrt_cost_delta_phi_i = + -1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2)); + // const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0); + auto neg_pt_plus = -1 * pt_p; + Eigen::Vector2d p1 = normalizedOrthogonalComplement( + pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm); + Eigen::Vector2d p2 = normalizedOrthogonalComplement( + neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm); + + const double & u = 2 * curvature_params.ki_minus_kmax; + const double & common_prefix = + (1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i; + const double & common_suffix = curvature_params.delta_phi_i / + (curvature_params.delta_xi_norm * curvature_params.delta_xi_norm); + const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi / + curvature_params.delta_xi_norm; + + const Eigen::Vector2d jacobian = u * + (common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_im1 = u * + (common_prefix * p2 + (common_suffix * d_delta_xi_d_xi)); + const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1); + j0 += weight * jacobian[0]; // xi x component of partial-derivative + j1 += weight * jacobian[1]; // xi y component of partial-derivative + // j0 += weight * + // (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]); + // j1 += weight * + // (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]); + } + /** + * @brief Computing the normalized orthogonal component of 2 vectors + * @param a Vector + * @param b Vector + * @param norm a Vector's norm + * @param norm b Vector's norm + * @return Normalized vector of orthogonal components + */ + inline Eigen::Vector2d normalizedOrthogonalComplement( + const Eigen::Vector2d & a, + const Eigen::Vector2d & b, + const double & a_norm, + const double & b_norm) const + { + return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm); + } + + std::vector _path; + SmootherParams _params; + int _upsample_ratio; + int index; +}; + +} // namespace smac_planner + +#endif // DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ diff --git a/smac_planner/test/path.png b/smac_planner/test/path.png new file mode 100644 index 00000000000..fc98e7709b1 Binary files /dev/null and b/smac_planner/test/path.png differ diff --git a/smac_planner/test/test_a_star.cpp b/smac_planner/test/test_a_star.cpp new file mode 100644 index 00000000000..a1e5dcf2080 --- /dev/null +++ b/smac_planner/test/test_a_star.cpp @@ -0,0 +1,183 @@ +// Copyright (c) 2020, 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/a_star.hpp" +#include "smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(AStarTest, test_a_star_2d) +{ + smac_planner::SearchInfo info; + smac_planner::AStarAlgorithm a_star(smac_planner::MotionModel::MOORE, info); + int max_iterations = 10000; + float tolerance = 0.0; + float some_tolerance = 20.0; + int it_on_approach = 10; + int num_it = 0; + + a_star.initialize(false, max_iterations, it_on_approach); + a_star.setFootprint(nav2_costmap_2d::Footprint(), true); + + nav2_costmap_2d::Costmap2D * costmapA = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + // island in the middle of lethal cost to cross + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmapA->setCost(i, j, 254); + } + } + + // functional case testing + a_star.createGraph(costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), 1, costmapA); + a_star.setStart(20u, 20u, 0); + a_star.setGoal(80u, 80u, 0); + smac_planner::Node2D::CoordinateVector path; + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + EXPECT_EQ(num_it, 556); + + // check path is the right size and collision free + EXPECT_EQ(path.size(), 81u); + for (unsigned int i = 0; i != path.size(); i++) { + EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); + } + + // setting non-zero dim 3 for 2D search + EXPECT_THROW( + a_star.createGraph( + costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), 10, costmapA), std::runtime_error); + EXPECT_THROW(a_star.setGoal(0, 0, 10), std::runtime_error); + EXPECT_THROW(a_star.setStart(0, 0, 10), std::runtime_error); + + path.clear(); + // failure cases with invalid inputs + smac_planner::AStarAlgorithm a_star_2( + smac_planner::MotionModel::VON_NEUMANN, info); + a_star_2.initialize(false, max_iterations, it_on_approach); + a_star_2.setFootprint(nav2_costmap_2d::Footprint(), true); + num_it = 0; + EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + a_star_2.createGraph(costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), 1, costmapA); + num_it = 0; + EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + a_star_2.setStart(50, 50, 0); // invalid + a_star_2.setGoal(0, 0, 0); // valid + num_it = 0; + EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + a_star_2.setStart(0, 0, 0); // valid + a_star_2.setGoal(50, 50, 0); // invalid + num_it = 0; + EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + num_it = 0; + // invalid goal but liberal tolerance + 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(), 32u); + for (unsigned int i = 0; i != path.size(); i++) { + EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); + } + + EXPECT_TRUE(a_star_2.getStart() != nullptr); + EXPECT_TRUE(a_star_2.getGoal() != nullptr); + EXPECT_EQ(a_star_2.getSizeX(), 100u); + EXPECT_EQ(a_star_2.getSizeY(), 100u); + EXPECT_EQ(a_star_2.getSizeDim3(), 1u); + EXPECT_EQ(a_star_2.getToleranceHeuristic(), 1000.0); + EXPECT_EQ(a_star_2.getOnApproachMaxIterations(), 10); + + delete costmapA; +} + +TEST(AStarTest, test_a_star_se2) +{ + smac_planner::SearchInfo info; + info.change_penalty = 1.2; + info.non_straight_penalty = 1.4; + info.reverse_penalty = 2.1; + info.minimum_turning_radius = 2.0; // in grid coordinates + unsigned int size_theta = 72; + smac_planner::AStarAlgorithm a_star( + smac_planner::MotionModel::DUBIN, info); + int max_iterations = 10000; + float tolerance = 10.0; + int it_on_approach = 10; + int num_it = 0; + + a_star.initialize(false, max_iterations, it_on_approach); + a_star.setFootprint(nav2_costmap_2d::Footprint(), true); + + nav2_costmap_2d::Costmap2D * costmapA = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + // island in the middle of lethal cost to cross + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmapA->setCost(i, j, 254); + } + } + + // functional case testing + a_star.createGraph( + costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), size_theta, costmapA); + a_star.setStart(10u, 10u, 0u); + a_star.setGoal(80u, 80u, 40u); + smac_planner::NodeSE2::CoordinateVector path; + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + + // check path is the right size and collision free + EXPECT_EQ(num_it, 61); + EXPECT_EQ(path.size(), 75u); + for (unsigned int i = 0; i != path.size(); i++) { + EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); + } + + delete costmapA; +} + +TEST(AStarTest, test_constants) +{ + smac_planner::MotionModel mm = smac_planner::MotionModel::UNKNOWN; // unknown + EXPECT_EQ(smac_planner::toString(mm), std::string("Unknown")); + mm = smac_planner::MotionModel::VON_NEUMANN; // vonneumann + EXPECT_EQ(smac_planner::toString(mm), std::string("Von Neumann")); + mm = smac_planner::MotionModel::MOORE; // moore + EXPECT_EQ(smac_planner::toString(mm), std::string("Moore")); + mm = smac_planner::MotionModel::DUBIN; // dubin + EXPECT_EQ(smac_planner::toString(mm), std::string("Dubin")); + mm = smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp + EXPECT_EQ(smac_planner::toString(mm), std::string("Reeds-Shepp")); + + EXPECT_EQ(smac_planner::fromString("VON_NEUMANN"), smac_planner::MotionModel::VON_NEUMANN); + EXPECT_EQ(smac_planner::fromString("MOORE"), smac_planner::MotionModel::MOORE); + EXPECT_EQ(smac_planner::fromString("DUBIN"), smac_planner::MotionModel::DUBIN); + EXPECT_EQ(smac_planner::fromString("REEDS_SHEPP"), smac_planner::MotionModel::REEDS_SHEPP); + EXPECT_EQ(smac_planner::fromString("NONE"), smac_planner::MotionModel::UNKNOWN); +} diff --git a/smac_planner/test/test_collision_checker.cpp b/smac_planner/test/test_collision_checker.cpp new file mode 100644 index 00000000000..e6590d610de --- /dev/null +++ b/smac_planner/test/test_collision_checker.cpp @@ -0,0 +1,172 @@ +// Copyright (c) 2020 Shivang Patel +// Copyright (c) 2020 Samsung Research +// +// 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 "gtest/gtest.h" +#include "smac_planner/collision_checker.hpp" + +using namespace nav2_costmap_2d; // NOLINT + +TEST(collision_footprint, test_basic) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); + + geometry_msgs::msg::Point p1; + p1.x = -0.5; + p1.y = 0.0; + geometry_msgs::msg::Point p2; + p2.x = 0.0; + p2.y = 0.5; + geometry_msgs::msg::Point p3; + p3.x = 0.5; + p3.y = 0.0; + geometry_msgs::msg::Point p4; + p4.x = 0.0; + p4.y = -0.5; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + smac_planner::GridCollisionChecker collision_checker(costmap_); + collision_checker.setFootprint(footprint, false /*use footprint*/); + collision_checker.inCollision(5.0, 5.0, 0.0, false); + float cost = collision_checker.getCost(); + EXPECT_NEAR(cost, 0.0, 0.001); + delete costmap_; +} + +TEST(collision_footprint, test_point_cost) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); + + smac_planner::GridCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::Footprint footprint; + collision_checker.setFootprint(footprint, true /*radius / pointcose*/); + + collision_checker.inCollision(5.0, 5.0, 0.0, false); + float cost = collision_checker.getCost(); + EXPECT_NEAR(cost, 0.0, 0.001); + delete costmap_; +} + +TEST(collision_footprint, test_world_to_map) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); + + smac_planner::GridCollisionChecker collision_checker(costmap_); + nav2_costmap_2d::Footprint footprint; + collision_checker.setFootprint(footprint, true /*radius / point cost*/); + + unsigned int x, y; + + collision_checker.worldToMap(1.0, 1.0, x, y); + + collision_checker.inCollision(x, y, 0.0, false); + float cost = collision_checker.getCost(); + + EXPECT_NEAR(cost, 0.0, 0.001); + + costmap_->setCost(50, 50, 200); + collision_checker.worldToMap(5.0, 5.0, x, y); + + collision_checker.inCollision(x, y, 0.0, false); + EXPECT_NEAR(collision_checker.getCost(), 200.0, 0.001); + delete costmap_; +} + +TEST(collision_footprint, test_footprint_at_pose_with_movement) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 254); + + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap_->setCost(i, j, 0); + } + } + + geometry_msgs::msg::Point p1; + p1.x = -1.0; + p1.y = 1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = 1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = -1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = -1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + smac_planner::GridCollisionChecker collision_checker(costmap_); + collision_checker.setFootprint(footprint, false /*use footprint*/); + + collision_checker.inCollision(50, 50, 0.0, false); + float cost = collision_checker.getCost(); + EXPECT_NEAR(cost, 0.0, 0.001); + + collision_checker.inCollision(50, 49, 0.0, false); + float up_value = collision_checker.getCost(); + EXPECT_NEAR(up_value, 254.0, 0.001); + + collision_checker.inCollision(50, 52, 0.0, false); + float down_value = collision_checker.getCost(); + EXPECT_NEAR(down_value, 254.0, 0.001); + delete costmap_; +} + +TEST(collision_footprint, test_point_and_line_cost) +{ + nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D( + 100, 100, 0.10000, 0, 0.0, + 0.0); + + costmap_->setCost(62, 50, 254); + costmap_->setCost(39, 60, 254); + + geometry_msgs::msg::Point p1; + p1.x = -1.0; + p1.y = 1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = 1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = -1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = -1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + smac_planner::GridCollisionChecker collision_checker(costmap_); + collision_checker.setFootprint(footprint, false /*use footprint*/); + + collision_checker.inCollision(50, 50, 0.0, false); + float value = collision_checker.getCost(); + EXPECT_NEAR(value, 0.0, 0.001); + + collision_checker.inCollision(49, 50, 0.0, false); + float left_value = collision_checker.getCost(); + EXPECT_NEAR(left_value, 254.0, 0.001); + + collision_checker.inCollision(52, 50, 0.0, false); + float right_value = collision_checker.getCost(); + EXPECT_NEAR(right_value, 254.0, 0.001); + delete costmap_; +} diff --git a/smac_planner/test/test_costmap_downsampler.cpp b/smac_planner/test/test_costmap_downsampler.cpp new file mode 100644 index 00000000000..5353dcb3871 --- /dev/null +++ b/smac_planner/test/test_costmap_downsampler.cpp @@ -0,0 +1,67 @@ +// Copyright (c) 2020, 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "smac_planner/costmap_downsampler.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(CostmapDownsampler, costmap_downsample_test) +{ + nav2_util::LifecycleNode::SharedPtr node = std::make_shared( + "CostmapDownsamplerTest"); + smac_planner::CostmapDownsampler downsampler; + + // create basic costmap + nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); + costmapA.setCost(0, 0, 100); + costmapA.setCost(5, 5, 50); + + // downsample it + downsampler.on_configure(node, "map", "unused_topic", &costmapA, 2); + nav2_costmap_2d::Costmap2D * downsampledCostmapA = downsampler.downsample(2); + + // validate it + EXPECT_EQ(downsampledCostmapA->getCost(0, 0), 100); + EXPECT_EQ(downsampledCostmapA->getCost(2, 2), 50); + EXPECT_EQ(downsampledCostmapA->getSizeInCellsX(), 5u); + EXPECT_EQ(downsampledCostmapA->getSizeInCellsY(), 5u); + + // give it another costmap of another size + nav2_costmap_2d::Costmap2D costmapB(4, 4, 0.10, 0.0, 0.0, 0); + + // downsample it + downsampler.on_configure(node, "map", "unused_topic", &costmapB, 4); + downsampler.on_activate(); + nav2_costmap_2d::Costmap2D * downsampledCostmapB = downsampler.downsample(4); + downsampler.on_deactivate(); + + // validate size + EXPECT_EQ(downsampledCostmapB->getSizeInCellsX(), 1u); + EXPECT_EQ(downsampledCostmapB->getSizeInCellsY(), 1u); + + downsampler.resizeCostmap(); +} diff --git a/smac_planner/test/test_node2d.cpp b/smac_planner/test/test_node2d.cpp new file mode 100644 index 00000000000..bc42988aca4 --- /dev/null +++ b/smac_planner/test/test_node2d.cpp @@ -0,0 +1,135 @@ +// Copyright (c) 2020, 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "smac_planner/node_2d.hpp" +#include "smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(Node2DTest, test_node_2d) +{ + nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); + smac_planner::GridCollisionChecker checker(&costmapA); + + // test construction + unsigned char cost = static_cast(1); + smac_planner::Node2D testA(cost, 1); + smac_planner::Node2D testB(cost, 1); + EXPECT_EQ(testA.getCost(), 1.0f); + + // test reset + testA.reset(10); + EXPECT_EQ(testA.getCost(), 10.0f); + + // Check constants + EXPECT_EQ(testA.neutral_cost, 50.0f); + + // check collision checking + EXPECT_EQ(testA.isNodeValid(false, checker), true); + testA.reset(254); + EXPECT_EQ(testA.isNodeValid(false, checker), false); + testA.reset(255); + EXPECT_EQ(testA.isNodeValid(true, checker), true); + EXPECT_EQ(testA.isNodeValid(false, checker), false); + testA.reset(10); + + // check traversal cost computation + EXPECT_EQ(testB.getTraversalCost(&testA), 58.0f); + + // check heuristic cost computation + smac_planner::Node2D::Coordinates A(0.0, 0.0); + smac_planner::Node2D::Coordinates B(10.0, 5.0); + EXPECT_NEAR(testB.getHeuristicCost(A, B), 559.016, 0.01); + + // check operator== works on index + unsigned char costC = '2'; + smac_planner::Node2D testC(costC, 1); + EXPECT_TRUE(testA == testC); + + // check accumulated costs are set + testC.setAccumulatedCost(100); + EXPECT_EQ(testC.getAccumulatedCost(), 100.0f); + + // check visiting state + EXPECT_EQ(testC.wasVisited(), false); + testC.queued(); + EXPECT_EQ(testC.isQueued(), true); + testC.visited(); + EXPECT_EQ(testC.wasVisited(), true); + EXPECT_EQ(testC.isQueued(), false); + + // check index + EXPECT_EQ(testC.getIndex(), 1u); + + // check static index functions + EXPECT_EQ(smac_planner::Node2D::getIndex(1u, 1u, 10u), 11u); + EXPECT_EQ(smac_planner::Node2D::getIndex(6u, 43u, 10u), 436u); + EXPECT_EQ(smac_planner::Node2D::getCoords(436u, 10u, 1u).x, 6u); + EXPECT_EQ(smac_planner::Node2D::getCoords(436u, 10u, 1u).y, 43u); + EXPECT_THROW(smac_planner::Node2D::getCoords(436u, 10u, 10u), std::runtime_error); +} + +TEST(Node2DTest, test_node_2d_neighbors) +{ + // test neighborhood computation + smac_planner::Node2D::initNeighborhood(10u, smac_planner::MotionModel::VON_NEUMANN); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets.size(), 4u); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[0], -1); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[1], 1); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[2], -10); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[3], 10); + + smac_planner::Node2D::initNeighborhood(100u, smac_planner::MotionModel::MOORE); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets.size(), 8u); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[0], -1); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[1], 1); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[2], -100); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[3], 100); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[4], -101); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[5], -99); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[6], 99); + EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[7], 101); + + nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); + smac_planner::GridCollisionChecker checker(&costmapA); + unsigned char cost = static_cast(1); + smac_planner::Node2D * node = new smac_planner::Node2D(cost, 1); + std::function neighborGetter = + [&, this](const unsigned int & index, smac_planner::Node2D * & neighbor_rtn) -> bool + { + return true; + }; + + smac_planner::Node2D::NodeVector neighbors; + smac_planner::Node2D::getNeighbors(node, neighborGetter, checker, false, neighbors); + delete node; + + // should be empty since totally invalid + EXPECT_EQ(neighbors.size(), 0u); +} diff --git a/smac_planner/test/test_nodebasic.cpp b/smac_planner/test/test_nodebasic.cpp new file mode 100644 index 00000000000..0ec90a112f3 --- /dev/null +++ b/smac_planner/test/test_nodebasic.cpp @@ -0,0 +1,49 @@ +// Copyright (c) 2020, 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "smac_planner/node_basic.hpp" +#include "smac_planner/node_2d.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(NodeBasicTest, test_node_basic) +{ + smac_planner::NodeBasic node(50); + + EXPECT_EQ(node.index, 50u); + EXPECT_EQ(node.graph_node_ptr, nullptr); + + smac_planner::NodeBasic node2(100); + + EXPECT_EQ(node2.index, 100u); + EXPECT_EQ(node2.graph_node_ptr, nullptr); +} diff --git a/smac_planner/test/test_nodese2.cpp b/smac_planner/test/test_nodese2.cpp new file mode 100644 index 00000000000..2f23c12aaf9 --- /dev/null +++ b/smac_planner/test/test_nodese2.cpp @@ -0,0 +1,214 @@ +// Copyright (c) 2020, 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(NodeSE2Test, test_node_se2) +{ + smac_planner::SearchInfo info; + info.change_penalty = 1.2; + info.non_straight_penalty = 1.4; + info.reverse_penalty = 2.1; + info.minimum_turning_radius = 0.20; + unsigned int size_x = 10; + unsigned int size_y = 10; + unsigned int size_theta = 72; + + smac_planner::NodeSE2::initMotionModel( + smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info); + + nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( + 10, 10, 0.05, 0.0, 0.0, 0); + smac_planner::GridCollisionChecker checker(costmapA); + checker.setFootprint(nav2_costmap_2d::Footprint(), true); + + // test construction + smac_planner::NodeSE2 testA(49); + smac_planner::NodeSE2 testB(49); + EXPECT_TRUE(std::isnan(testA.getCost())); + + // test node valid and cost + testA.pose.x = 5; + testA.pose.y = 5; + testA.pose.theta = 0; + EXPECT_EQ(testA.isNodeValid(true, checker), true); + EXPECT_EQ(testA.isNodeValid(false, checker), true); + EXPECT_EQ(testA.getCost(), 0.0f); + + // test reset + testA.reset(); + EXPECT_TRUE(std::isnan(testA.getCost())); + + // Check constants + EXPECT_EQ(testA.neutral_cost, sqrt(2)); + + // check collision checking + EXPECT_EQ(testA.isNodeValid(false, checker), true); + + // check traversal cost computation + // simulated first node, should return neutral cost + EXPECT_NEAR(testB.getTraversalCost(&testA), sqrt(2), 0.01); + // now with straight motion, cost is 0, so will be sqrt(2) as well + testB.setMotionPrimitiveIndex(1); + testA.setMotionPrimitiveIndex(0); + EXPECT_NEAR(testB.getTraversalCost(&testA), sqrt(2), 0.01); + // same direction as parent, testB + testA.setMotionPrimitiveIndex(1); + EXPECT_NEAR(testB.getTraversalCost(&testA), 1.9799f, 0.01); + // opposite direction as parent, testB + testA.setMotionPrimitiveIndex(2); + EXPECT_NEAR(testB.getTraversalCost(&testA), 3.67696f, 0.01); + + // will throw because never collision checked testB + EXPECT_THROW(testA.getTraversalCost(&testB), std::runtime_error); + + // check motion primitives + EXPECT_EQ(testA.getMotionPrimitiveIndex(), 2u); + + // check heuristic cost computation + smac_planner::NodeSE2::computeWavefrontHeuristic( + costmapA, + static_cast(10.0), + static_cast(5.0), + 0.0, 0.0); + smac_planner::NodeSE2::Coordinates A(0.0, 0.0, 4.2); + smac_planner::NodeSE2::Coordinates B(10.0, 5.0, 54.1); + EXPECT_NEAR(testB.getHeuristicCost(B, A), 16.723, 0.01); + + // check operator== works on index + smac_planner::NodeSE2 testC(49); + EXPECT_TRUE(testA == testC); + + // check accumulated costs are set + testC.setAccumulatedCost(100); + EXPECT_EQ(testC.getAccumulatedCost(), 100.0f); + + // check visiting state + EXPECT_EQ(testC.wasVisited(), false); + testC.queued(); + EXPECT_EQ(testC.isQueued(), true); + testC.visited(); + EXPECT_EQ(testC.wasVisited(), true); + EXPECT_EQ(testC.isQueued(), false); + + // check index + EXPECT_EQ(testC.getIndex(), 49u); + + // check set pose and pose + testC.setPose(smac_planner::NodeSE2::Coordinates(10.0, 5.0, 4)); + EXPECT_EQ(testC.pose.x, 10.0); + EXPECT_EQ(testC.pose.y, 5.0); + EXPECT_EQ(testC.pose.theta, 4); + + // check static index functions + EXPECT_EQ(smac_planner::NodeSE2::getIndex(1u, 1u, 4u, 10u, 72u), 796u); + EXPECT_EQ(smac_planner::NodeSE2::getCoords(796u, 10u, 72u).x, 1u); + EXPECT_EQ(smac_planner::NodeSE2::getCoords(796u, 10u, 72u).y, 1u); + EXPECT_EQ(smac_planner::NodeSE2::getCoords(796u, 10u, 72u).theta, 4u); + + delete costmapA; +} + +TEST(NodeSE2Test, test_node_2d_neighbors) +{ + smac_planner::SearchInfo info; + info.change_penalty = 1.2; + info.non_straight_penalty = 1.4; + info.reverse_penalty = 2.1; + info.minimum_turning_radius = 4; // 0.2 in grid coordinates + unsigned int size_x = 100; + unsigned int size_y = 100; + unsigned int size_theta = 72; + smac_planner::NodeSE2::initMotionModel( + smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info); + + + // test neighborhood computation + EXPECT_EQ(smac_planner::NodeSE2::motion_table.projections.size(), 3u); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01); + + smac_planner::NodeSE2::initMotionModel( + smac_planner::MotionModel::REEDS_SHEPP, size_x, size_y, size_theta, info); + + EXPECT_EQ(smac_planner::NodeSE2::motion_table.projections.size(), 6u); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._x, -1.731517, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._y, 0, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._theta, 0, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._x, -1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._y, 0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._theta, -5, 0.01); + + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._x, -1.69047, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._y, -0.3747, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._theta, 5, 0.01); + + nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0); + smac_planner::GridCollisionChecker checker(&costmapA); + smac_planner::NodeSE2 * node = new smac_planner::NodeSE2(49); + std::function neighborGetter = + [&, this](const unsigned int & index, smac_planner::NodeSE2 * & neighbor_rtn) -> bool + { + // because we don't return a real object + return false; + }; + + smac_planner::NodeSE2::NodeVector neighbors; + smac_planner::NodeSE2::getNeighbors(node, neighborGetter, checker, false, neighbors); + delete node; + + // should be empty since totally invalid + EXPECT_EQ(neighbors.size(), 0u); +} diff --git a/smac_planner/test/test_smac_2d.cpp b/smac_planner/test/test_smac_2d.cpp new file mode 100644 index 00000000000..63b4ba99876 --- /dev/null +++ b/smac_planner/test/test_smac_2d.cpp @@ -0,0 +1,80 @@ +// Copyright (c) 2020, 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/a_star.hpp" +#include "smac_planner/collision_checker.hpp" +#include "smac_planner/smac_planner.hpp" +#include "smac_planner/smac_planner_2d.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +// SMAC smoke tests for plugin-level issues rather than algorithms +// (covered by more extensively testing in other files) +// System tests in nav2_system_tests will actually plan with this work + +TEST(SmacTest, test_smac_2d) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node2D = + std::make_shared("Smac2DTest"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + node2D->declare_parameter("test.smooth_path", true); + node2D->set_parameter(rclcpp::Parameter("test.smooth_path", true)); + node2D->declare_parameter("test.downsample_costmap", true); + node2D->set_parameter(rclcpp::Parameter("test.downsample_costmap", true)); + node2D->declare_parameter("test.downsampling_factor", 2); + node2D->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2)); + + geometry_msgs::msg::PoseStamped start, goal; + start.pose.position.x = 0.0; + start.pose.position.y = 0.0; + start.pose.orientation.w = 1.0; + goal = start; + auto planner_2d = std::make_unique(); + planner_2d->configure(node2D, "test", nullptr, costmap_ros); + planner_2d->activate(); + try { + planner_2d->createPlan(start, goal); + } catch (...) { + } + + planner_2d->deactivate(); + planner_2d->cleanup(); + + planner_2d.reset(); + costmap_ros->on_cleanup(rclcpp_lifecycle::State()); + node2D.reset(); + costmap_ros.reset(); +} diff --git a/smac_planner/test/test_smac_se2.cpp b/smac_planner/test/test_smac_se2.cpp new file mode 100644 index 00000000000..caf7d3381e9 --- /dev/null +++ b/smac_planner/test/test_smac_se2.cpp @@ -0,0 +1,92 @@ +// Copyright (c) 2020, 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "smac_planner/node_se2.hpp" +#include "smac_planner/a_star.hpp" +#include "smac_planner/collision_checker.hpp" +#include "smac_planner/smac_planner.hpp" +#include "smac_planner/smac_planner_2d.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +// SMAC smoke tests for plugin-level issues rather than algorithms +// (covered by more extensively testing in other files) +// System tests in nav2_system_tests will actually plan with this work + +TEST(SmacTest, test_smac_se2) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 = + std::make_shared("SmacSE2Test"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + nodeSE2->declare_parameter("test.smooth_path", true); + nodeSE2->set_parameter(rclcpp::Parameter("test.smooth_path", true)); + nodeSE2->declare_parameter("test.downsample_costmap", true); + nodeSE2->set_parameter(rclcpp::Parameter("test.downsample_costmap", true)); + nodeSE2->declare_parameter("test.downsampling_factor", 2); + nodeSE2->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2)); + + geometry_msgs::msg::PoseStamped start, goal; + start.pose.position.x = 0.0; + start.pose.position.y = 0.0; + start.pose.orientation.w = 1.0; + goal = start; + auto planner = std::make_unique(); + planner->configure(nodeSE2, "test", nullptr, costmap_ros); + planner->activate(); + + try { + planner->createPlan(start, goal); + } catch (...) { + } + + planner->deactivate(); + planner->cleanup(); + + planner.reset(); + costmap_ros->on_cleanup(rclcpp_lifecycle::State()); + costmap_ros.reset(); + nodeSE2.reset(); +} + +TEST(SmacTestSE2, test_dist) +{ + Eigen::Vector2d p1; + p1[0] = 0.0; + p1[1] = 0.0; + Eigen::Vector2d p2; + p2[0] = 0.0; + p2[1] = 1.0; + EXPECT_NEAR(smac_planner::squaredDistance(p1, p2), 1.0, 0.001); +} diff --git a/smac_planner/test/test_smoother.cpp b/smac_planner/test/test_smoother.cpp new file mode 100644 index 00000000000..ab05a93959f --- /dev/null +++ b/smac_planner/test/test_smoother.cpp @@ -0,0 +1,104 @@ +// Copyright (c) 2020, 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "smac_planner/a_star.hpp" +#include "smac_planner/smoother.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(SmootherTest, test_smoother) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node2D = + std::make_shared("SmootherTest"); + + // create and populate costmap + nav2_costmap_2d::Costmap2D * costmap = new nav2_costmap_2d::Costmap2D(100, 100, 0.05, 0, 0, 0); + + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap->setCost(i, j, 254); + } + } + + // compute path to use + smac_planner::SearchInfo info; + info.change_penalty = 1.2; + info.non_straight_penalty = 1.4; + info.reverse_penalty = 2.1; + info.minimum_turning_radius = 4.0; // in grid coordinates + unsigned int size_theta = 72; + smac_planner::AStarAlgorithm a_star( + smac_planner::MotionModel::DUBIN, info); + int max_iterations = 1000000; + float tolerance = 0.0; + int it_on_approach = 1000000000; + int num_it = 0; + a_star.initialize(false, max_iterations, it_on_approach); + a_star.setFootprint(nav2_costmap_2d::Footprint(), true); + a_star.createGraph(costmap->getSizeInCellsX(), costmap->getSizeInCellsY(), size_theta, costmap); + a_star.setStart(10u, 10u, 0u); + a_star.setGoal(80u, 80u, 40u); + smac_planner::NodeSE2::CoordinateVector plan; + EXPECT_TRUE(a_star.createPath(plan, num_it, tolerance)); + + // populate our smoothing paths + std::vector path; + std::vector initial_path; + for (unsigned int i = 0; i != plan.size(); i++) { + path.push_back(Eigen::Vector2d(plan[i].x, plan[i].y)); + initial_path.push_back(Eigen::Vector2d(plan[i].x, plan[i].y)); + } + + smac_planner::OptimizerParams params; + params.debug = true; + params.get(node2D.get(), "test"); + + smac_planner::SmootherParams smoother_params; + smoother_params.get(node2D.get(), "test"); + smoother_params.max_curvature = 5.0; + smoother_params.curvature_weight = 30.0; + smoother_params.distance_weight = 0.0; + smoother_params.smooth_weight = 00.0; + smoother_params.costmap_weight = 0.025; + + smac_planner::Smoother smoother; + smoother.initialize(params); + smoother.smooth(path, costmap, smoother_params); + + // kept at the right size + EXPECT_EQ(path.size(), 73u); + + for (unsigned int i = 1; i != path.size() - 1; i++) { + // check distance between points is in a good range + EXPECT_NEAR( + hypot(path[i][0] - path[i + 1][0], path[i][1] - path[i + 1][1]), 1.407170, 0.5); + } + + delete costmap; +} diff --git a/tools/ros2_dependencies.repos b/tools/ros2_dependencies.repos index b6645197b0f..73d6b1f1e28 100644 --- a/tools/ros2_dependencies.repos +++ b/tools/ros2_dependencies.repos @@ -19,3 +19,11 @@ repositories: type: git url: https://github.com/ros-perception/vision_opencv.git version: ros2 + ros/bond_core: + type: git + url: https://github.com/ros/bond_core.git + version: ros2 + ompl/ompl: + type: git + url: https://github.com/ompl/ompl.git + version: 1.5.0