add methods to traverse through all solutions

This commit is contained in:
v4hn 2017-03-07 14:58:57 +01:00
parent 751c85cd3c
commit 4955a64181
3 changed files with 84 additions and 0 deletions

View File

@ -4,6 +4,8 @@
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <ros/ros.h>
#include <vector> #include <vector>
namespace planning_scene { namespace planning_scene {
@ -18,6 +20,10 @@ namespace planning_pipeline {
MOVEIT_CLASS_FORWARD(PlanningPipeline); MOVEIT_CLASS_FORWARD(PlanningPipeline);
} }
namespace robot_trajectory {
MOVEIT_CLASS_FORWARD(RobotTrajectory);
}
namespace moveit::task_constructor { namespace moveit::task_constructor {
MOVEIT_CLASS_FORWARD(SubTask); MOVEIT_CLASS_FORWARD(SubTask);
@ -27,6 +33,8 @@ MOVEIT_CLASS_FORWARD(Task);
class Task { class Task {
public: public:
typedef std::function<bool(std::vector<robot_trajectory::RobotTrajectoryPtr>&)> SolutionCallback;
Task(); Task();
~Task(); ~Task();
@ -35,6 +43,10 @@ public:
bool plan(); bool plan();
bool processSolutions(SolutionCallback& processor);
void publishPlans();
void printState(); void printState();
protected: protected:
@ -46,6 +58,8 @@ protected:
robot_model_loader::RobotModelLoaderPtr rml_; robot_model_loader::RobotModelLoaderPtr rml_;
planning_pipeline::PlanningPipelinePtr planner_; planning_pipeline::PlanningPipelinePtr planner_;
ros::Publisher pub;
}; };
} }

View File

@ -82,6 +82,8 @@ int main(int argc, char** argv){
t.plan(); t.plan();
t.publishPlans();
//t.printState(); //t.printState();
return 0; return 0;

View File

@ -4,7 +4,11 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <moveit_msgs/GetPlanningScene.h> #include <moveit_msgs/GetPlanningScene.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit/robot_model_loader/robot_model_loader.h> #include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/planning_pipeline/planning_pipeline.h> #include <moveit/planning_pipeline/planning_pipeline.h>
moveit::task_constructor::Task::Task(){ moveit::task_constructor::Task::Task(){
@ -13,6 +17,9 @@ moveit::task_constructor::Task::Task(){
throw Exception("Task failed to construct RobotModel"); throw Exception("Task failed to construct RobotModel");
ros::NodeHandle h; ros::NodeHandle h;
pub = h.advertise<moveit_msgs::DisplayTrajectory>("task_plan", 30);
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene"); ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
client.waitForExistence(); client.waitForExistence();
@ -93,3 +100,64 @@ void moveit::task_constructor::Task::printState(){
<< std::endl; << std::endl;
} }
} }
namespace {
bool traverseFullTrajectories(
const moveit::task_constructor::SubTrajectory& start,
int nr_of_trajectories,
moveit::task_constructor::Task::SolutionCallback& cb,
std::vector<robot_trajectory::RobotTrajectoryPtr>& 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<robot_trajectory::RobotTrajectoryPtr> 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<robot_trajectory::RobotTrajectoryPtr>& 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);
}