From 3c3ae44517f2de1fb6ee49a1dd7d3813c10bf653 Mon Sep 17 00:00:00 2001 From: v4hn Date: Sun, 12 Mar 2017 17:10:11 +0100 Subject: [PATCH] actually handle return values in traversal code... --- src/task.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/task.cpp b/src/task.cpp index 7d105ca6..c06910ca 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -108,16 +108,19 @@ bool traverseFullTrajectories( moveit::task_constructor::Task::SolutionCallback& cb, std::vector& trace) { + bool ret= true; + if(start.trajectory) trace.push_back(start.trajectory); if(nr_of_trajectories == 1){ - cb(trace); + ret= cb(trace); } else if( start.end ){ for( const moveit::task_constructor::SubTrajectory* successor : start.end->next_trajectory ){ if( !traverseFullTrajectories(*successor, nr_of_trajectories-1, cb, trace) ) - return false; + ret= false; + break; } } @@ -147,6 +150,8 @@ bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, st std::cout << "publishing solution" << std::endl; pub.publish(dt); ros::Duration(10.0).sleep(); + + return true; } }