Connect: skip initial PlanningScene::diff()

This commit is contained in:
Robert Haschke 2018-10-19 23:40:48 +02:00
parent 02ed4d3579
commit 70e32cb0b3
4 changed files with 23 additions and 13 deletions

View File

@ -77,9 +77,10 @@ public:
protected:
SolutionSequencePtr makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes, const InterfaceState& from, const InterfaceState& to);
const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
const InterfaceState& from, const InterfaceState& to);
SubTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
const moveit::core::RobotState& state);
protected:

View File

@ -99,6 +99,7 @@ public:
/// create an InterfaceState from a planning scene
InterfaceState(const planning_scene::PlanningScenePtr& ps);
InterfaceState(const planning_scene::PlanningSceneConstPtr& ps);
/// copy an existing InterfaceState, but not including incoming/outgoing trajectories
InterfaceState(const InterfaceState& other);

View File

@ -141,11 +141,11 @@ void Connect::compute(const InterfaceState &from, const InterfaceState &to) {
MergeMode mode = props.get<MergeMode>("merge_mode");
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState();
std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories;
planning_scene::PlanningScenePtr start = from.scene()->diff();
const moveit::core::RobotState& goal_state = to.scene()->getCurrentState();
std::vector<planning_scene::PlanningScenePtr> intermediate_scenes;
std::vector<planning_scene::PlanningSceneConstPtr> intermediate_scenes;
planning_scene::PlanningSceneConstPtr start = from.scene();
intermediate_scenes.push_back(start);
bool success = false;
@ -153,11 +153,12 @@ void Connect::compute(const InterfaceState &from, const InterfaceState &to) {
for (const GroupPlannerVector::value_type& pair : planner_) {
// set intermediate goal state
planning_scene::PlanningScenePtr end = start->diff();
const moveit::core::JointModelGroup *jmg = final_goal_state.getJointModelGroup(pair.first);
final_goal_state.copyJointGroupPositions(jmg, positions);
robot_state::RobotState& goal_state = end->getCurrentStateNonConst();
goal_state.setJointGroupPositions(jmg, positions);
goal_state.update();
intermediate_scenes.push_back(end);
const moveit::core::JointModelGroup *jmg = goal_state.getJointModelGroup(pair.first);
goal_state.copyJointGroupPositions(jmg, positions);
end->getCurrentStateNonConst().setJointGroupPositions(jmg, positions);
end->getCurrentStateNonConst().update();
robot_trajectory::RobotTrajectoryPtr trajectory;
success = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints);
@ -181,17 +182,17 @@ void Connect::compute(const InterfaceState &from, const InterfaceState &to) {
}
SolutionSequencePtr Connect::makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
const InterfaceState &from, const InterfaceState &to)
{
assert(sub_trajectories.size() + 1 == intermediate_scenes.size());
auto scene_it = intermediate_scenes.begin();
planning_scene::PlanningScenePtr start = *scene_it;
planning_scene::PlanningSceneConstPtr start = *scene_it;
const InterfaceState* state = &from;
SolutionSequence::container_type sub_solutions;
for (const auto &sub : sub_trajectories) {
planning_scene::PlanningScenePtr end = *++scene_it;
planning_scene::PlanningSceneConstPtr end = *++scene_it;
auto inserted = subsolutions_.insert(subsolutions_.end(), SubTrajectory(sub));
inserted->setCreator(pimpl_);
@ -214,7 +215,7 @@ SolutionSequencePtr Connect::makeSequential(const std::vector<robot_trajectory::
}
SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
const moveit::core::RobotState& state)
{
// no need to merge if there is only a single sub trajectory

View File

@ -58,6 +58,13 @@ InterfaceState::InterfaceState(const planning_scene::PlanningScenePtr& ps)
{
}
InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr& ps)
: scene_(ps)
{
if (scene_->getCurrentState().dirty())
ROS_ERROR_NAMED("InterfaceState", "Dirty PlanningScene! Please only forward clean ones into InterfaceState.");
}
InterfaceState::InterfaceState(const InterfaceState &other)
: scene_(other.scene_), properties_(other.properties_), priority_(other.priority_)
{