Planner issues (#29)

* Uncommented path_start_index_ and planner node now handles failed plan attempts

* linting

Author: eawilton

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() {

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_);
             }
         }