Use Exception Pointers in Locomotor (#47)

DLu · web-flow · commit bfd1f22c23e9 · 2019-06-22T20:14:54.000-04:00
diff --git a/locomotor/doc/ErrorHandling.md b/locomotor/doc/ErrorHandling.md
@@ -5,15 +5,15 @@ Let's take a precise look at what the error handling functions look like.
 using PlannerExceptionCallback = std::function<void (nav_core2::PlannerException, const ros::Duration&)>;
 ```
 
-There are several standard extensions for the base `PlannerException` class in `nav_core2/exceptions.h`. When the function is called, you should be able to check the class of the exception against your known types and react accordingly. For instance,
+There are several standard extensions for the base `PlannerException` class in `nav_core2/exceptions.h` that will be passed as an exception pointer. When the function is called, you should be able to check the class of the exception against your known types and react accordingly. For instance,
 
 ```
-void SomeStateMachine::onGlobalPlanningException(nav_core2::PlannerException e,
+void SomeStateMachine::onGlobalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr,
                                                  const ros::Duration& planning_time)
 {
   try
   {
-    throw e;
+    std::rethrow_exception(e_ptr);
   }
   catch (nav_core2::OccupiedGoalException& e)
   {
@@ -33,12 +33,12 @@ void SomeStateMachine::onGlobalPlanningException(nav_core2::PlannerException e,
 Importantly, the individual planners can implement their own extensions of `PlannerException` and have their version of `StateMachine` handle it however they like.
 
 ```
-void LocomotorBrown::onGlobalPlanningException(nav_core2::PlannerException e,
+void LocomotorBrown::onGlobalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr,
                                                const ros::Duration& planning_time)
 {
   try
   {
-    throw e;
+    std::rethrow_exception(e_ptr);
   }
   catch (LocomotorBrown::TimeTravelException& e)
   {
diff --git a/locomotor/include/locomotor/locomotor.h b/locomotor/include/locomotor/locomotor.h
@@ -55,10 +55,10 @@ namespace locomotor
 {
 // Callback Type Definitions
 using CostmapUpdateCallback = std::function<void (const ros::Duration&)>;
-using CostmapUpdateExceptionCallback = std::function<void (nav_core2::CostmapException, const ros::Duration&)>;
+using CostmapUpdateExceptionCallback = std::function<void (nav_core2::NavCore2ExceptionPtr, const ros::Duration&)>;
 using GlobalPlanCallback = std::function<void (const nav_2d_msgs::Path2D&, const ros::Duration&)>;
 using LocalPlanCallback = std::function<void (const nav_2d_msgs::Twist2DStamped&, const ros::Duration&)>;
-using PlannerExceptionCallback = std::function<void (nav_core2::PlannerException, const ros::Duration&)>;
+using PlannerExceptionCallback = std::function<void (nav_core2::NavCore2ExceptionPtr, const ros::Duration&)>;
 using NavigationCompleteCallback = std::function<void ()>;
 using NavigationFailureCallback = std::function<void (const locomotor_msgs::ResultCode)>;
 
diff --git a/locomotor/src/double_thread_locomotor.cpp b/locomotor/src/double_thread_locomotor.cpp
@@ -38,6 +38,8 @@
 
 namespace locomotor
 {
+using nav_core2::getResultCode;
+
 /**
  * @class DoubleThreadLocomotor
  * @brief Connect the callbacks in Locomotor to do global and local planning on two separate timers
@@ -107,9 +109,9 @@ class DoubleThreadLocomotor
       std::bind(&DoubleThreadLocomotor::onGlobalPlanningException, this, std::placeholders::_1, std::placeholders::_2));
   }
 
-  void onGlobalCostmapException(nav_core2::CostmapException e, const ros::Duration& planning_time)
+  void onGlobalCostmapException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)
   {
-    requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::GLOBAL_COSTMAP, e.getResultCode(),
+    requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::GLOBAL_COSTMAP, getResultCode(e_ptr),
                                             "Global Costmap failure."));
   }
 
@@ -128,10 +130,10 @@ class DoubleThreadLocomotor
     as_.publishFeedback(locomotor_.getNavigationState());
   }
 
-  void onGlobalPlanningException(nav_core2::PlannerException e, const ros::Duration& planning_time)
+  void onGlobalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)
   {
     ROS_ERROR_NAMED("Locomotor", "Global planning error. Giving up.");
-    requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::GLOBAL_PLANNER, e.getResultCode(),
+    requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::GLOBAL_PLANNER, getResultCode(e_ptr),
                                             "Global Planning Failure."));
   }
 
@@ -150,9 +152,9 @@ class DoubleThreadLocomotor
       std::bind(&DoubleThreadLocomotor::onNavigationCompleted, this));
   }
 
-  void onLocalCostmapException(nav_core2::CostmapException e, const ros::Duration& planning_time)
+  void onLocalCostmapException(nav_core2::CostmapException e_ptr, const ros::Duration& planning_time)
   {
-    requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::LOCAL_COSTMAP, e.getResultCode(),
+    requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::LOCAL_COSTMAP, getResultCode(e_ptr),
                                             "Local Costmap failure."));
   }
 
@@ -168,7 +170,7 @@ class DoubleThreadLocomotor
     as_.publishFeedback(locomotor_.getNavigationState());
   }
 
-  void onLocalPlanningException(nav_core2::PlannerException e, const ros::Duration& planning_time)
+  void onLocalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)
   {
     ROS_WARN_NAMED("Locomotor", "Local planning error. Creating new global plan.");
     control_loop_timer_.stop();
diff --git a/locomotor/src/locomotor.cpp b/locomotor/src/locomotor.cpp
@@ -168,7 +168,8 @@ void Locomotor::doCostmapUpdate(nav_core2::Costmap& costmap, Executor& result_ex
   }
   catch (const nav_core2::CostmapException& e)
   {
-    if (fail_cb) result_ex.addCallback(std::bind(fail_cb, e, getTimeDiffFromNow(start_t)));
+    if (fail_cb)
+      result_ex.addCallback(std::bind(fail_cb, std::current_exception(), getTimeDiffFromNow(start_t)));
   }
 }
 
@@ -188,7 +189,8 @@ void Locomotor::makeGlobalPlan(Executor& result_ex, GlobalPlanCallback cb, Plann
   // if we didn't get a plan and we are in the planning state (the robot isn't moving)
   catch (const nav_core2::PlannerException& e)
   {
-    if (fail_cb) result_ex.addCallback(std::bind(fail_cb, e, getTimeDiffFromNow(start_t)));
+    if (fail_cb)
+      result_ex.addCallback(std::bind(fail_cb, std::current_exception(), getTimeDiffFromNow(start_t)));
   }
 }
 
@@ -221,7 +223,8 @@ void Locomotor::makeLocalPlan(Executor& result_ex, LocalPlanCallback cb, Planner
     catch (const nav_core2::PlannerException& e)
     {
       lock.unlock();
-      if (fail_cb) result_ex.addCallback(std::bind(fail_cb, e, getTimeDiffFromNow(start_t)));
+      if (fail_cb)
+        result_ex.addCallback(std::bind(fail_cb, std::current_exception(), getTimeDiffFromNow(start_t)));
     }
   }
 }
diff --git a/locomotor/src/single_thread_locomotor.cpp b/locomotor/src/single_thread_locomotor.cpp
@@ -38,6 +38,8 @@
 
 namespace locomotor
 {
+using nav_core2::getResultCode;
+
 /**
  * @class SingleThreadLocomotor
  * @brief Connect the callbacks in Locomotor to do global planning once and then local planning on a timer
@@ -96,9 +98,9 @@ class SingleThreadLocomotor
       std::bind(&SingleThreadLocomotor::onGlobalPlanningException, this, std::placeholders::_1, std::placeholders::_2));
   }
 
-  void onGlobalCostmapException(nav_core2::CostmapException e, const ros::Duration& planning_time)
+  void onGlobalCostmapException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)
   {
-    requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::GLOBAL_COSTMAP, e.getResultCode(),
+    requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::GLOBAL_COSTMAP, getResultCode(e_ptr),
                                             "Global Costmap failure."));
   }
 
@@ -110,10 +112,10 @@ class SingleThreadLocomotor
     as_.publishFeedback(locomotor_.getNavigationState());
   }
 
-  void onGlobalPlanningException(nav_core2::PlannerException e, const ros::Duration& planning_time)
+  void onGlobalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)
   {
     ROS_ERROR_NAMED("Locomotor", "Global planning error. Giving up.");
-    requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::GLOBAL_PLANNER, e.getResultCode(),
+    requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::GLOBAL_PLANNER, getResultCode(e_ptr),
                                             "Global Planning Failure."));
   }
 
@@ -132,9 +134,9 @@ class SingleThreadLocomotor
       std::bind(&SingleThreadLocomotor::onNavigationCompleted, this));
   }
 
-  void onLocalCostmapException(nav_core2::CostmapException e, const ros::Duration& planning_time)
+  void onLocalCostmapException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)
   {
-    requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::LOCAL_COSTMAP, e.getResultCode(),
+    requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::LOCAL_COSTMAP, getResultCode(e_ptr),
                                             "Local Costmap failure."));
   }
 
@@ -150,7 +152,7 @@ class SingleThreadLocomotor
     as_.publishFeedback(locomotor_.getNavigationState());
   }
 
-  void onLocalPlanningException(nav_core2::PlannerException e, const ros::Duration& planning_time)
+  void onLocalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)
   {
     ROS_WARN_NAMED("Locomotor", "Local planning error. Creating new global plan.");
     control_loop_timer_.stop();
diff --git a/locomove_base/include/locomove_base/locomove_base.h b/locomove_base/include/locomove_base/locomove_base.h
@@ -62,18 +62,18 @@ class LocoMoveBase
   void requestGlobalCostmapUpdate();
 
   void onGlobalCostmapUpdate(const ros::Duration& planning_time);
-  void onGlobalCostmapException(nav_core2::CostmapException e, const ros::Duration& planning_time);
+  void onGlobalCostmapException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time);
 
   void onNewGlobalPlan(nav_2d_msgs::Path2D new_global_plan, const ros::Duration& planning_time);
-  void onGlobalPlanningException(nav_core2::PlannerException e, const ros::Duration& planning_time);
+  void onGlobalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time);
 
   void controlLoopCallback(const ros::TimerEvent& event);
 
   void onLocalCostmapUpdate(const ros::Duration& planning_time);
-  void onLocalCostmapException(nav_core2::CostmapException e, const ros::Duration& planning_time);
+  void onLocalCostmapException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time);
 
   void onNewLocalPlan(nav_2d_msgs::Twist2DStamped new_command, const ros::Duration& planning_time);
-  void onLocalPlanningException(nav_core2::PlannerException e, const ros::Duration& planning_time);
+  void onLocalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time);
 
   void onNavigationCompleted();
   void onNavigationFailure(const locomotor_msgs::ResultCode result);
diff --git a/locomove_base/src/locomove_base.cpp b/locomove_base/src/locomove_base.cpp
@@ -281,7 +281,7 @@ void LocoMoveBase::onGlobalCostmapUpdate(const ros::Duration& planning_time)
     std::bind(&LocoMoveBase::onGlobalPlanningException, this, std::placeholders::_1, std::placeholders::_2));
 }
 
-void LocoMoveBase::onGlobalCostmapException(nav_core2::CostmapException e, const ros::Duration& planning_time)
+void LocoMoveBase::onGlobalCostmapException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)
 {
   // If the planner_frequency is non-zero, the costmap will attempt to update again on its own (via the Timer).
   // If it is zero, then we manually request a new update
@@ -311,7 +311,7 @@ void LocoMoveBase::onNewGlobalPlan(nav_2d_msgs::Path2D new_global_plan, const ro
   control_loop_timer_.start();
 }
 
-void LocoMoveBase::onGlobalPlanningException(nav_core2::PlannerException e, const ros::Duration& planning_time)
+void LocoMoveBase::onGlobalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)
 {
   if (has_global_plan_)
   {
@@ -352,7 +352,7 @@ void LocoMoveBase::onLocalCostmapUpdate(const ros::Duration& planning_time)
     std::bind(&LocoMoveBase::onNavigationCompleted, this));
 }
 
-void LocoMoveBase::onLocalCostmapException(nav_core2::CostmapException e, const ros::Duration& planning_time)
+void LocoMoveBase::onLocalCostmapException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)
 {
   ROS_WARN_NAMED("LocoMoveBase",
                  "Sensor data is out of date, we're not going to allow commanding of the base for safety");
@@ -394,7 +394,7 @@ void LocoMoveBase::onNewLocalPlan(nav_2d_msgs::Twist2DStamped new_command, const
   server_.publishFeedback(feedback);
 }
 
-void LocoMoveBase::onLocalPlanningException(nav_core2::PlannerException e, const ros::Duration& planning_time)
+void LocoMoveBase::onLocalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)
 {
   // check if we've tried to find a valid control for longer than our time limit
   if (ros::Time::now() > last_valid_control_ + ros::Duration(controller_patience_))
diff --git a/nav_core2/CMakeLists.txt b/nav_core2/CMakeLists.txt
@@ -27,6 +27,7 @@ if(CATKIN_ENABLE_TESTING)
   roslint_add_test()
 
   catkin_add_gtest(bounds_test test/bounds_test.cpp)
+  catkin_add_gtest(exception_test test/exception_test.cpp)
 endif()
 
 install(DIRECTORY include/${PROJECT_NAME}/
diff --git a/nav_core2/README.md b/nav_core2/README.md
@@ -46,7 +46,7 @@ Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-pl
 ## Exceptions
 A hierarchical collection of [exceptions](include/nav_core2/exceptions.h) is provided to allow for reacting to navigation failures in a more robust and contextual way.
 ![exception hierarchy tree](doc/exceptions.png)
-Each exception has a corresponding integer "result code" that can be used in ROS interfaces where passing the C++ object is infeasible.
+Each exception has a corresponding integer "result code" that can be used in ROS interfaces where passing the C++ object is infeasible. Note that due to the hierarchy, the result_code will be for the child-most exception. For example, if you throw a `StartBoundsException` which has a corresponding result code of `6`, it could also be seen as a `InvalidStartPoseException`, `GlobalPlannerException`, `PlannerException` or `NavCore2Exception`, all of which would also have the result code of `6`.
 
 ## Bounds
 For use in tracking `Costmap` changes and more, this package also provides an implementation of [bounding boxes](include/nav_core2/bounds.h). These are represented with the ranges `[min_x, max_x]` and `[min_y, max_y]` (inclusive).
diff --git a/nav_core2/doc/exceptions.png b/nav_core2/doc/exceptions.png
diff --git a/nav_core2/include/nav_core2/exceptions.h b/nav_core2/include/nav_core2/exceptions.h
diff --git a/nav_core2/test/exception_test.cpp b/nav_core2/test/exception_test.cpp

Original file line number	Diff line number	Diff line change
`@@ -5,15 +5,15 @@ Let's take a precise look at what the error handling functions look like.`
`5`	`5`	`using PlannerExceptionCallback = std::function<void (nav_core2::PlannerException, const ros::Duration&)>;`
`6`	`6`	```
`7`	`7`
`8`		-There are several standard extensions for the base `PlannerException` class in `nav_core2/exceptions.h`. When the function is called, you should be able to check the class of the exception against your known types and react accordingly. For instance,
	`8`	+There are several standard extensions for the base `PlannerException` class in `nav_core2/exceptions.h` that will be passed as an exception pointer. When the function is called, you should be able to check the class of the exception against your known types and react accordingly. For instance,
`9`	`9`
`10`	`10`	```
`11`		`-void SomeStateMachine::onGlobalPlanningException(nav_core2::PlannerException e,`
	`11`	`+void SomeStateMachine::onGlobalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr,`
`12`	`12`	`const ros::Duration& planning_time)`
`13`	`13`	`{`
`14`	`14`	`try`
`15`	`15`	`{`
`16`		`- throw e;`
	`16`	`+ std::rethrow_exception(e_ptr);`
`17`	`17`	`}`
`18`	`18`	`catch (nav_core2::OccupiedGoalException& e)`
`19`	`19`	`{`
`@@ -33,12 +33,12 @@ void SomeStateMachine::onGlobalPlanningException(nav_core2::PlannerException e,`
`33`	`33`	Importantly, the individual planners can implement their own extensions of `PlannerException` and have their version of `StateMachine` handle it however they like.
`34`	`34`
`35`	`35`	```
`36`		`-void LocomotorBrown::onGlobalPlanningException(nav_core2::PlannerException e,`
	`36`	`+void LocomotorBrown::onGlobalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr,`
`37`	`37`	`const ros::Duration& planning_time)`
`38`	`38`	`{`
`39`	`39`	`try`
`40`	`40`	`{`
`41`		`- throw e;`
	`41`	`+ std::rethrow_exception(e_ptr);`
`42`	`42`	`}`
`43`	`43`	`catch (LocomotorBrown::TimeTravelException& e)`
`44`	`44`	`{`
Original file line number	Diff line number	Diff line change
`@@ -38,6 +38,8 @@`
`38`	`38`
`39`	`39`	`namespace locomotor`
`40`	`40`	`{`
	`41`	`+using nav_core2::getResultCode;`
	`42`	`+`
`41`	`43`	`/**`
`42`	`44`	`* @class DoubleThreadLocomotor`
`43`	`45`	`* @brief Connect the callbacks in Locomotor to do global and local planning on two separate timers`
`@@ -107,9 +109,9 @@ class DoubleThreadLocomotor`
`107`	`109`	`std::bind(&DoubleThreadLocomotor::onGlobalPlanningException, this, std::placeholders::_1, std::placeholders::_2));`
`108`	`110`	`}`
`109`	`111`
`110`		`- void onGlobalCostmapException(nav_core2::CostmapException e, const ros::Duration& planning_time)`
	`112`	`+ void onGlobalCostmapException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)`
`111`	`113`	`{`
`112`		`- requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::GLOBAL_COSTMAP, e.getResultCode(),`
	`114`	`+ requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::GLOBAL_COSTMAP, getResultCode(e_ptr),`
`113`	`115`	`"Global Costmap failure."));`
`114`	`116`	`}`
`115`	`117`
`@@ -128,10 +130,10 @@ class DoubleThreadLocomotor`
`128`	`130`	`as_.publishFeedback(locomotor_.getNavigationState());`
`129`	`131`	`}`
`130`	`132`
`131`		`- void onGlobalPlanningException(nav_core2::PlannerException e, const ros::Duration& planning_time)`
	`133`	`+ void onGlobalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)`
`132`	`134`	`{`
`133`	`135`	`ROS_ERROR_NAMED("Locomotor", "Global planning error. Giving up.");`
`134`		`- requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::GLOBAL_PLANNER, e.getResultCode(),`
	`136`	`+ requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::GLOBAL_PLANNER, getResultCode(e_ptr),`
`135`	`137`	`"Global Planning Failure."));`
`136`	`138`	`}`
`137`	`139`
`@@ -150,9 +152,9 @@ class DoubleThreadLocomotor`
`150`	`152`	`std::bind(&DoubleThreadLocomotor::onNavigationCompleted, this));`
`151`	`153`	`}`
`152`	`154`
`153`		`- void onLocalCostmapException(nav_core2::CostmapException e, const ros::Duration& planning_time)`
	`155`	`+ void onLocalCostmapException(nav_core2::CostmapException e_ptr, const ros::Duration& planning_time)`
`154`	`156`	`{`
`155`		`- requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::LOCAL_COSTMAP, e.getResultCode(),`
	`157`	`+ requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::LOCAL_COSTMAP, getResultCode(e_ptr),`
`156`	`158`	`"Local Costmap failure."));`
`157`	`159`	`}`
`158`	`160`
`@@ -168,7 +170,7 @@ class DoubleThreadLocomotor`
`168`	`170`	`as_.publishFeedback(locomotor_.getNavigationState());`
`169`	`171`	`}`
`170`	`172`
`171`		`- void onLocalPlanningException(nav_core2::PlannerException e, const ros::Duration& planning_time)`
	`173`	`+ void onLocalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)`
`172`	`174`	`{`
`173`	`175`	`ROS_WARN_NAMED("Locomotor", "Local planning error. Creating new global plan.");`
`174`	`176`	`control_loop_timer_.stop();`
Original file line number	Diff line number	Diff line change
`@@ -168,7 +168,8 @@ void Locomotor::doCostmapUpdate(nav_core2::Costmap& costmap, Executor& result_ex`
`168`	`168`	`}`
`169`	`169`	`catch (const nav_core2::CostmapException& e)`
`170`	`170`	`{`
`171`		`- if (fail_cb) result_ex.addCallback(std::bind(fail_cb, e, getTimeDiffFromNow(start_t)));`
	`171`	`+ if (fail_cb)`
	`172`	`+ result_ex.addCallback(std::bind(fail_cb, std::current_exception(), getTimeDiffFromNow(start_t)));`
`172`	`173`	`}`
`173`	`174`	`}`
`174`	`175`
`@@ -188,7 +189,8 @@ void Locomotor::makeGlobalPlan(Executor& result_ex, GlobalPlanCallback cb, Plann`
`188`	`189`	`// if we didn't get a plan and we are in the planning state (the robot isn't moving)`
`189`	`190`	`catch (const nav_core2::PlannerException& e)`
`190`	`191`	`{`
`191`		`- if (fail_cb) result_ex.addCallback(std::bind(fail_cb, e, getTimeDiffFromNow(start_t)));`
	`192`	`+ if (fail_cb)`
	`193`	`+ result_ex.addCallback(std::bind(fail_cb, std::current_exception(), getTimeDiffFromNow(start_t)));`
`192`	`194`	`}`
`193`	`195`	`}`
`194`	`196`
`@@ -221,7 +223,8 @@ void Locomotor::makeLocalPlan(Executor& result_ex, LocalPlanCallback cb, Planner`
`221`	`223`	`catch (const nav_core2::PlannerException& e)`
`222`	`224`	`{`
`223`	`225`	`lock.unlock();`
`224`		`- if (fail_cb) result_ex.addCallback(std::bind(fail_cb, e, getTimeDiffFromNow(start_t)));`
	`226`	`+ if (fail_cb)`
	`227`	`+ result_ex.addCallback(std::bind(fail_cb, std::current_exception(), getTimeDiffFromNow(start_t)));`
`225`	`228`	`}`
`226`	`229`	`}`
`227`	`230`	`}`
Original file line number	Diff line number	Diff line change
`@@ -281,7 +281,7 @@ void LocoMoveBase::onGlobalCostmapUpdate(const ros::Duration& planning_time)`
`281`	`281`	`std::bind(&LocoMoveBase::onGlobalPlanningException, this, std::placeholders::_1, std::placeholders::_2));`
`282`	`282`	`}`
`283`	`283`
`284`		`-void LocoMoveBase::onGlobalCostmapException(nav_core2::CostmapException e, const ros::Duration& planning_time)`
	`284`	`+void LocoMoveBase::onGlobalCostmapException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)`
`285`	`285`	`{`
`286`	`286`	`// If the planner_frequency is non-zero, the costmap will attempt to update again on its own (via the Timer).`
`287`	`287`	`// If it is zero, then we manually request a new update`
`@@ -311,7 +311,7 @@ void LocoMoveBase::onNewGlobalPlan(nav_2d_msgs::Path2D new_global_plan, const ro`
`311`	`311`	`control_loop_timer_.start();`
`312`	`312`	`}`
`313`	`313`
`314`		`-void LocoMoveBase::onGlobalPlanningException(nav_core2::PlannerException e, const ros::Duration& planning_time)`
	`314`	`+void LocoMoveBase::onGlobalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)`
`315`	`315`	`{`
`316`	`316`	`if (has_global_plan_)`
`317`	`317`	`{`
`@@ -352,7 +352,7 @@ void LocoMoveBase::onLocalCostmapUpdate(const ros::Duration& planning_time)`
`352`	`352`	`std::bind(&LocoMoveBase::onNavigationCompleted, this));`
`353`	`353`	`}`
`354`	`354`
`355`		`-void LocoMoveBase::onLocalCostmapException(nav_core2::CostmapException e, const ros::Duration& planning_time)`
	`355`	`+void LocoMoveBase::onLocalCostmapException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)`
`356`	`356`	`{`
`357`	`357`	`ROS_WARN_NAMED("LocoMoveBase",`
`358`	`358`	`"Sensor data is out of date, we're not going to allow commanding of the base for safety");`
`@@ -394,7 +394,7 @@ void LocoMoveBase::onNewLocalPlan(nav_2d_msgs::Twist2DStamped new_command, const`
`394`	`394`	`server_.publishFeedback(feedback);`
`395`	`395`	`}`
`396`	`396`
`397`		`-void LocoMoveBase::onLocalPlanningException(nav_core2::PlannerException e, const ros::Duration& planning_time)`
	`397`	`+void LocoMoveBase::onLocalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration& planning_time)`
`398`	`398`	`{`
`399`	`399`	`// check if we've tried to find a valid control for longer than our time limit`
`400`	`400`	`if (ros::Time::now() > last_valid_control_ + ros::Duration(controller_patience_))`