Planner issues (#29)

* Uncommented path_start_index_ and planner node now handles failed plan attempts * linting
Pitt-RAS · Nov 4, 2019 · 9318e04 · 9318e04
1 parent 24f8b26
commit 9318e04
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 3 deletions.
diff --git 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
@@ -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_);
             }
         }