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_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 bf8ead1cf38..433ac40fdc6 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 0.4.1 + 0.4.2 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..fcb761322c6 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -51,6 +51,8 @@ BtNavigator::BtNavigator() "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_speed_controller_bt_node", + "nav2_truncate_path_action_bt_node", + "nav2_change_goal_node_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", @@ -206,6 +208,7 @@ 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_.reset(); @@ -214,13 +217,6 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) 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 +243,19 @@ BtNavigator::navigateToPose() return action_server_->is_cancel_requested(); }; + 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; + } + RosTopicLogger topic_logger(client_node_, tree_); std::shared_ptr feedback_msg = std::make_shared(); @@ -276,19 +285,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 +306,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_common/package.xml b/nav2_common/package.xml index f51ddff6659..6820a84f4fa 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 0.4.1 + 0.4.2 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 c678de23c75..d0c6bbf7763 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,13 +2,14 @@ nav2_controller - 0.4.1 + 0.4.2 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..97846ba8eee 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -22,7 +22,6 @@ #include "nav_2d_utils/conversions.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 +31,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 +45,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 +68,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 +103,30 @@ 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()); + } + 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()); + } for (size_t i = 0; i != controller_ids_.size(); i++) { try { @@ -96,8 +141,9 @@ 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()); } } @@ -105,6 +151,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 +215,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 +236,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 +249,7 @@ bool ControllerServer::findControllerId( return false; } } else { + RCLCPP_DEBUG(get_logger(), "Selected controller: %s.", c_name.c_str()); current_controller = c_name; } @@ -237,13 +275,8 @@ void ControllerServer::computeControl() rclcpp::Rate 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 +309,7 @@ void ControllerServer::computeControl() return; } - RCLCPP_DEBUG(get_logger(), "DWB succeeded, setting result"); + RCLCPP_DEBUG(get_logger(), "Controller succeeded, setting result"); publishZeroVelocity(); @@ -294,7 +327,8 @@ 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(); + goal_checker_->reset(); RCLCPP_DEBUG( get_logger(), "Path end point is (%.2f, %.2f)", @@ -310,7 +344,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 +386,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 +403,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 +418,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 d9e50dcd9cb..2ae59bc25ad 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 0.4.1 + 0.4.2 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_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 5524f5737d3..541a246a513 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 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 7ca65cc7c2f..02f6c7fd9db 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 0.4.1 + 0.4.2 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 86561d334b3..df8405a3c91 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" @@ -163,7 +165,7 @@ InflationLayer::updateCosts( } // 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 +217,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 +263,7 @@ InflationLayer::updateCosts( } } - for (auto & dist:inflation_cells_) { + for (auto & dist : inflation_cells_) { dist.clear(); dist.reserve(200); } 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..8a6124a47a9 --- /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_ + "." + "phi", phi_v_); + 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/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 75c7552628b..b172bd87df2 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]); 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..ad1fb15d3a0 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" 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_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 490a7190f73..b1d715f7d14 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 0.4.1 + 0.4.2 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..2da4b44a52c 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. * @@ -210,9 +196,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 bd787e51b6f..ccd46651690 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 0.4.1 + 0.4.2 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..245323db510 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,9 +86,6 @@ 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)); @@ -98,7 +94,6 @@ void DWBLocalPlanner::configure( 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,7 +104,6 @@ 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_); @@ -118,10 +112,8 @@ void DWBLocalPlanner::configure( 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 +141,6 @@ DWBLocalPlanner::cleanup() pub_->on_cleanup(); traj_generator_.reset(); - goal_checker_.reset(); } std::string @@ -263,42 +254,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 +263,6 @@ DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) } traj_generator_->reset(); - goal_checker_->reset(); pub_->publishGlobalPlan(path2d); global_plan_ = path2d; @@ -371,6 +325,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 +347,8 @@ DWBLocalPlanner::computeVelocityCommands( critic->debrief(cmd_vel.velocity); } + lock.unlock(); + pub_->publishLocalPlan(pose.header, best.traj); pub_->publishCostGrid(costmap_ros_, critics_); @@ -401,6 +360,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_); diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index a11a78356bc..1e1c22895ae 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 0.4.1 + 0.4.2 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 a6fc418451c..e98ba11d3d7 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 0.4.1 + 0.4.2 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 fbc20f0ab3d..08b6a78bec0 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 0.4.1 + 0.4.2 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/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index b3d19ae98b9..0a48874c49b 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.4.1 + 0.4.2 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 be971939c34..e08b3d97629 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.4.1 + 0.4.2 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..a0b268f3ef7 100644 --- a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt +++ b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt @@ -43,7 +43,15 @@ ament_target_dependencies(path_ops ${dependencies} ) -install(TARGETS conversions path_ops +add_library(tf_help SHARED + src/tf_help.cpp +) + +ament_target_dependencies(tf_help + ${dependencies} +) + +install(TARGETS conversions path_ops tf_help ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -60,7 +68,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/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 5f0a0a0910a..ab5945eb9d8 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.4.1 + 0.4.2 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/package.xml b/nav2_lifecycle_manager/package.xml index a9b16bd4867..f68735154e2 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 0.4.1 + 0.4.2 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..618d318935c 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -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; } 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..3aa4e2e2332 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 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 dbd43a0410a..165a7234bcf 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 0.4.1 + 0.4.2 Refactored map server for ROS2 Navigation 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..7555ebd6326 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -94,13 +94,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*/) { @@ -208,7 +201,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 a2d40860249..dbdc5167c16 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 0.4.1 + 0.4.2 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..c6a4fa9699d 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp @@ -258,13 +258,13 @@ class NavFn /** 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..e9d34279a56 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -88,8 +88,8 @@ class NavfnPlanner : public nav2_core::GlobalPlanner // 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); + // 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 inline double squared_distance( diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 4f6a56ce193..e3fa60a5942 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 0.4.1 + 0.4.2 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..a873a3e5d1c 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -152,6 +152,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 +177,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 +186,8 @@ NavfnPlanner::makePlan( planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_); + lock.unlock(); + int map_start[2]; map_start[0] = mx; map_start[1] = my; @@ -312,20 +319,17 @@ NavfnPlanner::getPlanFromPotential( int path_len = planner_->calcPath(costmap_->getSizeInCellsX() * 4); 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 +361,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; } 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 598fe05acbb..f0beb74f099 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 0.4.1 + 0.4.2 TODO Steve Macenski Apache-2.0 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index edf273e87fe..ee3a07e4739 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -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,6 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_FATAL( get_logger(), "Failed to create global planner. Exception: %s", ex.what()); - exit(-1); } } @@ -112,17 +109,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 +186,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 +208,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 +221,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 +247,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 +260,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 +268,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_server.hpp b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp index 1c02fc1f598..bb9efa571c1 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp @@ -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 327bf898a47..67433aed718 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_recoveries/package.xml @@ -2,7 +2,7 @@ nav2_recoveries - 0.4.1 + 0.4.2 TODO Carlos Orduno Steve Macenski diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index 3f258402036..585d8a77410 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -123,7 +123,8 @@ bool BackUp::isCollisionFree( while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->linear.x * (cycle_count / cycle_frequency_); - pose2d.x += sim_position_change; + pose2d.x += sim_position_change * cos(pose2d.theta); + pose2d.y += sim_position_change * sin(pose2d.theta); cycle_count++; if (diff_dist - abs(sim_position_change) <= 0.) { diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index 49b7428d1f3..d657a9d0563 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_recoveries/src/recovery_server.cpp @@ -162,13 +162,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_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 431f12a0370..e424873ab42 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 0.4.1 + 0.4.2 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..5d2b5147fc8 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -410,7 +410,7 @@ Nav2Panel::onCancelButtonPressed() 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) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel waypoint follower"); return; @@ -419,7 +419,7 @@ Nav2Panel::onCancelButtonPressed() 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) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal"); return; @@ -523,7 +523,7 @@ 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) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); return; @@ -563,7 +563,7 @@ 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) + 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 eac98acd40d..7c80e085d1a 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 0.4.1 + 0.4.2 TODO Carlos Orduno Apache-2.0 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..a7ae2aa43fa 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" @@ -9,7 +12,7 @@ ament_add_test(test_bt_navigator 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=True + ASTAR=False ) ament_add_test(test_bt_navigator_with_dijkstra @@ -37,7 +40,7 @@ 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 ) # ament_add_test(test_multi_robot diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index dde6618cdc7..92ea252eba0 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -41,7 +41,8 @@ def generate_launch_description(): 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')} + param_substitutions = { + 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR')} configured_params = RewrittenYaml( source_file=params_file, root_key='', diff --git a/nav2_system_tests/src/system/tester_node.py b/nav2_system_tests/src/system/tester_node.py index a66e2f7b89d..83e9df9dc9f 100755 --- a/nav2_system_tests/src/system/tester_node.py +++ b/nav2_system_tests/src/system/tester_node.py @@ -228,12 +228,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/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 57cbbd9f7e9..4b797e694af 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 0.4.1 + 0.4.2 TODO Michael Jeronimo Mohammad Haghighipanah @@ -26,6 +26,7 @@ rclcpp_lifecycle launch launch_testing_ament_cmake + action_msgs libboost-program-options @@ -35,6 +36,7 @@ launch launch_testing_ament_cmake std_srvs + action_msgs ament_cmake 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 082b4d2e07e..e9fe98957f8 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 0.4.1 + 0.4.2 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 2c5f86ea522..4edd90431fa 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 0.4.1 + 0.4.2 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 3a0edabce6e..129c3fcbb18 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -95,13 +95,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*/) { @@ -133,7 +126,10 @@ WaypointFollower::followWaypoints() 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 +153,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 29065f56d8b..ecb24e016a0 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 0.4.1 + 0.4.2 ROS2 Navigation Stack