From 60229db010ea305296bc1c90d04faa3e4dacd976 Mon Sep 17 00:00:00 2001 From: Wyatt Rees Date: Fri, 3 Jun 2022 10:39:51 -0600 Subject: [PATCH 1/2] Add KinematicsQueryOptions property in CartesianPath solver (#370) --- core/src/solvers/cartesian_path.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 6044c8309..3caecd757 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -41,6 +41,7 @@ #include #include +#include #if MOVEIT_HAS_CARTESIAN_INTERPOLATOR #include #endif @@ -56,6 +57,8 @@ CartesianPath::CartesianPath() { p.declare("step_size", 0.01, "step size between consecutive waypoints"); p.declare("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step"); p.declare("min_fraction", 1.0, "fraction of motion required for success"); + p.declare("kinematics_options", kinematics::KinematicsQueryOptions(), + "KinematicsQueryOptions to pass to CartesianInterpolator"); } void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {} @@ -97,11 +100,12 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath( &(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true, moveit::core::MaxEEFStep(props.get("step_size")), - moveit::core::JumpThreshold(props.get("jump_threshold")), is_valid); + moveit::core::JumpThreshold(props.get("jump_threshold")), is_valid, + props.get("kinematics_options")); #else double achieved_fraction = sandbox_scene->getCurrentStateNonConst().computeCartesianPath( jmg, trajectory, &link, target, true, props.get("step_size"), props.get("jump_threshold"), - is_valid); + is_valid, props.get("kinematics_options")); #endif assert(!trajectory.empty()); // there should be at least the start state From e923fbc0c6bd477d15532b335581734857d0eafc Mon Sep 17 00:00:00 2001 From: Jafar Date: Tue, 14 Jun 2022 23:13:19 +0300 Subject: [PATCH 2/2] Fix Task's move constructor (#371) * Add unit test * Fix TaskPrivate's move assignment operator * Slightly simplify code Co-authored-by: Robert Haschke --- core/src/task.cpp | 14 ++++++++------ core/test/test_move_to.cpp | 29 +++++++++++++++++++++++++++++ 2 files changed, 37 insertions(+), 6 deletions(-) diff --git a/core/src/task.cpp b/core/src/task.cpp index 5cee598ce..a3535b4c6 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -77,10 +77,12 @@ TaskPrivate::TaskPrivate(Task* me, const std::string& ns) TaskPrivate& TaskPrivate::operator=(TaskPrivate&& other) { this->WrapperBasePrivate::operator=(std::move(other)); ns_ = std::move(other.ns_); - introspection_ = std::move(other.introspection_); robot_model_ = std::move(other.robot_model_); robot_model_loader_ = std::move(other.robot_model_loader_); 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(me_)->enableIntrospection(static_cast(other.introspection_)); return *this; } @@ -209,17 +211,17 @@ void Task::init() { stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE })); // provide introspection instance to all stages - impl->setIntrospection(impl->introspection_.get()); + auto* introspection = impl->introspection_.get(); impl->traverseStages( - [impl](Stage& stage, int /*depth*/) { - stage.pimpl()->setIntrospection(impl->introspection_.get()); + [introspection](Stage& stage, int /*depth*/) { + stage.pimpl()->setIntrospection(introspection); return true; }, 1, UINT_MAX); // first time publish task - if (impl->introspection_) - impl->introspection_->publishTaskDescription(); + if (introspection) + introspection->publishTaskDescription(); } bool Task::canCompute() const { diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp index 1fbc512ba..19fdaf071 100644 --- a/core/test/test_move_to.cpp +++ b/core/test/test_move_to.cpp @@ -1,4 +1,5 @@ #include "models.h" +#include "stage_mockups.h" #include #include @@ -160,6 +161,34 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) { } #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(t.getRobotModel()); + ref->setState(scene); + + t.add(Stage::pointer(ref)); + t.add(std::make_unique()); + t.add(std::make_unique(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) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "move_to_test");