mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
add methods to traverse through all solutions
This commit is contained in:
parent
751c85cd3c
commit
4955a64181
@ -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;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -82,6 +82,8 @@ int main(int argc, char** argv){
|
|||||||
|
|
||||||
t.plan();
|
t.plan();
|
||||||
|
|
||||||
|
t.publishPlans();
|
||||||
|
|
||||||
//t.printState();
|
//t.printState();
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
|
|||||||
68
src/task.cpp
68
src/task.cpp
@ -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);
|
||||||
|
|
||||||
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user