From bc67ecb28ef89863dba214b190bf3f8421e9e7f1 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 28 Oct 2022 12:56:06 +0200
Subject: [PATCH] Added check return value of getGoalPose
---
src/ackermann_planner_ros.cpp | 21 +++++++++++++++++----
1 file changed, 17 insertions(+), 4 deletions(-)
diff --git a/src/ackermann_planner_ros.cpp b/src/ackermann_planner_ros.cpp
index 5bb412f..d192bbb 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)
--
GitLab