Fix Task's move constructor (#371)

* Add unit test
* Fix TaskPrivate's move assignment operator
* Slightly simplify code

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
This commit is contained in:
Jafar 2022-06-14 23:13:19 +03:00 committed by GitHub
parent 60229db010
commit e923fbc0c6
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 37 additions and 6 deletions

View File

@ -77,10 +77,12 @@ TaskPrivate::TaskPrivate(Task* me, const std::string& ns)
TaskPrivate& TaskPrivate::operator=(TaskPrivate&& other) { TaskPrivate& TaskPrivate::operator=(TaskPrivate&& other) {
this->WrapperBasePrivate::operator=(std::move(other)); this->WrapperBasePrivate::operator=(std::move(other));
ns_ = std::move(other.ns_); ns_ = std::move(other.ns_);
introspection_ = std::move(other.introspection_);
robot_model_ = std::move(other.robot_model_); robot_model_ = std::move(other.robot_model_);
robot_model_loader_ = std::move(other.robot_model_loader_); robot_model_loader_ = std::move(other.robot_model_loader_);
task_cbs_ = std::move(other.task_cbs_); task_cbs_ = std::move(other.task_cbs_);
// Ensure same introspection status, but keep the existing introspection instance,
// which stores this task pointer and includes it in its task_id_
static_cast<Task*>(me_)->enableIntrospection(static_cast<bool>(other.introspection_));
return *this; return *this;
} }
@ -209,17 +211,17 @@ void Task::init() {
stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE })); stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE }));
// provide introspection instance to all stages // provide introspection instance to all stages
impl->setIntrospection(impl->introspection_.get()); auto* introspection = impl->introspection_.get();
impl->traverseStages( impl->traverseStages(
[impl](Stage& stage, int /*depth*/) { [introspection](Stage& stage, int /*depth*/) {
stage.pimpl()->setIntrospection(impl->introspection_.get()); stage.pimpl()->setIntrospection(introspection);
return true; return true;
}, },
1, UINT_MAX); 1, UINT_MAX);
// first time publish task // first time publish task
if (impl->introspection_) if (introspection)
impl->introspection_->publishTaskDescription(); introspection->publishTaskDescription();
} }
bool Task::canCompute() const { bool Task::canCompute() const {

View File

@ -1,4 +1,5 @@
#include "models.h" #include "models.h"
#include "stage_mockups.h"
#include <moveit/task_constructor/task.h> #include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/move_to.h> #include <moveit/task_constructor/stages/move_to.h>
@ -160,6 +161,34 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) {
} }
#endif #endif
// This test require a running rosmaster
TEST(Task, taskMoveConstructor) {
auto create_task = [] {
moveit::core::RobotModelConstPtr robot_model = getModel();
Task t("first");
t.setRobotModel(robot_model);
auto ref = new stages::FixedState("fixed");
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
ref->setState(scene);
t.add(Stage::pointer(ref));
t.add(std::make_unique<ConnectMockup>());
t.add(std::make_unique<MonitoringGeneratorMockup>(ref));
return t;
};
// Segfaults when introspection is enabled
Task t;
t = create_task();
try {
t.init();
EXPECT_TRUE(t.plan(1));
} catch (const InitStageException& e) {
ADD_FAILURE() << "InitStageException:" << std::endl << e << t;
}
}
int main(int argc, char** argv) { int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "move_to_test"); ros::init(argc, argv, "move_to_test");