validate merged trajectories

This commit is contained in:
Robert Haschke 2018-05-26 20:11:47 +02:00 committed by v4hn
parent 941e9df737
commit b35aba6e19
3 changed files with 17 additions and 15 deletions

View File

@ -245,12 +245,12 @@ public:
void onNewPropagateSolution(const SolutionBase& s); void onNewPropagateSolution(const SolutionBase& s);
void onNewGeneratorSolution(const SolutionBase& s); void onNewGeneratorSolution(const SolutionBase& s);
void mergeAnyCombination(const ChildSolutionMap& all_solutions, const SolutionBase& current, void mergeAnyCombination(const ChildSolutionMap& all_solutions, const SolutionBase& current,
const moveit::core::RobotState& state, const Spawner& spawner); const planning_scene::PlanningSceneConstPtr& start_scene, const Spawner& spawner);
void merge(const ChildSolutionList& sub_solutions, const moveit::core::RobotState& state, const Spawner& spawner); void merge(const ChildSolutionList& sub_solutions,
const planning_scene::PlanningSceneConstPtr& start_scene, const Spawner& spawner);
void sendForward(SubTrajectory&& t, const InterfaceState* from); void sendForward(SubTrajectory&& t, const InterfaceState* from);
void sendBackward(SubTrajectory&& t, const InterfaceState* to); void sendBackward(SubTrajectory&& t, const InterfaceState* to);
void spawn(SubTrajectory&& t);
}; };
PIMPL_FUNCTIONS(Merger) PIMPL_FUNCTIONS(Merger)

View File

@ -1031,7 +1031,7 @@ void MergerPrivate::onNewPropagateSolution(const SolutionBase& s)
// combine the new solution with all solutions from other children // combine the new solution with all solutions from other children
auto spawner = dir == PROPAGATE_FORWARDS ? &MergerPrivate::sendForward : &MergerPrivate::sendBackward; auto spawner = dir == PROPAGATE_FORWARDS ? &MergerPrivate::sendForward : &MergerPrivate::sendBackward;
mergeAnyCombination(all_solutions, s, external_source_state->scene()->getCurrentState(), mergeAnyCombination(all_solutions, s, external_source_state->scene(),
std::bind(spawner, this, std::placeholders::_1, external_source_state)); std::bind(spawner, this, std::placeholders::_1, external_source_state));
} }
@ -1053,16 +1053,11 @@ void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to)
void MergerPrivate::onNewGeneratorSolution(const SolutionBase& s) void MergerPrivate::onNewGeneratorSolution(const SolutionBase& s)
{ {
// TODO // TODO: implement in similar fashion as onNewPropagateSolution(), but also merge start/end states
}
void MergerPrivate::spawn(SubTrajectory&& t)
{
} }
void MergerPrivate::mergeAnyCombination(const ChildSolutionMap& all_solutions, const SolutionBase& current, void MergerPrivate::mergeAnyCombination(const ChildSolutionMap& all_solutions, const SolutionBase& current,
const moveit::core::RobotState& state, const Spawner& spawner) const planning_scene::PlanningSceneConstPtr& start_scene, const Spawner& spawner)
{ {
std::vector<size_t> indeces; // which solution index was considered last for i-th child? std::vector<size_t> indeces; // which solution index was considered last for i-th child?
indeces.reserve(children().size()); indeces.reserve(children().size());
@ -1077,7 +1072,7 @@ void MergerPrivate::mergeAnyCombination(const ChildSolutionMap& all_solutions, c
sub_solutions.push_back(pair.second[indeces.back()]); sub_solutions.push_back(pair.second[indeces.back()]);
} }
while (true) { while (true) {
merge(sub_solutions, state, spawner); merge(sub_solutions, start_scene, spawner);
// compose next combination // compose next combination
size_t child = 0; size_t child = 0;
@ -1097,7 +1092,8 @@ void MergerPrivate::mergeAnyCombination(const ChildSolutionMap& all_solutions, c
} }
} }
void MergerPrivate::merge(const ChildSolutionList& sub_solutions, const moveit::core::RobotState& state, const Spawner& spawner) void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
const planning_scene::PlanningSceneConstPtr& start_scene, const Spawner& spawner)
{ {
// transform vector of SubTrajectories into vector of RobotTrajectories // transform vector of SubTrajectories into vector of RobotTrajectories
std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories; std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories;
@ -1110,11 +1106,15 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions, const moveit::
} }
moveit::core::JointModelGroup *jmg = jmg_merged_.get(); moveit::core::JointModelGroup *jmg = jmg_merged_.get();
robot_trajectory::RobotTrajectoryPtr merged = task_constructor::merge(sub_trajectories, state, jmg); robot_trajectory::RobotTrajectoryPtr merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg);
if (jmg_merged_.get() != jmg) if (jmg_merged_.get() != jmg)
jmg_merged_.reset(jmg); jmg_merged_.reset(jmg);
if (!merged) return; if (!merged) return;
// check merged trajectory for collisions
if (!start_scene->isPathValid(*merged))
return;
SubTrajectory t(merged); SubTrajectory t(merged);
// accumulate costs and markers // accumulate costs and markers
double costs = 0.0; double costs = 0.0;

View File

@ -227,7 +227,9 @@ SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTraject
if (!trajectory) if (!trajectory)
return SubTrajectoryPtr(); return SubTrajectoryPtr();
// TODO: check merged trajectory for collisions // check merged trajectory for collisions
if (!intermediate_scenes.front()->isPathValid(*trajectory, properties().get<moveit_msgs::Constraints>("path_constraints")))
return SubTrajectoryPtr();
return std::make_shared<SubTrajectory>(trajectory); return std::make_shared<SubTrajectory>(trajectory);
} }