diff --git a/src/ackermann_planner_ros.cpp b/src/ackermann_planner_ros.cpp index 5bb412f241d18e3553d9032fd0df90a71ee7e5b2..d192bbbc76510011f1d08b94a4f508ea46f254b4 100644 --- a/src/ackermann_planner_ros.cpp +++ b/src/ackermann_planner_ros.cpp @@ -468,10 +468,23 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) { //ROS_INFO_STREAM("linear.x " << cmd_vel.linear.x << " deadzone: " << current_config_.trans_vel_deadzone ); // get the position of the goal - base_local_planner::getGoalPose(*tf_, - transformed_plan, - costmap_ros_->getGlobalFrameID(), - goal_pose); + if(!base_local_planner::getGoalPose(*tf_, + transformed_plan, + costmap_ros_->getGlobalFrameID(), + goal_pose)) + { + ROS_WARN_NAMED("ackermann_local_planner", "Ackermann planner failed to produce path."); + std::vector<geometry_msgs::PoseStamped> empty_plan; + publish_global_plan(empty_plan); + cmd_vel.linear.x = 0.0; + cmd_vel.linear.y = 0.0; + cmd_vel.angular.z = 0.0; + this->stuck=false; + new_segment=false; + replan_count=0; + stuck_count=0; + return false; + } // get the distance to the goal double dist=base_local_planner::getGoalPositionDistance(current_pose_,goal_pose.pose.position.x,goal_pose.pose.position.y); if(dist>current_config_.xy_goal_tolerance && dist<2*current_config_.xy_goal_tolerance)