BytesRobotics · Michael-Equi · Aug 4, 2020 · Aug 1, 2020 · Aug 4, 2020 · Aug 4, 2020
diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
@@ -66,7 +66,6 @@ class AmclNode : 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;
 
   // Since the sensor data from gazebo or the robot is not lifecycle enabled, we won't
   // respond until we're in the active state

diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
@@ -389,13 +389,6 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
-nav2_util::CallbackReturn
-AmclNode::on_error(const rclcpp_lifecycle::State & /*state*/)
-{
-  RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state");
-  return nav2_util::CallbackReturn::SUCCESS;
-}
-
 nav2_util::CallbackReturn
 AmclNode::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
 {

diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp
@@ -109,7 +109,7 @@ TEST_F(SpinActionTestFixture, test_ports)
     R"(
       <root main_tree_to_execute = "MainTree" >
         <BehaviorTree ID="MainTree">
-            <Spin />
+            <Spin server_name="spin"/>
         </BehaviorTree>
       </root>)";
 

diff --git 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/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -214,6 +214,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();
@@ -222,13 +223,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*/)
 {

diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp
@@ -102,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<Action>;

diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp
@@ -216,25 +216,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 &)
 {

diff --git a/nav2_core/include/nav2_core/progress_checker.hpp b/nav2_core/include/nav2_core/progress_checker.hpp
@@ -55,7 +55,7 @@ class ProgressChecker
   /**
    * @brief Reset class state upon calling
    */
-  virtual void reset() {}
+  virtual void reset() = 0;
 };
 }  // namespace nav2_core
 

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
@@ -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/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 &)
 {

diff --git 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
@@ -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/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<nav2_map_server::MapSaver>();
-    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<nav2_map_server::MapSaver>();
+  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
@@ -95,13 +95,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*/)
 {
@@ -215,7 +208,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
@@ -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<nav2_map_server::MapServer>();
-    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<nav2_map_server::MapServer>();
+  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
@@ -160,13 +160,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_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

diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp
@@ -89,12 +89,6 @@ 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<nav2_msgs::action::ComputePathToPose>;
 

diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
@@ -197,13 +197,6 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state)
   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 &)
 {

diff --git 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<tf2_ros::Buffer> tf_;
   std::shared_ptr<tf2_ros::TransformListener> transform_listener_;

diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp
@@ -169,13 +169,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_util/CMakeLists.txt b/nav2_util/CMakeLists.txt
@@ -17,6 +17,7 @@ find_package(rclcpp_lifecycle REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(bondcpp REQUIRED)
 find_package(bond REQUIRED)
+find_package(action_msgs REQUIRED)
 
 set(dependencies
     nav2_msgs
@@ -32,6 +33,7 @@ set(dependencies
     rclcpp_lifecycle
     bondcpp
     bond
+    action_msgs
 )
 
 nav2_package()

diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp
@@ -123,6 +123,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;
+  }
+
   // bond connection to lifecycle manager
   void createBond();
   void destroyBond();

diff --git a/nav2_util/package.xml b/nav2_util/package.xml
@@ -28,6 +28,7 @@
   <depend>rclcpp_lifecycle</depend>
   <depend>launch</depend>
   <depend>launch_testing_ament_cmake</depend>
+  <depend>action_msgs</depend>
 
   <exec_depend>libboost-program-options</exec_depend>
 
@@ -37,6 +38,7 @@
   <test_depend>launch</test_depend>
   <test_depend>launch_testing_ament_cmake</test_depend>
   <test_depend>std_srvs</test_depend>
+  <test_depend>action_msgs</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp
@@ -516,6 +516,31 @@ TEST_F(ActionTest, test_handle_goal_deactivated)
   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_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