diff --git a/src/NeoMpcPlanner.cpp b/src/NeoMpcPlanner.cpp index 2ca1203..dd1a62d 100755 --- a/src/NeoMpcPlanner.cpp +++ b/src/NeoMpcPlanner.cpp @@ -31,7 +31,7 @@ SOFTWARE. #include #include "nav2_util/line_iterator.hpp" #include "nav2_core/goal_checker.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/controller_exceptions.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" @@ -67,13 +67,13 @@ nav_msgs::msg::Path NeoMpcPlanner::transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose) { if (global_plan_.poses.empty()) { - throw nav2_core::PlannerException("Received plan with zero length"); + throw nav2_core::ControllerException("Received plan with zero length"); } // let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { - throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame"); + throw nav2_core::ControllerException("Unable to transform robot pose into global plan's frame"); } // We'll discard points on the plan that are outside the local costmap @@ -128,7 +128,7 @@ nav_msgs::msg::Path NeoMpcPlanner::transformGlobalPlan( global_path_pub_->publish(transformed_plan); if (transformed_plan.poses.empty()) { - throw nav2_core::PlannerException("Resulting plan has 0 poses in it."); + throw nav2_core::ControllerException("Resulting plan has 0 poses in it."); } return transformed_plan; @@ -231,7 +231,7 @@ geometry_msgs::msg::TwistStamped NeoMpcPlanner::computeVelocityCommands( } if (footprint_cost == 255) { - throw nav2_core::PlannerException("MPC detected collision!"); + throw nav2_core::ControllerException("MPC detected collision!"); } carrot_pub_->publish(createCarrotMsg(carrot_pose)); @@ -288,7 +288,7 @@ void NeoMpcPlanner::configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr & p auto node = parent.lock(); node_ = parent; if (!node) { - throw nav2_core::PlannerException("Unable to lock node!"); + throw nav2_core::ControllerException("Unable to lock node!"); } costmap_ros_ = costmap_ros;