Skip to content

Commit

Permalink
eband_local_planner now fails when new frames cannot be added to the …
Browse files Browse the repository at this point in the history
…band. closes #9
  • Loading branch information
piyushk committed Oct 21, 2013
1 parent b58585d commit 5d1a053
Showing 1 changed file with 5 additions and 3 deletions.
8 changes: 5 additions & 3 deletions src/eband_local_planner_ros.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -303,8 +303,10 @@ bool EBandPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
ROS_DEBUG("Sucessfully added frames to band");
plan_start_end_counter_ = plan_start_end_counter;
}
else
ROS_DEBUG("Failed to add frames to existing band");
else {
ROS_WARN("Failed to add frames to existing band");
return false;
}
}
else
ROS_DEBUG("Nothing to add");
Expand All @@ -314,7 +316,7 @@ bool EBandPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
std::vector<eband_local_planner::Bubble> current_band;
if(!eband_->optimizeBand())
{
ROS_DEBUG("Optimization failed - Band invalid - No controls availlable");
ROS_WARN("Optimization failed - Band invalid - No controls availlable");
band_snapped_hack_ = true;
// display current band
if(eband_->getBand(current_band))
Expand Down

0 comments on commit 5d1a053

Please sign in to comment.