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/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_relative.h>
|
||||
#include <moveit/task_constructor/stages/connect.h>
|
||||
@ -18,7 +18,7 @@
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
void spawnObject(planning_scene::PlanningScenePtr scene) {
|
||||
void spawnObject(const planning_scene::PlanningScenePtr& scene) {
|
||||
moveit_msgs::CollisionObject o;
|
||||
o.id= "object";
|
||||
o.header.frame_id= "world";
|
||||
@ -52,7 +52,21 @@ int main(int argc, char** argv){
|
||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||
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>();
|
||||
stage->restrictDirection(stages::MoveTo::FORWARD);
|
||||
@ -97,6 +111,7 @@ int main(int argc, char** argv){
|
||||
Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
|
||||
"lh_tool_frame");
|
||||
gengrasp->setAngleDelta(M_PI / 10.);
|
||||
gengrasp->setMonitoredStage(initial_stage);
|
||||
|
||||
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
||||
ik->properties().configureInitFrom(Stage::PARENT, {"group", "eef", "default_pose"});
|
||||
@ -157,12 +172,6 @@ int main(int argc, char** argv){
|
||||
}
|
||||
|
||||
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();
|
||||
std::cout << "waiting for <enter>\n";
|
||||
char ch;
|
||||
|
||||
@ -43,7 +43,13 @@ int main(int argc, char** argv){
|
||||
|
||||
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");
|
||||
@ -81,6 +87,7 @@ int main(int argc, char** argv){
|
||||
gengrasp->setObject("object");
|
||||
gengrasp->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
|
||||
gengrasp->setAngleDelta(.2);
|
||||
gengrasp->setMonitoredStage(initial_stage);
|
||||
|
||||
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
||||
ik->setEndEffector("left_gripper");
|
||||
|
||||
@ -46,7 +46,13 @@ int main(int argc, char** argv){
|
||||
t.setProperty("planner", std::string("RRTConnectkConfigDefault"));
|
||||
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");
|
||||
@ -82,6 +88,7 @@ int main(int argc, char** argv){
|
||||
gengrasp->setObject("object");
|
||||
gengrasp->setToolToGraspTF(Eigen::Translation3d(.03,0,0), "s_model_tool0");
|
||||
gengrasp->setAngleDelta(-.2);
|
||||
gengrasp->setMonitoredStage(initial_stage);
|
||||
|
||||
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
||||
ik->properties().configureInitFrom(Stage::PARENT, {"eef"});
|
||||
|
||||
@ -62,7 +62,7 @@ public:
|
||||
virtual void clear();
|
||||
|
||||
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)
|
||||
virtual void validateConnectivity() const;
|
||||
|
||||
@ -90,7 +90,7 @@ public:
|
||||
SerialContainer(const std::string& name = "serial container");
|
||||
|
||||
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)
|
||||
void validateConnectivity() const override;
|
||||
@ -140,7 +140,7 @@ public:
|
||||
ParallelContainerBase(const std::string &name);
|
||||
|
||||
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)
|
||||
void validateConnectivity() const override;
|
||||
@ -202,7 +202,7 @@ public:
|
||||
Fallbacks(const std::string &name) : ParallelContainerBase(name) {}
|
||||
|
||||
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 compute() override;
|
||||
};
|
||||
|
||||
@ -50,6 +50,10 @@
|
||||
inline const Class##Private* pimpl() const; \
|
||||
inline Class##Private* pimpl();
|
||||
|
||||
namespace moveit { namespace core {
|
||||
MOVEIT_CLASS_FORWARD(RobotModel)
|
||||
} }
|
||||
|
||||
namespace planning_scene {
|
||||
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
|
||||
* 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 std::string& name() const;
|
||||
@ -220,7 +224,7 @@ public:
|
||||
void restrictDirection(Direction dir);
|
||||
|
||||
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 computeBackward(const InterfaceState& to) = 0;
|
||||
@ -314,7 +318,7 @@ public:
|
||||
MonitoringGenerator(const std::string &name = "monitoring generator", Stage* monitored = nullptr);
|
||||
void setMonitoredStage(Stage* monitored);
|
||||
|
||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
|
||||
protected:
|
||||
MonitoringGenerator(MonitoringGeneratorPrivate* impl);
|
||||
|
||||
@ -47,7 +47,7 @@ class Connect : public Connecting {
|
||||
public:
|
||||
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);
|
||||
|
||||
protected:
|
||||
|
||||
@ -49,7 +49,7 @@ class CurrentState : public Generator {
|
||||
public:
|
||||
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 compute() override;
|
||||
|
||||
@ -58,8 +58,8 @@ public:
|
||||
}
|
||||
|
||||
protected:
|
||||
moveit::core::RobotModelConstPtr robot_model_;
|
||||
planning_scene::PlanningScenePtr scene_;
|
||||
bool ran_ = false;
|
||||
};
|
||||
|
||||
} } }
|
||||
|
||||
@ -43,11 +43,10 @@
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
class GenerateGraspPose : public Generator {
|
||||
class GenerateGraspPose : public MonitoringGenerator {
|
||||
public:
|
||||
GenerateGraspPose(std::string name);
|
||||
GenerateGraspPose(const std::string& name);
|
||||
|
||||
void init(const planning_scene::PlanningSceneConstPtr& scene) override;
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
|
||||
@ -64,6 +63,9 @@ public:
|
||||
}
|
||||
void setAngleDelta(double delta);
|
||||
|
||||
protected:
|
||||
void onNewSolution(const SolutionBase& s) override;
|
||||
|
||||
protected:
|
||||
planning_scene::PlanningScenePtr scene_;
|
||||
double current_angle_ = 0.0;
|
||||
|
||||
@ -50,7 +50,7 @@ class Gripper : public PropagatingEitherWay {
|
||||
public:
|
||||
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 computeBackward(const InterfaceState& to) override;
|
||||
|
||||
|
||||
@ -46,7 +46,7 @@ class Move : public Connecting {
|
||||
public:
|
||||
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);
|
||||
|
||||
void setGroup(const std::string &group);
|
||||
|
||||
@ -50,7 +50,7 @@ class MoveRelative : public PropagatingEitherWay {
|
||||
public:
|
||||
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 computeBackward(const InterfaceState& to) override;
|
||||
|
||||
|
||||
@ -49,7 +49,7 @@ class MoveTo : public PropagatingEitherWay {
|
||||
public:
|
||||
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 computeBackward(const InterfaceState& to) override;
|
||||
|
||||
|
||||
@ -46,9 +46,6 @@
|
||||
|
||||
#include <moveit/macros/class_forward.h>
|
||||
|
||||
namespace robot_model_loader {
|
||||
MOVEIT_CLASS_FORWARD(RobotModelLoader)
|
||||
}
|
||||
namespace moveit { namespace core {
|
||||
MOVEIT_CLASS_FORWARD(RobotModel)
|
||||
MOVEIT_CLASS_FORWARD(RobotState)
|
||||
@ -76,7 +73,7 @@ public:
|
||||
~Task();
|
||||
|
||||
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 clear() override;
|
||||
@ -92,14 +89,10 @@ public:
|
||||
/// remove function callback
|
||||
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
|
||||
void reset() override;
|
||||
/// 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
|
||||
bool plan();
|
||||
@ -124,15 +117,13 @@ public:
|
||||
void setProperty(const std::string& name, const boost::any& value);
|
||||
|
||||
protected:
|
||||
void initModel();
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
void onNewSolution(const SolutionBase &s) override;
|
||||
|
||||
private:
|
||||
std::string id_;
|
||||
robot_model_loader::RobotModelLoaderPtr rml_;
|
||||
planning_scene::PlanningScenePtr scene_; // initial scene
|
||||
moveit::core::RobotModelConstPtr robot_model_;
|
||||
|
||||
// introspection and monitoring
|
||||
std::unique_ptr<Introspection> introspection_;
|
||||
|
||||
@ -198,12 +198,12 @@ void ContainerBase::reset()
|
||||
Stage::reset();
|
||||
}
|
||||
|
||||
void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||
{
|
||||
auto impl = pimpl();
|
||||
auto& children = impl->children();
|
||||
|
||||
Stage::init(scene);
|
||||
Stage::init(robot_model);
|
||||
|
||||
// we need to have some children to do the actual work
|
||||
if (children.empty())
|
||||
@ -212,7 +212,7 @@ void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
// recursively init all children and accumulate errors
|
||||
InitStageException errors;
|
||||
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;
|
||||
@ -408,14 +408,14 @@ bool SerialContainerPrivate::connect(container_type::const_iterator cur)
|
||||
* This context is provided by two stages pushing from both ends
|
||||
* 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
|
||||
auto impl = pimpl();
|
||||
impl->starts_.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 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 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
|
||||
ContainerBase::init(scene);
|
||||
ContainerBase::init(robot_model);
|
||||
auto impl = pimpl();
|
||||
|
||||
// determine the union of interfaces required by children
|
||||
@ -886,9 +886,9 @@ void Fallbacks::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();
|
||||
}
|
||||
|
||||
|
||||
@ -128,7 +128,7 @@ void Stage::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
|
||||
auto impl = pimpl();
|
||||
@ -390,9 +390,9 @@ void PropagatingEitherWay::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();
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
void MonitoringGenerator::init(const planning_scene::PlanningSceneConstPtr& scene)
|
||||
void MonitoringGenerator::init(const moveit::core::RobotModelConstPtr& 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");
|
||||
}
|
||||
|
||||
void Connect::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
void Connect::init(const core::RobotModelConstPtr& robot_model)
|
||||
{
|
||||
Connecting::init(scene);
|
||||
planner_->init(scene->getRobotModel());
|
||||
Connecting::init(robot_model);
|
||||
planner_->init(robot_model);
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
void CurrentState::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
void CurrentState::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||
{
|
||||
Generator::init(scene);
|
||||
scene_ = scene->diff();
|
||||
Generator::init(robot_model);
|
||||
robot_model_ = robot_model;
|
||||
scene_.reset();
|
||||
}
|
||||
|
||||
bool CurrentState::canCompute() const{
|
||||
return !ran_;
|
||||
return !scene_;
|
||||
}
|
||||
|
||||
bool CurrentState::compute() {
|
||||
auto rml = std::make_shared<robot_model_loader::RobotModelLoader>();
|
||||
scene_ = std::make_shared<planning_scene::PlanningScene>(rml->getModel());
|
||||
scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
|
||||
|
||||
ros::NodeHandle h;
|
||||
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
|
||||
@ -90,7 +90,6 @@ bool CurrentState::compute(){
|
||||
if (client.call(req, res)) {
|
||||
scene_->setPlanningSceneMsg(res.scene);
|
||||
spawn(InterfaceState(scene_), 0.0);
|
||||
ran_= true;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
@ -46,8 +46,8 @@
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
GenerateGraspPose::GenerateGraspPose(std::string name)
|
||||
: Generator(name)
|
||||
GenerateGraspPose::GenerateGraspPose(const std::string& name)
|
||||
: MonitoringGenerator(name)
|
||||
{
|
||||
auto& p = properties();
|
||||
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)");
|
||||
}
|
||||
|
||||
void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
{
|
||||
Generator::init(scene);
|
||||
scene_ = scene->diff();
|
||||
}
|
||||
|
||||
void GenerateGraspPose::setEndEffector(const std::string &eef){
|
||||
setProperty("eef", eef);
|
||||
}
|
||||
@ -91,8 +85,15 @@ void GenerateGraspPose::setAngleDelta(double 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{
|
||||
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(){
|
||||
|
||||
@ -59,10 +59,10 @@ Gripper::Gripper(std::string name)
|
||||
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);
|
||||
planner_ = Task::createPlanner(scene->getRobotModel());
|
||||
PropagatingEitherWay::init(robot_model);
|
||||
planner_ = Task::createPlanner(robot_model);
|
||||
}
|
||||
|
||||
void Gripper::setEndEffector(std::string eef){
|
||||
|
||||
@ -58,10 +58,10 @@ Move::Move(std::string name)
|
||||
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);
|
||||
planner_ = Task::createPlanner(scene->getRobotModel());
|
||||
Connecting::init(robot_model);
|
||||
planner_ = Task::createPlanner(robot_model);
|
||||
}
|
||||
|
||||
void Move::setGroup(const std::string& group){
|
||||
|
||||
@ -90,10 +90,10 @@ void MoveRelative::along(const geometry_msgs::Vector3Stamped &direction)
|
||||
setProperty("direction", direction);
|
||||
}
|
||||
|
||||
void MoveRelative::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||
{
|
||||
PropagatingEitherWay::init(scene);
|
||||
planner_->init(scene->getRobotModel());
|
||||
PropagatingEitherWay::init(robot_model);
|
||||
planner_->init(robot_model);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
void MoveTo::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||
{
|
||||
PropagatingEitherWay::init(scene);
|
||||
planner_->init(scene->getRobotModel());
|
||||
PropagatingEitherWay::init(robot_model);
|
||||
planner_->init(robot_model);
|
||||
}
|
||||
|
||||
bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
|
||||
|
||||
@ -43,8 +43,6 @@
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <moveit_msgs/GetPlanningScene.h>
|
||||
|
||||
#include <moveit/robot_model_loader/robot_model_loader.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)
|
||||
: 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
|
||||
//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);
|
||||
}
|
||||
|
||||
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
|
||||
Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns,
|
||||
const std::string& planning_plugin_param_name,
|
||||
@ -184,7 +144,7 @@ void Task::reset()
|
||||
WrapperBase::reset();
|
||||
}
|
||||
|
||||
void Task::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
void Task::init(const moveit::core::RobotModelConstPtr& model)
|
||||
{
|
||||
auto impl = pimpl();
|
||||
// initialize push connections of wrapped child
|
||||
@ -193,7 +153,7 @@ void Task::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
child->setNextStarts(impl->pendingForward());
|
||||
|
||||
// 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
|
||||
stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE}));
|
||||
// and *finally* validate connectivity
|
||||
@ -223,8 +183,7 @@ bool Task::compute()
|
||||
bool Task::plan()
|
||||
{
|
||||
reset();
|
||||
if (!scene_) initScene();
|
||||
init(scene_);
|
||||
init(robot_model_);
|
||||
|
||||
while(ros::ok() && canCompute()) {
|
||||
if (compute()) {
|
||||
|
||||
@ -68,7 +68,7 @@ void append(ContainerBase& c, const std::initializer_list<StageType>& types, int
|
||||
|
||||
class SerialTest : public ::testing::Test {
|
||||
protected:
|
||||
planning_scene::PlanningScenePtr scene;
|
||||
moveit::core::RobotModelConstPtr robot_model;
|
||||
SerialContainer serial;
|
||||
InterfacePtr dummy;
|
||||
|
||||
@ -112,7 +112,7 @@ protected:
|
||||
reset(start, end);
|
||||
append(serial, types);
|
||||
try {
|
||||
serial.init(scene);
|
||||
serial.init(robot_model);
|
||||
// prune pull interfaces based on provided external interface (start, end)
|
||||
InterfaceFlags accepted;
|
||||
if (start) accepted |= WRITES_PREV_END;
|
||||
@ -141,7 +141,7 @@ TEST_F(SerialTest, insertion_order) {
|
||||
SerialContainerPrivate *impl = serial.pimpl();
|
||||
|
||||
EXPECT_EQ(impl->parent(), nullptr);
|
||||
EXPECT_THROW(serial.init(scene), InitStageException);
|
||||
EXPECT_THROW(serial.init(robot_model), InitStageException);
|
||||
VALIDATE();
|
||||
|
||||
/***** inserting first stage *****/
|
||||
|
||||
Loading…
Reference in New Issue
Block a user