diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index a4bddaaf..ecbba8c6 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -77,9 +77,10 @@ public: protected: SolutionSequencePtr makeSequential(const std::vector& sub_trajectories, - const std::vector& intermediate_scenes, const InterfaceState& from, const InterfaceState& to); + const std::vector& intermediate_scenes, + const InterfaceState& from, const InterfaceState& to); SubTrajectoryPtr merge(const std::vector& sub_trajectories, - const std::vector& intermediate_scenes, + const std::vector& intermediate_scenes, const moveit::core::RobotState& state); protected: diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 7cf6d9c1..d5c2bf22 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -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); diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 4408b386..2422d1f2 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -141,11 +141,11 @@ void Connect::compute(const InterfaceState &from, const InterfaceState &to) { MergeMode mode = props.get("merge_mode"); const auto& path_constraints = props.get("path_constraints"); + const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState(); std::vector sub_trajectories; - planning_scene::PlanningScenePtr start = from.scene()->diff(); - const moveit::core::RobotState& goal_state = to.scene()->getCurrentState(); - std::vector intermediate_scenes; + std::vector 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& sub_trajectories, - const std::vector& intermediate_scenes, + const std::vector& 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& sub_trajectories, - const std::vector& intermediate_scenes, + const std::vector& intermediate_scenes, const moveit::core::RobotState& state) { // no need to merge if there is only a single sub trajectory diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 9ae9352a..63044bb2 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -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_) {