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: protected:
SolutionSequencePtr makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories, 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, 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); const moveit::core::RobotState& state);
protected: protected:

View File

@ -99,6 +99,7 @@ public:
/// create an InterfaceState from a planning scene /// create an InterfaceState from a planning scene
InterfaceState(const planning_scene::PlanningScenePtr& ps); InterfaceState(const planning_scene::PlanningScenePtr& ps);
InterfaceState(const planning_scene::PlanningSceneConstPtr& ps);
/// copy an existing InterfaceState, but not including incoming/outgoing trajectories /// copy an existing InterfaceState, but not including incoming/outgoing trajectories
InterfaceState(const InterfaceState& other); 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"); MergeMode mode = props.get<MergeMode>("merge_mode");
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints"); 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; 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); intermediate_scenes.push_back(start);
bool success = false; bool success = false;
@ -153,11 +153,12 @@ void Connect::compute(const InterfaceState &from, const InterfaceState &to) {
for (const GroupPlannerVector::value_type& pair : planner_) { for (const GroupPlannerVector::value_type& pair : planner_) {
// set intermediate goal state // set intermediate goal state
planning_scene::PlanningScenePtr end = start->diff(); 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); 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; robot_trajectory::RobotTrajectoryPtr trajectory;
success = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints); 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, 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) const InterfaceState &from, const InterfaceState &to)
{ {
assert(sub_trajectories.size() + 1 == intermediate_scenes.size()); assert(sub_trajectories.size() + 1 == intermediate_scenes.size());
auto scene_it = intermediate_scenes.begin(); auto scene_it = intermediate_scenes.begin();
planning_scene::PlanningScenePtr start = *scene_it; planning_scene::PlanningSceneConstPtr start = *scene_it;
const InterfaceState* state = &from; const InterfaceState* state = &from;
SolutionSequence::container_type sub_solutions; SolutionSequence::container_type sub_solutions;
for (const auto &sub : sub_trajectories) { 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)); auto inserted = subsolutions_.insert(subsolutions_.end(), SubTrajectory(sub));
inserted->setCreator(pimpl_); 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, 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) const moveit::core::RobotState& state)
{ {
// no need to merge if there is only a single sub trajectory // 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) InterfaceState::InterfaceState(const InterfaceState &other)
: scene_(other.scene_), properties_(other.properties_), priority_(other.priority_) : scene_(other.scene_), properties_(other.properties_), priority_(other.priority_)
{ {