Added check return value of getGoalPose

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)