mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branches 'robot-model', 'task-move-constructor' and 'const-robot-trajectory'
This commit is contained in:
commit
556d9e1a5e
@ -63,7 +63,7 @@ bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& gro
|
||||
* or created on the fly. This JMG needs to stay alive during the lifetime of the trajectory.
|
||||
* For now, only the trajectory path is considered. Timings, velocities, etc. are ignored.
|
||||
*/
|
||||
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
|
||||
core::JointModelGroup*& merged_group);
|
||||
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||
moveit::core::JointModelGroup*& merged_group);
|
||||
|
||||
} }
|
||||
|
||||
@ -197,7 +197,7 @@ protected:
|
||||
Stage(const Stage&) = delete;
|
||||
|
||||
protected:
|
||||
StagePrivate* const pimpl_; // constness guarantees one initial write
|
||||
StagePrivate* pimpl_;
|
||||
};
|
||||
std::ostream& operator<<(std::ostream& os, const Stage& stage);
|
||||
|
||||
@ -245,21 +245,6 @@ public:
|
||||
const InterfaceState& to,
|
||||
SubTrajectory&& trajectory);
|
||||
|
||||
void sendForward(const InterfaceState& from,
|
||||
InterfaceState&& to,
|
||||
SubTrajectory&& trajectory,
|
||||
double cost) {
|
||||
trajectory.setCost(cost);
|
||||
sendForward(from, std::move(to), std::move(trajectory));
|
||||
}
|
||||
void sendBackward(InterfaceState&& from,
|
||||
const InterfaceState& to,
|
||||
SubTrajectory&& trajectory,
|
||||
double cost) {
|
||||
trajectory.setCost(cost);
|
||||
sendBackward(std::move(from), to, std::move(trajectory));
|
||||
}
|
||||
|
||||
protected:
|
||||
// constructor for use in derived classes
|
||||
PropagatingEitherWay(PropagatingEitherWayPrivate* impl);
|
||||
|
||||
@ -70,9 +70,9 @@ public:
|
||||
void processFailures(const SolutionProcessor &processor) const override;
|
||||
|
||||
protected:
|
||||
SolutionBase* storeSequential(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
|
||||
SolutionBase* storeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes);
|
||||
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
|
||||
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes);
|
||||
|
||||
protected:
|
||||
|
||||
@ -242,8 +242,9 @@ private:
|
||||
/// SubTrajectory connects interface states of ComputeStages
|
||||
class SubTrajectory : public SolutionBase {
|
||||
public:
|
||||
SubTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory = robot_trajectory::RobotTrajectoryPtr())
|
||||
: SolutionBase(), trajectory_(trajectory)
|
||||
SubTrajectory(const robot_trajectory::RobotTrajectoryConstPtr& trajectory = robot_trajectory::RobotTrajectoryConstPtr(),
|
||||
double cost = 0.0)
|
||||
: SolutionBase(nullptr, cost), trajectory_(trajectory)
|
||||
{}
|
||||
|
||||
robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
|
||||
@ -254,7 +255,7 @@ public:
|
||||
|
||||
private:
|
||||
// actual trajectory, might be empty
|
||||
robot_trajectory::RobotTrajectoryPtr trajectory_;
|
||||
robot_trajectory::RobotTrajectoryConstPtr trajectory_;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -64,12 +64,14 @@ MOVEIT_CLASS_FORWARD(Task)
|
||||
*/
|
||||
class Task : protected WrapperBase {
|
||||
public:
|
||||
Task(const std::string& id = "",
|
||||
Stage::pointer &&container = std::make_unique<SerialContainer>("task pipeline"));
|
||||
static planning_pipeline::PlanningPipelinePtr createPlanner(const moveit::core::RobotModelConstPtr &model,
|
||||
const std::string &ns = "move_group",
|
||||
const std::string &planning_plugin_param_name = "planning_plugin",
|
||||
const std::string &adapter_plugins_param_name = "request_adapters");
|
||||
Task(const std::string& id = "",
|
||||
Stage::pointer &&container = std::make_unique<SerialContainer>("task pipeline"));
|
||||
Task(Task &&other);
|
||||
Task& operator=(Task&& other);
|
||||
~Task();
|
||||
|
||||
std::string id() const;
|
||||
|
||||
@ -88,7 +88,7 @@ bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& gro
|
||||
return duplicates.size() > 0;
|
||||
}
|
||||
|
||||
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
|
||||
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||
moveit::core::JointModelGroup*& merged_group)
|
||||
{
|
||||
if (sub_trajectories.size() <= 1)
|
||||
@ -105,7 +105,7 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::R
|
||||
size_t max_num_joints = 0; // maximum number of joints in sub groups
|
||||
size_t num_joints = 0; // sum of joints in all sub groups
|
||||
|
||||
for (const robot_trajectory::RobotTrajectoryPtr& sub : sub_trajectories) {
|
||||
for (const robot_trajectory::RobotTrajectoryConstPtr& sub : sub_trajectories) {
|
||||
if (sub->getRobotModel() != robot_model)
|
||||
throw std::runtime_error("subsolutions refer to multiple robot models");
|
||||
|
||||
@ -153,7 +153,7 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::R
|
||||
size_t index = merged_traj->getWayPointCount();
|
||||
auto merged_state = index == 0 ? std::make_shared<robot_state::RobotState>(robot_model)
|
||||
: std::make_shared<robot_state::RobotState>(merged_traj->getLastWayPoint());
|
||||
for (const robot_trajectory::RobotTrajectoryPtr& sub : sub_trajectories) {
|
||||
for (const robot_trajectory::RobotTrajectoryConstPtr& sub : sub_trajectories) {
|
||||
if (index >= sub->getWayPointCount())
|
||||
continue; // no more waypoints in this sub solution
|
||||
|
||||
|
||||
@ -189,7 +189,7 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
||||
= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
||||
for( auto& tp : trajectory_steps )
|
||||
traj->addSuffixWayPoint(tp, 0.0);
|
||||
sendForward(from, InterfaceState(result_scene), traj);
|
||||
sendForward(from, InterfaceState(result_scene), SubTrajectory(traj));
|
||||
}
|
||||
|
||||
return succeeded;
|
||||
@ -256,7 +256,7 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
|
||||
robot_trajectory::RobotTrajectoryPtr traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
||||
for( auto& tp : trajectory_steps )
|
||||
traj->addPrefixWayPoint(tp, 0.0);
|
||||
sendBackward(InterfaceState(result_scene), to, traj);
|
||||
sendBackward(InterfaceState(result_scene), to, SubTrajectory(traj));
|
||||
}
|
||||
|
||||
return succeeded;
|
||||
|
||||
@ -141,7 +141,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
|
||||
double timeout = props.get<double>("timeout");
|
||||
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
|
||||
|
||||
std::vector<robot_trajectory::RobotTrajectoryPtr> 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();
|
||||
|
||||
@ -188,7 +188,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
|
||||
return !solution->isFailure();
|
||||
}
|
||||
|
||||
SolutionBase* Connect::storeSequential(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
|
||||
SolutionBase* Connect::storeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes)
|
||||
{
|
||||
assert(sub_trajectories.size() + 1 == intermediate_scenes.size());
|
||||
@ -217,7 +217,7 @@ SolutionBase* Connect::storeSequential(const std::vector<robot_trajectory::Robot
|
||||
return &solutions_.back();
|
||||
}
|
||||
|
||||
robot_trajectory::RobotTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
|
||||
robot_trajectory::RobotTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes)
|
||||
{
|
||||
auto jmg = merged_jmg_.get();
|
||||
|
||||
@ -145,7 +145,7 @@ bool Gripper::computeForward(const InterfaceState &from){
|
||||
|
||||
if (!compute(from, to, trajectory, cost, FORWARD))
|
||||
return false;
|
||||
sendForward(from, InterfaceState(to), trajectory, cost);
|
||||
sendForward(from, InterfaceState(to), SubTrajectory(trajectory, cost));
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -157,7 +157,7 @@ bool Gripper::computeBackward(const InterfaceState &to)
|
||||
|
||||
if (!compute(to, from, trajectory, cost, BACKWARD))
|
||||
return false;
|
||||
sendBackward(InterfaceState(from), to, trajectory, cost);
|
||||
sendBackward(InterfaceState(from), to, SubTrajectory(trajectory, cost));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
@ -65,13 +65,13 @@ void ModifyPlanningScene::enableCollisions(const std::string &first, const movei
|
||||
}
|
||||
|
||||
bool ModifyPlanningScene::computeForward(const InterfaceState &from){
|
||||
sendForward(from, apply(from, false), robot_trajectory::RobotTrajectoryPtr());
|
||||
sendForward(from, apply(from, false), SubTrajectory());
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ModifyPlanningScene::computeBackward(const InterfaceState &to)
|
||||
{
|
||||
sendBackward(apply(to, true), to, robot_trajectory::RobotTrajectoryPtr());
|
||||
sendBackward(apply(to, true), to, SubTrajectory());
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
@ -95,7 +95,7 @@ bool Move::compute(const InterfaceState &from, const InterfaceState &to) {
|
||||
return false;
|
||||
|
||||
// finish stage
|
||||
connect(from, to, res.trajectory_);
|
||||
connect(from, to, SubTrajectory(res.trajectory_));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -60,6 +60,22 @@ Task::Task(const std::string& id, ContainerBase::pointer &&container)
|
||||
enableIntrospection(true);
|
||||
}
|
||||
|
||||
Task::Task(Task&& other)
|
||||
: WrapperBase(std::string(), std::make_unique<SerialContainer>())
|
||||
{
|
||||
*this = std::move(other);
|
||||
}
|
||||
|
||||
Task& Task::operator=(Task&& other)
|
||||
{
|
||||
id_ = std::move(other.id_);
|
||||
robot_model_ = std::move(other.robot_model_);
|
||||
introspection_ = std::move(other.introspection_);
|
||||
task_cbs_ = std::move(other.task_cbs_);
|
||||
std::swap(pimpl_, other.pimpl_);
|
||||
return *this;
|
||||
}
|
||||
|
||||
planning_pipeline::PlanningPipelinePtr
|
||||
Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns,
|
||||
const std::string& planning_plugin_param_name,
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
#include <moveit/task_constructor/container_p.h>
|
||||
#include <moveit/task_constructor/stage_p.h>
|
||||
#include <moveit/task_constructor/task.h>
|
||||
|
||||
#include "gtest_value_printers.h"
|
||||
#include <gtest/gtest.h>
|
||||
@ -450,3 +451,19 @@ TEST_F(ParallelTest, init_mismatching) {
|
||||
EXPECT_INIT_FAILURE(false, true, BOTH, GEN);
|
||||
EXPECT_INIT_FAILURE(true, true, BOTH, GEN);
|
||||
}
|
||||
|
||||
|
||||
TEST(Task, move) {
|
||||
Task t1("foo");
|
||||
t1.add(std::make_unique<GeneratorMockup>());
|
||||
t1.add(std::make_unique<GeneratorMockup>());
|
||||
EXPECT_EQ(t1.stages()->numChildren(), 2);
|
||||
|
||||
Task t2 = std::move(t1);
|
||||
EXPECT_EQ(t2.stages()->numChildren(), 2);
|
||||
EXPECT_EQ(t1.stages()->numChildren(), 0);
|
||||
|
||||
t1 = std::move(t2);
|
||||
EXPECT_EQ(t1.stages()->numChildren(), 2);
|
||||
EXPECT_EQ(t2.stages()->numChildren(), 0);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user