mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Connect: skip initial PlanningScene::diff()
This commit is contained in:
parent
02ed4d3579
commit
70e32cb0b3
@ -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:
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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_)
|
||||||
{
|
{
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user