diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index 98028570..aaa010dc 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -4,6 +4,8 @@ #include +#include + #include namespace planning_scene { @@ -18,6 +20,10 @@ namespace planning_pipeline { MOVEIT_CLASS_FORWARD(PlanningPipeline); } +namespace robot_trajectory { + MOVEIT_CLASS_FORWARD(RobotTrajectory); +} + namespace moveit::task_constructor { MOVEIT_CLASS_FORWARD(SubTask); @@ -27,6 +33,8 @@ MOVEIT_CLASS_FORWARD(Task); class Task { public: + typedef std::function&)> SolutionCallback; + Task(); ~Task(); @@ -35,6 +43,10 @@ public: bool plan(); + bool processSolutions(SolutionCallback& processor); + + void publishPlans(); + void printState(); protected: @@ -46,6 +58,8 @@ protected: robot_model_loader::RobotModelLoaderPtr rml_; planning_pipeline::PlanningPipelinePtr planner_; + + ros::Publisher pub; }; } diff --git a/src/plan_pick.cpp b/src/plan_pick.cpp index b38775c4..55a4ad9e 100644 --- a/src/plan_pick.cpp +++ b/src/plan_pick.cpp @@ -82,6 +82,8 @@ int main(int argc, char** argv){ t.plan(); + t.publishPlans(); + //t.printState(); return 0; diff --git a/src/task.cpp b/src/task.cpp index 7c304992..7d105ca6 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -4,7 +4,11 @@ #include #include +#include + #include +#include +#include #include moveit::task_constructor::Task::Task(){ @@ -13,6 +17,9 @@ moveit::task_constructor::Task::Task(){ throw Exception("Task failed to construct RobotModel"); ros::NodeHandle h; + + pub = h.advertise("task_plan", 30); + ros::ServiceClient client = h.serviceClient("get_planning_scene"); client.waitForExistence(); @@ -93,3 +100,64 @@ void moveit::task_constructor::Task::printState(){ << std::endl; } } + +namespace { +bool traverseFullTrajectories( + const moveit::task_constructor::SubTrajectory& start, + int nr_of_trajectories, + moveit::task_constructor::Task::SolutionCallback& cb, + std::vector& trace) +{ + if(start.trajectory) + trace.push_back(start.trajectory); + + if(nr_of_trajectories == 1){ + 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; + } + } + + if(start.trajectory) + trace.pop_back(); + + return true; +} +} + +bool moveit::task_constructor::Task::processSolutions(moveit::task_constructor::Task::SolutionCallback& processor ){ + const size_t nr_of_trajectories= subtasks_.size(); + std::vector trace; + trace.reserve(nr_of_trajectories); + for(const SubTrajectory& subtraj : subtasks_.front()->getTrajectories()) + if( !traverseFullTrajectories(subtraj, subtasks_.size(), processor, trace) ) + return false; + return true; +} + +namespace { +bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, std::vector& solution){ + for(auto& t : solution){ + dt.trajectory.emplace_back(); + t->getRobotTrajectoryMsg(dt.trajectory.back()); + } + std::cout << "publishing solution" << std::endl; + pub.publish(dt); + ros::Duration(10.0).sleep(); +} +} + +void moveit::task_constructor::Task::publishPlans(){ + moveit_msgs::DisplayTrajectory dt; + robot_state::robotStateToRobotStateMsg(scene_->getCurrentState(), dt.trajectory_start); + dt.model_id= scene_->getRobotModel()->getName(); + + moveit::task_constructor::Task::SolutionCallback processor= std::bind( + &publishSolution, pub, dt, std::placeholders::_1); + + processSolutions(processor); + +}