mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Stage::init(PlanningScene) -> Stage::init(RobotModel)
This commit is contained in:
parent
2a2d406368
commit
9cd6efe46f
@ -3,7 +3,7 @@
|
|||||||
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||||
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
||||||
|
|
||||||
#include <moveit/task_constructor/stages/current_state.h>
|
#include <moveit/task_constructor/stages/fixed_state.h>
|
||||||
#include <moveit/task_constructor/stages/move_to.h>
|
#include <moveit/task_constructor/stages/move_to.h>
|
||||||
#include <moveit/task_constructor/stages/move_relative.h>
|
#include <moveit/task_constructor/stages/move_relative.h>
|
||||||
#include <moveit/task_constructor/stages/connect.h>
|
#include <moveit/task_constructor/stages/connect.h>
|
||||||
@ -18,7 +18,7 @@
|
|||||||
|
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
|
|
||||||
void spawnObject(planning_scene::PlanningScenePtr scene) {
|
void spawnObject(const planning_scene::PlanningScenePtr& scene) {
|
||||||
moveit_msgs::CollisionObject o;
|
moveit_msgs::CollisionObject o;
|
||||||
o.id= "object";
|
o.id= "object";
|
||||||
o.header.frame_id= "world";
|
o.header.frame_id= "world";
|
||||||
@ -52,7 +52,21 @@ int main(int argc, char** argv){
|
|||||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||||
auto cartesian = std::make_shared<solvers::CartesianPath>();
|
auto cartesian = std::make_shared<solvers::CartesianPath>();
|
||||||
|
|
||||||
t.add(std::make_unique<stages::CurrentState>());
|
Stage* initial_stage = nullptr;
|
||||||
|
// create a fixed initial scene
|
||||||
|
{
|
||||||
|
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
|
||||||
|
auto& state = scene->getCurrentStateNonConst();
|
||||||
|
state.setToDefaultValues(state.getJointModelGroup("left_arm"), "home");
|
||||||
|
state.setToDefaultValues(state.getJointModelGroup("right_arm"), "home");
|
||||||
|
state.update();
|
||||||
|
spawnObject(scene);
|
||||||
|
|
||||||
|
auto initial = std::make_unique<stages::FixedState>();
|
||||||
|
initial_stage = initial.get();
|
||||||
|
initial->setState(scene);
|
||||||
|
t.add(std::move(initial));
|
||||||
|
}
|
||||||
{
|
{
|
||||||
auto stage = std::make_unique<stages::FixCollisionObjects>();
|
auto stage = std::make_unique<stages::FixCollisionObjects>();
|
||||||
stage->restrictDirection(stages::MoveTo::FORWARD);
|
stage->restrictDirection(stages::MoveTo::FORWARD);
|
||||||
@ -97,6 +111,7 @@ int main(int argc, char** argv){
|
|||||||
Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
|
Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
|
||||||
"lh_tool_frame");
|
"lh_tool_frame");
|
||||||
gengrasp->setAngleDelta(M_PI / 10.);
|
gengrasp->setAngleDelta(M_PI / 10.);
|
||||||
|
gengrasp->setMonitoredStage(initial_stage);
|
||||||
|
|
||||||
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
||||||
ik->properties().configureInitFrom(Stage::PARENT, {"group", "eef", "default_pose"});
|
ik->properties().configureInitFrom(Stage::PARENT, {"group", "eef", "default_pose"});
|
||||||
@ -157,12 +172,6 @@ int main(int argc, char** argv){
|
|||||||
}
|
}
|
||||||
|
|
||||||
try {
|
try {
|
||||||
auto scene = t.initScene(ros::Duration(0));
|
|
||||||
auto& state = scene->getCurrentStateNonConst();
|
|
||||||
state.setToDefaultValues(state.getJointModelGroup("left_arm"), "home");
|
|
||||||
state.setToDefaultValues(state.getJointModelGroup("right_arm"), "home");
|
|
||||||
state.update();
|
|
||||||
spawnObject(scene);
|
|
||||||
t.plan();
|
t.plan();
|
||||||
std::cout << "waiting for <enter>\n";
|
std::cout << "waiting for <enter>\n";
|
||||||
char ch;
|
char ch;
|
||||||
|
|||||||
@ -43,7 +43,13 @@ int main(int argc, char** argv){
|
|||||||
|
|
||||||
Task t;
|
Task t;
|
||||||
|
|
||||||
t.add( std::make_unique<stages::CurrentState>("current state") );
|
Stage* initial_stage = nullptr;
|
||||||
|
|
||||||
|
{
|
||||||
|
auto initial = std::make_unique<stages::CurrentState>("current state");
|
||||||
|
initial_stage = initial.get();
|
||||||
|
t.add(std::move(initial));
|
||||||
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move= std::make_unique<stages::Gripper>("open gripper");
|
auto move= std::make_unique<stages::Gripper>("open gripper");
|
||||||
@ -81,6 +87,7 @@ int main(int argc, char** argv){
|
|||||||
gengrasp->setObject("object");
|
gengrasp->setObject("object");
|
||||||
gengrasp->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
|
gengrasp->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
|
||||||
gengrasp->setAngleDelta(.2);
|
gengrasp->setAngleDelta(.2);
|
||||||
|
gengrasp->setMonitoredStage(initial_stage);
|
||||||
|
|
||||||
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
||||||
ik->setEndEffector("left_gripper");
|
ik->setEndEffector("left_gripper");
|
||||||
|
|||||||
@ -46,7 +46,13 @@ int main(int argc, char** argv){
|
|||||||
t.setProperty("planner", std::string("RRTConnectkConfigDefault"));
|
t.setProperty("planner", std::string("RRTConnectkConfigDefault"));
|
||||||
t.setProperty("link", std::string("s_model_tool0"));
|
t.setProperty("link", std::string("s_model_tool0"));
|
||||||
|
|
||||||
t.add(std::make_unique<stages::CurrentState>("current state"));
|
Stage* initial_stage = nullptr;
|
||||||
|
|
||||||
|
{
|
||||||
|
auto initial = std::make_unique<stages::CurrentState>("current state");
|
||||||
|
initial_stage = initial.get();
|
||||||
|
t.add(std::move(initial));
|
||||||
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move = std::make_unique<stages::Gripper>("open gripper");
|
auto move = std::make_unique<stages::Gripper>("open gripper");
|
||||||
@ -82,6 +88,7 @@ int main(int argc, char** argv){
|
|||||||
gengrasp->setObject("object");
|
gengrasp->setObject("object");
|
||||||
gengrasp->setToolToGraspTF(Eigen::Translation3d(.03,0,0), "s_model_tool0");
|
gengrasp->setToolToGraspTF(Eigen::Translation3d(.03,0,0), "s_model_tool0");
|
||||||
gengrasp->setAngleDelta(-.2);
|
gengrasp->setAngleDelta(-.2);
|
||||||
|
gengrasp->setMonitoredStage(initial_stage);
|
||||||
|
|
||||||
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
||||||
ik->properties().configureInitFrom(Stage::PARENT, {"eef"});
|
ik->properties().configureInitFrom(Stage::PARENT, {"eef"});
|
||||||
|
|||||||
@ -62,7 +62,7 @@ public:
|
|||||||
virtual void clear();
|
virtual void clear();
|
||||||
|
|
||||||
void reset() override;
|
void reset() override;
|
||||||
void init(const planning_scene::PlanningSceneConstPtr& scene) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
/// validate connectivity of children (after init() was done)
|
/// validate connectivity of children (after init() was done)
|
||||||
virtual void validateConnectivity() const;
|
virtual void validateConnectivity() const;
|
||||||
|
|
||||||
@ -90,7 +90,7 @@ public:
|
|||||||
SerialContainer(const std::string& name = "serial container");
|
SerialContainer(const std::string& name = "serial container");
|
||||||
|
|
||||||
void reset() override;
|
void reset() override;
|
||||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
|
|
||||||
/// validate connectivity of children (after init() was done)
|
/// validate connectivity of children (after init() was done)
|
||||||
void validateConnectivity() const override;
|
void validateConnectivity() const override;
|
||||||
@ -140,7 +140,7 @@ public:
|
|||||||
ParallelContainerBase(const std::string &name);
|
ParallelContainerBase(const std::string &name);
|
||||||
|
|
||||||
void reset() override;
|
void reset() override;
|
||||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
|
|
||||||
/// validate connectivity of children (after init() was done)
|
/// validate connectivity of children (after init() was done)
|
||||||
void validateConnectivity() const override;
|
void validateConnectivity() const override;
|
||||||
@ -202,7 +202,7 @@ public:
|
|||||||
Fallbacks(const std::string &name) : ParallelContainerBase(name) {}
|
Fallbacks(const std::string &name) : ParallelContainerBase(name) {}
|
||||||
|
|
||||||
void reset() override;
|
void reset() override;
|
||||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
bool compute() override;
|
bool compute() override;
|
||||||
};
|
};
|
||||||
|
|||||||
@ -50,6 +50,10 @@
|
|||||||
inline const Class##Private* pimpl() const; \
|
inline const Class##Private* pimpl() const; \
|
||||||
inline Class##Private* pimpl();
|
inline Class##Private* pimpl();
|
||||||
|
|
||||||
|
namespace moveit { namespace core {
|
||||||
|
MOVEIT_CLASS_FORWARD(RobotModel)
|
||||||
|
} }
|
||||||
|
|
||||||
namespace planning_scene {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene)
|
MOVEIT_CLASS_FORWARD(PlanningScene)
|
||||||
}
|
}
|
||||||
@ -148,7 +152,7 @@ public:
|
|||||||
* The planning scene is the initial planning scene of the task. Use it as is or
|
* The planning scene is the initial planning scene of the task. Use it as is or
|
||||||
* to learn about the robot model.
|
* to learn about the robot model.
|
||||||
*/
|
*/
|
||||||
virtual void init(const planning_scene::PlanningSceneConstPtr& scene);
|
virtual void init(const moveit::core::RobotModelConstPtr& robot_model);
|
||||||
|
|
||||||
const ContainerBase* parent() const;
|
const ContainerBase* parent() const;
|
||||||
const std::string& name() const;
|
const std::string& name() const;
|
||||||
@ -220,7 +224,7 @@ public:
|
|||||||
void restrictDirection(Direction dir);
|
void restrictDirection(Direction dir);
|
||||||
|
|
||||||
void reset() override;
|
void reset() override;
|
||||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
|
|
||||||
virtual bool computeForward(const InterfaceState& from) = 0;
|
virtual bool computeForward(const InterfaceState& from) = 0;
|
||||||
virtual bool computeBackward(const InterfaceState& to) = 0;
|
virtual bool computeBackward(const InterfaceState& to) = 0;
|
||||||
@ -314,7 +318,7 @@ public:
|
|||||||
MonitoringGenerator(const std::string &name = "monitoring generator", Stage* monitored = nullptr);
|
MonitoringGenerator(const std::string &name = "monitoring generator", Stage* monitored = nullptr);
|
||||||
void setMonitoredStage(Stage* monitored);
|
void setMonitoredStage(Stage* monitored);
|
||||||
|
|
||||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
MonitoringGenerator(MonitoringGeneratorPrivate* impl);
|
MonitoringGenerator(MonitoringGeneratorPrivate* impl);
|
||||||
|
|||||||
@ -47,7 +47,7 @@ class Connect : public Connecting {
|
|||||||
public:
|
public:
|
||||||
Connect(std::string name, const solvers::PlannerInterfacePtr &planner);
|
Connect(std::string name, const solvers::PlannerInterfacePtr &planner);
|
||||||
|
|
||||||
void init(const planning_scene::PlanningSceneConstPtr &scene);
|
void init(const moveit::core::RobotModelConstPtr& robot_model);
|
||||||
bool compute(const InterfaceState &from, const InterfaceState &to);
|
bool compute(const InterfaceState &from, const InterfaceState &to);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|||||||
@ -49,7 +49,7 @@ class CurrentState : public Generator {
|
|||||||
public:
|
public:
|
||||||
CurrentState(const std::string& name = "current state");
|
CurrentState(const std::string& name = "current state");
|
||||||
|
|
||||||
void init(const planning_scene::PlanningSceneConstPtr& scene) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
bool compute() override;
|
bool compute() override;
|
||||||
|
|
||||||
@ -58,8 +58,8 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
moveit::core::RobotModelConstPtr robot_model_;
|
||||||
planning_scene::PlanningScenePtr scene_;
|
planning_scene::PlanningScenePtr scene_;
|
||||||
bool ran_ = false;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -43,11 +43,10 @@
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
class GenerateGraspPose : public Generator {
|
class GenerateGraspPose : public MonitoringGenerator {
|
||||||
public:
|
public:
|
||||||
GenerateGraspPose(std::string name);
|
GenerateGraspPose(const std::string& name);
|
||||||
|
|
||||||
void init(const planning_scene::PlanningSceneConstPtr& scene) override;
|
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
bool compute() override;
|
bool compute() override;
|
||||||
|
|
||||||
@ -64,6 +63,9 @@ public:
|
|||||||
}
|
}
|
||||||
void setAngleDelta(double delta);
|
void setAngleDelta(double delta);
|
||||||
|
|
||||||
|
protected:
|
||||||
|
void onNewSolution(const SolutionBase& s) override;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
planning_scene::PlanningScenePtr scene_;
|
planning_scene::PlanningScenePtr scene_;
|
||||||
double current_angle_ = 0.0;
|
double current_angle_ = 0.0;
|
||||||
|
|||||||
@ -50,7 +50,7 @@ class Gripper : public PropagatingEitherWay {
|
|||||||
public:
|
public:
|
||||||
Gripper(std::string name);
|
Gripper(std::string name);
|
||||||
|
|
||||||
void init(const planning_scene::PlanningSceneConstPtr &scene);
|
void init(const moveit::core::RobotModelConstPtr& robot_model);
|
||||||
bool computeForward(const InterfaceState& from) override;
|
bool computeForward(const InterfaceState& from) override;
|
||||||
bool computeBackward(const InterfaceState& to) override;
|
bool computeBackward(const InterfaceState& to) override;
|
||||||
|
|
||||||
|
|||||||
@ -46,7 +46,7 @@ class Move : public Connecting {
|
|||||||
public:
|
public:
|
||||||
Move(std::string name);
|
Move(std::string name);
|
||||||
|
|
||||||
void init(const planning_scene::PlanningSceneConstPtr &scene);
|
void init(const moveit::core::RobotModelConstPtr& robot_model);
|
||||||
bool compute(const InterfaceState &from, const InterfaceState &to);
|
bool compute(const InterfaceState &from, const InterfaceState &to);
|
||||||
|
|
||||||
void setGroup(const std::string &group);
|
void setGroup(const std::string &group);
|
||||||
|
|||||||
@ -50,7 +50,7 @@ class MoveRelative : public PropagatingEitherWay {
|
|||||||
public:
|
public:
|
||||||
MoveRelative(std::string name, const solvers::PlannerInterfacePtr& planner);
|
MoveRelative(std::string name, const solvers::PlannerInterfacePtr& planner);
|
||||||
|
|
||||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
bool computeForward(const InterfaceState& from) override;
|
bool computeForward(const InterfaceState& from) override;
|
||||||
bool computeBackward(const InterfaceState& to) override;
|
bool computeBackward(const InterfaceState& to) override;
|
||||||
|
|
||||||
|
|||||||
@ -49,7 +49,7 @@ class MoveTo : public PropagatingEitherWay {
|
|||||||
public:
|
public:
|
||||||
MoveTo(std::string name, const solvers::PlannerInterfacePtr& planner);
|
MoveTo(std::string name, const solvers::PlannerInterfacePtr& planner);
|
||||||
|
|
||||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
bool computeForward(const InterfaceState& from) override;
|
bool computeForward(const InterfaceState& from) override;
|
||||||
bool computeBackward(const InterfaceState& to) override;
|
bool computeBackward(const InterfaceState& to) override;
|
||||||
|
|
||||||
|
|||||||
@ -46,9 +46,6 @@
|
|||||||
|
|
||||||
#include <moveit/macros/class_forward.h>
|
#include <moveit/macros/class_forward.h>
|
||||||
|
|
||||||
namespace robot_model_loader {
|
|
||||||
MOVEIT_CLASS_FORWARD(RobotModelLoader)
|
|
||||||
}
|
|
||||||
namespace moveit { namespace core {
|
namespace moveit { namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(RobotModel)
|
MOVEIT_CLASS_FORWARD(RobotModel)
|
||||||
MOVEIT_CLASS_FORWARD(RobotState)
|
MOVEIT_CLASS_FORWARD(RobotState)
|
||||||
@ -76,7 +73,7 @@ public:
|
|||||||
~Task();
|
~Task();
|
||||||
|
|
||||||
std::string id() const;
|
std::string id() const;
|
||||||
const moveit::core::RobotModelPtr getRobotModel() const;
|
const moveit::core::RobotModelConstPtr getRobotModel() const { return robot_model_; }
|
||||||
|
|
||||||
void add(Stage::pointer &&stage);
|
void add(Stage::pointer &&stage);
|
||||||
void clear() override;
|
void clear() override;
|
||||||
@ -92,14 +89,10 @@ public:
|
|||||||
/// remove function callback
|
/// remove function callback
|
||||||
void erase(TaskCallbackList::const_iterator which);
|
void erase(TaskCallbackList::const_iterator which);
|
||||||
|
|
||||||
/// initialize planning scene from get_planning_scene service (waiting given timeout for it)
|
|
||||||
/// if service is not available or timeout is zero, use an empty planning scene
|
|
||||||
planning_scene::PlanningScenePtr initScene(ros::Duration timeout = ros::Duration(-1));
|
|
||||||
|
|
||||||
/// reset all stages
|
/// reset all stages
|
||||||
void reset() override;
|
void reset() override;
|
||||||
/// initialize all stages with given scene
|
/// initialize all stages with given scene
|
||||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
void init(const moveit::core::RobotModelConstPtr& model) override;
|
||||||
|
|
||||||
/// reset, init scene (if not yet done), and init all stages, then start planning
|
/// reset, init scene (if not yet done), and init all stages, then start planning
|
||||||
bool plan();
|
bool plan();
|
||||||
@ -124,15 +117,13 @@ public:
|
|||||||
void setProperty(const std::string& name, const boost::any& value);
|
void setProperty(const std::string& name, const boost::any& value);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
void initModel();
|
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
bool compute() override;
|
bool compute() override;
|
||||||
void onNewSolution(const SolutionBase &s) override;
|
void onNewSolution(const SolutionBase &s) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
std::string id_;
|
std::string id_;
|
||||||
robot_model_loader::RobotModelLoaderPtr rml_;
|
moveit::core::RobotModelConstPtr robot_model_;
|
||||||
planning_scene::PlanningScenePtr scene_; // initial scene
|
|
||||||
|
|
||||||
// introspection and monitoring
|
// introspection and monitoring
|
||||||
std::unique_ptr<Introspection> introspection_;
|
std::unique_ptr<Introspection> introspection_;
|
||||||
|
|||||||
@ -198,12 +198,12 @@ void ContainerBase::reset()
|
|||||||
Stage::reset();
|
Stage::reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
auto& children = impl->children();
|
auto& children = impl->children();
|
||||||
|
|
||||||
Stage::init(scene);
|
Stage::init(robot_model);
|
||||||
|
|
||||||
// we need to have some children to do the actual work
|
// we need to have some children to do the actual work
|
||||||
if (children.empty())
|
if (children.empty())
|
||||||
@ -212,7 +212,7 @@ void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
|||||||
// recursively init all children and accumulate errors
|
// recursively init all children and accumulate errors
|
||||||
InitStageException errors;
|
InitStageException errors;
|
||||||
for (auto& child : children) {
|
for (auto& child : children) {
|
||||||
try { child->init(scene); } catch (InitStageException &e) { errors.append(e); }
|
try { child->init(robot_model); } catch (InitStageException &e) { errors.append(e); }
|
||||||
}
|
}
|
||||||
|
|
||||||
if (errors) throw errors;
|
if (errors) throw errors;
|
||||||
@ -408,14 +408,14 @@ bool SerialContainerPrivate::connect(container_type::const_iterator cur)
|
|||||||
* This context is provided by two stages pushing from both ends
|
* This context is provided by two stages pushing from both ends
|
||||||
* into a (potentially long) sequence of propagating stages (tbd).
|
* into a (potentially long) sequence of propagating stages (tbd).
|
||||||
*/
|
*/
|
||||||
void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
|
void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
// reset pull interfaces
|
// reset pull interfaces
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
impl->starts_.reset();
|
impl->starts_.reset();
|
||||||
impl->ends_.reset();
|
impl->ends_.reset();
|
||||||
|
|
||||||
ContainerBase::init(scene); // throws if there are no children
|
ContainerBase::init(robot_model); // throws if there are no children
|
||||||
|
|
||||||
auto start = impl->children().begin();
|
auto start = impl->children().begin();
|
||||||
auto last = --impl->children().end();
|
auto last = --impl->children().end();
|
||||||
@ -703,10 +703,10 @@ void ParallelContainerBase::reset()
|
|||||||
/* States received by the container need to be copied to all children's pull interfaces.
|
/* States received by the container need to be copied to all children's pull interfaces.
|
||||||
* States generated by children can be directly forwarded into the container's push interfaces.
|
* States generated by children can be directly forwarded into the container's push interfaces.
|
||||||
*/
|
*/
|
||||||
void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
void ParallelContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
// recursively init children
|
// recursively init children
|
||||||
ContainerBase::init(scene);
|
ContainerBase::init(robot_model);
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
|
|
||||||
// determine the union of interfaces required by children
|
// determine the union of interfaces required by children
|
||||||
@ -886,9 +886,9 @@ void Fallbacks::reset()
|
|||||||
ParallelContainerBase::reset();
|
ParallelContainerBase::reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Fallbacks::init(const planning_scene::PlanningSceneConstPtr &scene)
|
void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
ParallelContainerBase::init(scene);
|
ParallelContainerBase::init(robot_model);
|
||||||
active_child_ = pimpl()->children().front().get();
|
active_child_ = pimpl()->children().front().get();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -128,7 +128,7 @@ void Stage::reset()
|
|||||||
impl->properties_.reset();
|
impl->properties_.reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Stage::init(const planning_scene::PlanningSceneConstPtr &scene)
|
void Stage::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
// init properties once from parent
|
// init properties once from parent
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
@ -390,9 +390,9 @@ void PropagatingEitherWay::reset()
|
|||||||
ComputeBase::reset();
|
ComputeBase::reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr& scene)
|
void PropagatingEitherWay::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
Stage::init(scene);
|
Stage::init(robot_model);
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
|
|
||||||
// In AUTO-mode, i.e. when auto-detecting direction of propagation from context,
|
// In AUTO-mode, i.e. when auto-detecting direction of propagation from context,
|
||||||
@ -524,7 +524,7 @@ void MonitoringGenerator::setMonitoredStage(Stage* monitored)
|
|||||||
impl->monitored_ = monitored;
|
impl->monitored_ = monitored;
|
||||||
}
|
}
|
||||||
|
|
||||||
void MonitoringGenerator::init(const planning_scene::PlanningSceneConstPtr& scene)
|
void MonitoringGenerator::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
Generator::init(robot_model);
|
Generator::init(robot_model);
|
||||||
|
|
||||||
|
|||||||
@ -50,10 +50,10 @@ Connect::Connect(std::string name, const solvers::PlannerInterfacePtr& planner)
|
|||||||
p.declare<std::string>("group", "name of planning group");
|
p.declare<std::string>("group", "name of planning group");
|
||||||
}
|
}
|
||||||
|
|
||||||
void Connect::init(const planning_scene::PlanningSceneConstPtr &scene)
|
void Connect::init(const core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
Connecting::init(scene);
|
Connecting::init(robot_model);
|
||||||
planner_->init(scene->getRobotModel());
|
planner_->init(robot_model);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
|
bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
|
||||||
|
|||||||
@ -53,19 +53,19 @@ CurrentState::CurrentState(const std::string &name)
|
|||||||
p.declare<ros::Duration>("timeout", ros::Duration(-1), "max time to wait for get_planning_scene service");
|
p.declare<ros::Duration>("timeout", ros::Duration(-1), "max time to wait for get_planning_scene service");
|
||||||
}
|
}
|
||||||
|
|
||||||
void CurrentState::init(const planning_scene::PlanningSceneConstPtr &scene)
|
void CurrentState::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
Generator::init(scene);
|
Generator::init(robot_model);
|
||||||
scene_ = scene->diff();
|
robot_model_ = robot_model;
|
||||||
|
scene_.reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool CurrentState::canCompute() const{
|
bool CurrentState::canCompute() const{
|
||||||
return !ran_;
|
return !scene_;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool CurrentState::compute(){
|
bool CurrentState::compute() {
|
||||||
auto rml = std::make_shared<robot_model_loader::RobotModelLoader>();
|
scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
|
||||||
scene_ = std::make_shared<planning_scene::PlanningScene>(rml->getModel());
|
|
||||||
|
|
||||||
ros::NodeHandle h;
|
ros::NodeHandle h;
|
||||||
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
|
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
|
||||||
@ -90,7 +90,6 @@ bool CurrentState::compute(){
|
|||||||
if (client.call(req, res)) {
|
if (client.call(req, res)) {
|
||||||
scene_->setPlanningSceneMsg(res.scene);
|
scene_->setPlanningSceneMsg(res.scene);
|
||||||
spawn(InterfaceState(scene_), 0.0);
|
spawn(InterfaceState(scene_), 0.0);
|
||||||
ran_= true;
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -46,8 +46,8 @@
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
GenerateGraspPose::GenerateGraspPose(std::string name)
|
GenerateGraspPose::GenerateGraspPose(const std::string& name)
|
||||||
: Generator(name)
|
: MonitoringGenerator(name)
|
||||||
{
|
{
|
||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<std::string>("eef", "name of end-effector group");
|
p.declare<std::string>("eef", "name of end-effector group");
|
||||||
@ -57,12 +57,6 @@ GenerateGraspPose::GenerateGraspPose(std::string name)
|
|||||||
p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
|
p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
|
||||||
}
|
}
|
||||||
|
|
||||||
void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene)
|
|
||||||
{
|
|
||||||
Generator::init(scene);
|
|
||||||
scene_ = scene->diff();
|
|
||||||
}
|
|
||||||
|
|
||||||
void GenerateGraspPose::setEndEffector(const std::string &eef){
|
void GenerateGraspPose::setEndEffector(const std::string &eef){
|
||||||
setProperty("eef", eef);
|
setProperty("eef", eef);
|
||||||
}
|
}
|
||||||
@ -91,8 +85,15 @@ void GenerateGraspPose::setAngleDelta(double delta){
|
|||||||
setProperty("angle_delta", delta);
|
setProperty("angle_delta", delta);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void GenerateGraspPose::onNewSolution(const SolutionBase& s)
|
||||||
|
{
|
||||||
|
if (scene_)
|
||||||
|
ROS_WARN_NAMED("GenerateGraspPose", "got additional solution from monitored stage");
|
||||||
|
scene_ = s.end()->scene()->diff();
|
||||||
|
}
|
||||||
|
|
||||||
bool GenerateGraspPose::canCompute() const{
|
bool GenerateGraspPose::canCompute() const{
|
||||||
return current_angle_ < 2*M_PI && current_angle_ > -2*M_PI;
|
return scene_ && current_angle_ < 2*M_PI && current_angle_ > -2*M_PI;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool GenerateGraspPose::compute(){
|
bool GenerateGraspPose::compute(){
|
||||||
|
|||||||
@ -59,10 +59,10 @@ Gripper::Gripper(std::string name)
|
|||||||
p.declare<std::string>("grasp_object", "name of grasp object");
|
p.declare<std::string>("grasp_object", "name of grasp object");
|
||||||
}
|
}
|
||||||
|
|
||||||
void Gripper::init(const planning_scene::PlanningSceneConstPtr &scene)
|
void Gripper::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
PropagatingEitherWay::init(scene);
|
PropagatingEitherWay::init(robot_model);
|
||||||
planner_ = Task::createPlanner(scene->getRobotModel());
|
planner_ = Task::createPlanner(robot_model);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Gripper::setEndEffector(std::string eef){
|
void Gripper::setEndEffector(std::string eef){
|
||||||
|
|||||||
@ -58,10 +58,10 @@ Move::Move(std::string name)
|
|||||||
p.declare<std::string>("planner", "", "planner id");
|
p.declare<std::string>("planner", "", "planner id");
|
||||||
}
|
}
|
||||||
|
|
||||||
void Move::init(const planning_scene::PlanningSceneConstPtr &scene)
|
void Move::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
Connecting::init(scene);
|
Connecting::init(robot_model);
|
||||||
planner_ = Task::createPlanner(scene->getRobotModel());
|
planner_ = Task::createPlanner(robot_model);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Move::setGroup(const std::string& group){
|
void Move::setGroup(const std::string& group){
|
||||||
|
|||||||
@ -90,10 +90,10 @@ void MoveRelative::along(const geometry_msgs::Vector3Stamped &direction)
|
|||||||
setProperty("direction", direction);
|
setProperty("direction", direction);
|
||||||
}
|
}
|
||||||
|
|
||||||
void MoveRelative::init(const planning_scene::PlanningSceneConstPtr &scene)
|
void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
PropagatingEitherWay::init(scene);
|
PropagatingEitherWay::init(robot_model);
|
||||||
planner_->init(scene->getRobotModel());
|
planner_->init(robot_model);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
|
bool MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
|
||||||
|
|||||||
@ -79,10 +79,10 @@ void MoveTo::setGoal(const std::string &joint_pose)
|
|||||||
setProperty("joint_pose", joint_pose);
|
setProperty("joint_pose", joint_pose);
|
||||||
}
|
}
|
||||||
|
|
||||||
void MoveTo::init(const planning_scene::PlanningSceneConstPtr &scene)
|
void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||||
{
|
{
|
||||||
PropagatingEitherWay::init(scene);
|
PropagatingEitherWay::init(robot_model);
|
||||||
planner_->init(scene->getRobotModel());
|
planner_->init(robot_model);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
|
bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
|
||||||
|
|||||||
@ -43,8 +43,6 @@
|
|||||||
|
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
|
|
||||||
#include <moveit_msgs/GetPlanningScene.h>
|
|
||||||
|
|
||||||
#include <moveit/robot_model_loader/robot_model_loader.h>
|
#include <moveit/robot_model_loader/robot_model_loader.h>
|
||||||
#include <moveit/planning_pipeline/planning_pipeline.h>
|
#include <moveit/planning_pipeline/planning_pipeline.h>
|
||||||
|
|
||||||
@ -55,7 +53,10 @@ namespace moveit { namespace task_constructor {
|
|||||||
Task::Task(const std::string& id, ContainerBase::pointer &&container)
|
Task::Task(const std::string& id, ContainerBase::pointer &&container)
|
||||||
: WrapperBase(std::string(), std::move(container)), id_(id)
|
: WrapperBase(std::string(), std::move(container)), id_(id)
|
||||||
{
|
{
|
||||||
initModel();
|
robot_model_loader::RobotModelLoader rml;
|
||||||
|
robot_model_ = rml.getModel();
|
||||||
|
if (!robot_model_)
|
||||||
|
throw Exception("Task failed to construct RobotModel");
|
||||||
|
|
||||||
// monitor state on commandline
|
// monitor state on commandline
|
||||||
//addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout)));
|
//addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout)));
|
||||||
@ -63,47 +64,6 @@ Task::Task(const std::string& id, ContainerBase::pointer &&container)
|
|||||||
enableIntrospection(true);
|
enableIntrospection(true);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Task::initModel () {
|
|
||||||
rml_.reset(new robot_model_loader::RobotModelLoader);
|
|
||||||
if( !rml_->getModel() )
|
|
||||||
throw Exception("Task failed to construct RobotModel");
|
|
||||||
}
|
|
||||||
|
|
||||||
const moveit::core::RobotModelPtr Task::getRobotModel() const {
|
|
||||||
return rml_ ? rml_->getModel() : moveit::core::RobotModelPtr();
|
|
||||||
}
|
|
||||||
|
|
||||||
planning_scene::PlanningScenePtr Task::initScene(ros::Duration timeout) {
|
|
||||||
assert(rml_);
|
|
||||||
|
|
||||||
scene_ = std::make_shared<planning_scene::PlanningScene>(rml_->getModel());
|
|
||||||
|
|
||||||
ros::NodeHandle h;
|
|
||||||
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
|
|
||||||
if (timeout != ros::Duration() && client.waitForExistence(timeout)) {
|
|
||||||
moveit_msgs::GetPlanningScene::Request req;
|
|
||||||
moveit_msgs::GetPlanningScene::Response res;
|
|
||||||
|
|
||||||
req.components.components =
|
|
||||||
moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS
|
|
||||||
| moveit_msgs::PlanningSceneComponents::ROBOT_STATE
|
|
||||||
| moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS
|
|
||||||
| moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES
|
|
||||||
| moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY
|
|
||||||
| moveit_msgs::PlanningSceneComponents::OCTOMAP
|
|
||||||
| moveit_msgs::PlanningSceneComponents::TRANSFORMS
|
|
||||||
| moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX
|
|
||||||
| moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING
|
|
||||||
| moveit_msgs::PlanningSceneComponents::OBJECT_COLORS;
|
|
||||||
|
|
||||||
if (client.call(req, res))
|
|
||||||
std::const_pointer_cast<planning_scene::PlanningScene>(scene_)->setPlanningSceneMsg(res.scene);
|
|
||||||
return scene_;
|
|
||||||
}
|
|
||||||
ROS_WARN("failed to acquire current PlanningScene, using empty one");
|
|
||||||
return scene_;
|
|
||||||
}
|
|
||||||
|
|
||||||
planning_pipeline::PlanningPipelinePtr
|
planning_pipeline::PlanningPipelinePtr
|
||||||
Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns,
|
Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns,
|
||||||
const std::string& planning_plugin_param_name,
|
const std::string& planning_plugin_param_name,
|
||||||
@ -184,7 +144,7 @@ void Task::reset()
|
|||||||
WrapperBase::reset();
|
WrapperBase::reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Task::init(const planning_scene::PlanningSceneConstPtr &scene)
|
void Task::init(const moveit::core::RobotModelConstPtr& model)
|
||||||
{
|
{
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
// initialize push connections of wrapped child
|
// initialize push connections of wrapped child
|
||||||
@ -193,7 +153,7 @@ void Task::init(const planning_scene::PlanningSceneConstPtr &scene)
|
|||||||
child->setNextStarts(impl->pendingForward());
|
child->setNextStarts(impl->pendingForward());
|
||||||
|
|
||||||
// and *afterwards* initialize all children recursively
|
// and *afterwards* initialize all children recursively
|
||||||
stages()->init(scene);
|
stages()->init(model);
|
||||||
// task expects its wrapped child to push to both ends, this triggers interface resolution
|
// task expects its wrapped child to push to both ends, this triggers interface resolution
|
||||||
stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE}));
|
stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE}));
|
||||||
// and *finally* validate connectivity
|
// and *finally* validate connectivity
|
||||||
@ -223,8 +183,7 @@ bool Task::compute()
|
|||||||
bool Task::plan()
|
bool Task::plan()
|
||||||
{
|
{
|
||||||
reset();
|
reset();
|
||||||
if (!scene_) initScene();
|
init(robot_model_);
|
||||||
init(scene_);
|
|
||||||
|
|
||||||
while(ros::ok() && canCompute()) {
|
while(ros::ok() && canCompute()) {
|
||||||
if (compute()) {
|
if (compute()) {
|
||||||
|
|||||||
@ -68,7 +68,7 @@ void append(ContainerBase& c, const std::initializer_list<StageType>& types, int
|
|||||||
|
|
||||||
class SerialTest : public ::testing::Test {
|
class SerialTest : public ::testing::Test {
|
||||||
protected:
|
protected:
|
||||||
planning_scene::PlanningScenePtr scene;
|
moveit::core::RobotModelConstPtr robot_model;
|
||||||
SerialContainer serial;
|
SerialContainer serial;
|
||||||
InterfacePtr dummy;
|
InterfacePtr dummy;
|
||||||
|
|
||||||
@ -112,7 +112,7 @@ protected:
|
|||||||
reset(start, end);
|
reset(start, end);
|
||||||
append(serial, types);
|
append(serial, types);
|
||||||
try {
|
try {
|
||||||
serial.init(scene);
|
serial.init(robot_model);
|
||||||
// prune pull interfaces based on provided external interface (start, end)
|
// prune pull interfaces based on provided external interface (start, end)
|
||||||
InterfaceFlags accepted;
|
InterfaceFlags accepted;
|
||||||
if (start) accepted |= WRITES_PREV_END;
|
if (start) accepted |= WRITES_PREV_END;
|
||||||
@ -141,7 +141,7 @@ TEST_F(SerialTest, insertion_order) {
|
|||||||
SerialContainerPrivate *impl = serial.pimpl();
|
SerialContainerPrivate *impl = serial.pimpl();
|
||||||
|
|
||||||
EXPECT_EQ(impl->parent(), nullptr);
|
EXPECT_EQ(impl->parent(), nullptr);
|
||||||
EXPECT_THROW(serial.init(scene), InitStageException);
|
EXPECT_THROW(serial.init(robot_model), InitStageException);
|
||||||
VALIDATE();
|
VALIDATE();
|
||||||
|
|
||||||
/***** inserting first stage *****/
|
/***** inserting first stage *****/
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user