Merge branches 'robot-model', 'task-move-constructor' and 'const-robot-trajectory'

This commit is contained in:
Robert Haschke 2018-04-09 21:11:19 +02:00
commit 556d9e1a5e
13 changed files with 59 additions and 38 deletions

View File

@ -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);
} }

View File

@ -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);

View File

@ -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:

View File

@ -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_;
};

View File

@ -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;

View File

@ -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

View File

@ -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;

View File

@ -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();

View File

@ -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;
}

View File

@ -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;
}

View File

@ -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;
}

View File

@ -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,

View File

@ -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);
}