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:
|
||||
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:
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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_)
|
||||
{
|
||||
|
||||
Loading…
Reference in New Issue
Block a user