mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Task::setRobotModel() / Task::loadRobotModel()
This commit is contained in:
parent
70065c98e7
commit
bd60e6f65d
@ -41,6 +41,7 @@ int main(int argc, char** argv){
|
|||||||
spinner.start();
|
spinner.start();
|
||||||
|
|
||||||
Task t;
|
Task t;
|
||||||
|
t.loadRobotModel();
|
||||||
// define global properties used by most stages
|
// define global properties used by most stages
|
||||||
t.setProperty("group", std::string("left_arm"));
|
t.setProperty("group", std::string("left_arm"));
|
||||||
t.setProperty("eef", std::string("la_tool_mount"));
|
t.setProperty("eef", std::string("la_tool_mount"));
|
||||||
|
|||||||
@ -74,6 +74,10 @@ public:
|
|||||||
|
|
||||||
std::string id() const;
|
std::string id() const;
|
||||||
const moveit::core::RobotModelConstPtr getRobotModel() const { return robot_model_; }
|
const moveit::core::RobotModelConstPtr getRobotModel() const { return robot_model_; }
|
||||||
|
/// setting the robot model also resets the task
|
||||||
|
void setRobotModel(const moveit::core::RobotModelConstPtr& robot_model);
|
||||||
|
/// load robot model from given parameter
|
||||||
|
void loadRobotModel(const std::string& robot_description = "robot_description");
|
||||||
|
|
||||||
void add(Stage::pointer &&stage);
|
void add(Stage::pointer &&stage);
|
||||||
void clear() override;
|
void clear() override;
|
||||||
@ -92,7 +96,7 @@ public:
|
|||||||
/// 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 moveit::core::RobotModelConstPtr& model) override;
|
void init();
|
||||||
|
|
||||||
/// 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();
|
||||||
|
|||||||
@ -53,11 +53,6 @@ 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)
|
||||||
{
|
{
|
||||||
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)));
|
||||||
// enable introspection by default
|
// enable introspection by default
|
||||||
@ -90,6 +85,19 @@ Task::~Task()
|
|||||||
reset();
|
reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void Task::setRobotModel(const core::RobotModelConstPtr& robot_model)
|
||||||
|
{
|
||||||
|
reset();
|
||||||
|
robot_model_ = robot_model;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Task::loadRobotModel(const std::string& robot_description) {
|
||||||
|
robot_model_loader::RobotModelLoader rml(robot_description);
|
||||||
|
setRobotModel(rml.getModel());
|
||||||
|
if (!robot_model_)
|
||||||
|
throw Exception("Task failed to construct RobotModel");
|
||||||
|
}
|
||||||
|
|
||||||
void Task::add(pointer &&stage) {
|
void Task::add(pointer &&stage) {
|
||||||
if (!stage)
|
if (!stage)
|
||||||
throw std::runtime_error("stage insertion failed: invalid stage pointer");
|
throw std::runtime_error("stage insertion failed: invalid stage pointer");
|
||||||
@ -144,8 +152,11 @@ void Task::reset()
|
|||||||
WrapperBase::reset();
|
WrapperBase::reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Task::init(const moveit::core::RobotModelConstPtr& model)
|
void Task::init()
|
||||||
{
|
{
|
||||||
|
if (!robot_model_)
|
||||||
|
loadRobotModel();
|
||||||
|
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
// initialize push connections of wrapped child
|
// initialize push connections of wrapped child
|
||||||
StagePrivate *child = wrapped()->pimpl();
|
StagePrivate *child = wrapped()->pimpl();
|
||||||
@ -153,7 +164,7 @@ void Task::init(const moveit::core::RobotModelConstPtr& model)
|
|||||||
child->setNextStarts(impl->pendingForward());
|
child->setNextStarts(impl->pendingForward());
|
||||||
|
|
||||||
// and *afterwards* initialize all children recursively
|
// and *afterwards* initialize all children recursively
|
||||||
stages()->init(model);
|
stages()->init(robot_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
|
||||||
@ -183,7 +194,7 @@ bool Task::compute()
|
|||||||
bool Task::plan()
|
bool Task::plan()
|
||||||
{
|
{
|
||||||
reset();
|
reset();
|
||||||
init(robot_model_);
|
init();
|
||||||
|
|
||||||
while(ros::ok() && canCompute()) {
|
while(ros::ok() && canCompute()) {
|
||||||
if (compute()) {
|
if (compute()) {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user