From 9318e046a2fd3939d9eda3aaaac044fd9ac68666 Mon Sep 17 00:00:00 2001
From: eawilton <46731050+eawilton@users.noreply.github.com>
Date: Sun, 3 Nov 2019 21:19:11 -0500
Subject: [PATCH] Planner issues (#29)

* Uncommented path_start_index_ and planner node now handles failed plan attempts

* linting
---
 src/magellan_motion/src/path_follower/path_follower.cpp | 2 +-
 src/magellan_motion/src/path_planner/planner_node.cpp   | 9 +++++++--
 2 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/magellan_motion/src/path_follower/path_follower.cpp b/src/magellan_motion/src/path_follower/path_follower.cpp
index 563d330..1c2d067 100644
--- a/src/magellan_motion/src/path_follower/path_follower.cpp
+++ b/src/magellan_motion/src/path_follower/path_follower.cpp
@@ -46,7 +46,7 @@ void PathFollower::UpdateUserInput(std_msgs::Int32::ConstPtr input) {
 
 void PathFollower::UpdatePath(nav_msgs::Path::ConstPtr path) {
     current_path_ = path;
-    //path_start_index_ = 0;
+    path_start_index_ = 0;
 }
 
 void PathFollower::Update() {
diff --git a/src/magellan_motion/src/path_planner/planner_node.cpp b/src/magellan_motion/src/path_planner/planner_node.cpp
index b2e672e..f2871e8 100644
--- a/src/magellan_motion/src/path_planner/planner_node.cpp
+++ b/src/magellan_motion/src/path_planner/planner_node.cpp
@@ -97,11 +97,16 @@ int main(int argc, char** argv)
                 ROS_INFO("New goal accepted by planner");
 
                 Path plan = planner.plan(goal_->goal);
-                plan_pub.publish(plan);
 
                 magellan_motion::PlannerRequestResult result_;
 
-                result_.success = true;
+                if ( plan.poses.size() >= 0 ) {
+                    plan_pub.publish(plan);
+                    result_.success = true;
+                } else {
+                    result_.success = false;
+                }
+
                 server.setSucceeded(result_);
             }
         }