mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branch 'master' into ros2
This commit is contained in:
commit
cc7f9f0585
5
.github/workflows/ci.yaml
vendored
5
.github/workflows/ci.yaml
vendored
@ -59,15 +59,14 @@ jobs:
|
||||
submodules: recursive
|
||||
|
||||
- name: Cache ccache
|
||||
uses: rhaschke/cache@main
|
||||
uses: actions/cache@v4
|
||||
with:
|
||||
save-always: true
|
||||
path: ${{ env.CCACHE_DIR }}
|
||||
key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
|
||||
restore-keys: |
|
||||
ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}
|
||||
ccache-${{ env.CACHE_PREFIX }}
|
||||
env:
|
||||
GHA_CACHE_SAVE: always
|
||||
|
||||
- id: ici
|
||||
name: Run industrial_ci
|
||||
|
||||
3
.gitmodules
vendored
3
.gitmodules
vendored
@ -3,3 +3,6 @@
|
||||
url = https://github.com/pybind/pybind11
|
||||
branch = smart_holder
|
||||
shallow = true
|
||||
[submodule "core/src/scope_guard"]
|
||||
path = core/src/scope_guard
|
||||
url = https://github.com/ricab/scope_guard
|
||||
|
||||
@ -29,7 +29,7 @@ repos:
|
||||
- id: trailing-whitespace
|
||||
|
||||
- repo: https://github.com/psf/black
|
||||
rev: 22.3.0
|
||||
rev: 24.8.0
|
||||
hooks:
|
||||
- id: black
|
||||
args: ["--line-length", "100"]
|
||||
|
||||
@ -185,8 +185,10 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
|
||||
exec_traj.effect_on_success = [this,
|
||||
&scene_diff = const_cast<::moveit_msgs::msg::PlanningScene&>(sub_traj.scene_diff),
|
||||
description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
|
||||
// Never modify joint state directly (only via robot trajectories)
|
||||
scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState();
|
||||
scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState();
|
||||
scene_diff.robot_state.is_diff = true; // silent empty JointState msg error
|
||||
|
||||
if (!moveit::core::isEmpty(scene_diff)) {
|
||||
RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description);
|
||||
|
||||
@ -64,9 +64,9 @@ public:
|
||||
template <typename PropertyType, typename... Extra>
|
||||
class_& property(const char* name, const Extra&... extra) {
|
||||
PropertyConverter<PropertyType>(); // register corresponding property converter
|
||||
auto getter = [name](const type_& self) {
|
||||
const moveit::task_constructor::PropertyMap& props = self.properties();
|
||||
return props.get<PropertyType>(name);
|
||||
auto getter = [name](type_& self) -> PropertyType& {
|
||||
moveit::task_constructor::PropertyMap& props = self.properties();
|
||||
return const_cast<PropertyType&>(props.get<PropertyType>(name));
|
||||
};
|
||||
auto setter = [name](type_& self, const PropertyType& value) {
|
||||
moveit::task_constructor::PropertyMap& props = self.properties();
|
||||
|
||||
@ -39,6 +39,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <moveit/task_constructor/solvers/planner_interface.h>
|
||||
#include <moveit/robot_state/cartesian_interpolator.h>
|
||||
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
@ -57,13 +58,17 @@ public:
|
||||
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
|
||||
|
||||
void setStepSize(double step_size) { setProperty("step_size", step_size); }
|
||||
void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); }
|
||||
void setPrecision(const moveit::core::CartesianPrecision& precision) { setProperty("precision", precision); }
|
||||
template <typename T = float>
|
||||
void setJumpThreshold(double) {
|
||||
static_assert(std::is_integral<T>::value, "setJumpThreshold() is deprecated. Replace with setPrecision.");
|
||||
}
|
||||
void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }
|
||||
|
||||
[[deprecated("Replace with setMaxVelocityScalingFactor")]] // clang-format off
|
||||
void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); }
|
||||
void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); } // clang-format on
|
||||
[[deprecated("Replace with setMaxAccelerationScalingFactor")]] // clang-format off
|
||||
void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); }
|
||||
void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); } // clang-format on
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
|
||||
@ -72,8 +77,8 @@ public:
|
||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
||||
|
||||
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
|
||||
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
||||
|
||||
std::string getPlannerId() const override { return "CartesianPath"; }
|
||||
|
||||
@ -57,6 +57,13 @@
|
||||
namespace moveit {
|
||||
namespace task_constructor {
|
||||
|
||||
/// exception thrown by StagePrivate::runCompute()
|
||||
class PreemptStageException : public std::exception
|
||||
{
|
||||
public:
|
||||
explicit PreemptStageException() {}
|
||||
};
|
||||
|
||||
class ContainerBase;
|
||||
class StagePrivate
|
||||
{
|
||||
@ -146,6 +153,10 @@ public:
|
||||
bool storeFailures() const { return introspection_ != nullptr; }
|
||||
void runCompute() {
|
||||
RCLCPP_DEBUG_STREAM(LOGGER, fmt::format("Computing stage '{}'", name()));
|
||||
|
||||
if (preempted())
|
||||
throw PreemptStageException();
|
||||
|
||||
auto compute_start_time = std::chrono::steady_clock::now();
|
||||
try {
|
||||
compute();
|
||||
@ -159,6 +170,11 @@ public:
|
||||
/** compute cost for solution through configured CostTerm */
|
||||
void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
|
||||
|
||||
void setPreemptRequestedMember(const std::atomic<bool>* preempt_requested) {
|
||||
preempt_requested_ = preempt_requested;
|
||||
}
|
||||
bool preempted() const { return preempt_requested_ != nullptr && *preempt_requested_; }
|
||||
|
||||
protected:
|
||||
StagePrivate& operator=(StagePrivate&& other);
|
||||
|
||||
@ -197,6 +213,8 @@ private:
|
||||
InterfaceWeakPtr next_starts_; // interface to be used for sendForward()
|
||||
|
||||
Introspection* introspection_; // task's introspection instance
|
||||
const std::atomic<bool>* preempt_requested_;
|
||||
|
||||
inline static const rclcpp::Logger LOGGER = rclcpp::get_logger("stage");
|
||||
};
|
||||
PIMPL_FUNCTIONS(Stage)
|
||||
|
||||
@ -129,8 +129,9 @@ public:
|
||||
|
||||
/// reset, init scene (if not yet done), and init all stages, then start planning
|
||||
moveit::core::MoveItErrorCode plan(size_t max_solutions = 0);
|
||||
/// interrupt current planning (or execution)
|
||||
/// interrupt current planning
|
||||
void preempt();
|
||||
void resetPreemptRequest();
|
||||
/// execute solution, return the result
|
||||
moveit::core::MoveItErrorCode execute(const SolutionBase& s);
|
||||
|
||||
|
||||
@ -63,7 +63,7 @@ private:
|
||||
std::string ns_;
|
||||
robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
|
||||
moveit::core::RobotModelConstPtr robot_model_;
|
||||
bool preempt_requested_;
|
||||
std::atomic<bool> preempt_requested_;
|
||||
|
||||
// introspection and monitoring
|
||||
std::unique_ptr<Introspection> introspection_;
|
||||
|
||||
@ -38,6 +38,7 @@
|
||||
#include <moveit/task_constructor/solvers/joint_interpolation.h>
|
||||
#include <moveit/task_constructor/solvers/multi_planner.h>
|
||||
#include <moveit_msgs/msg/workspace_parameters.hpp>
|
||||
#include <fmt/core.h>
|
||||
#include "utils.h"
|
||||
|
||||
namespace py = pybind11;
|
||||
@ -96,6 +97,22 @@ void export_solvers(py::module& m) {
|
||||
.property<double>("max_step", "float: Limit any (single) joint change between two waypoints to this amount")
|
||||
.def(py::init<>());
|
||||
|
||||
const moveit::core::CartesianPrecision default_precision;
|
||||
py::class_<moveit::core::CartesianPrecision>(m, "CartesianPrecision", "precision for Cartesian interpolation")
|
||||
.def(py::init([](double translational, double rotational, double max_resolution) {
|
||||
return new moveit::core::CartesianPrecision{ translational, rotational, max_resolution };
|
||||
}),
|
||||
py::arg("translational") = default_precision.translational,
|
||||
py::arg("rotational") = default_precision.rotational,
|
||||
py::arg("max_resolution") = default_precision.max_resolution)
|
||||
.def_readwrite("translational", &moveit::core::CartesianPrecision::translational)
|
||||
.def_readwrite("rotational", &moveit::core::CartesianPrecision::rotational)
|
||||
.def_readwrite("max_resolution", &moveit::core::CartesianPrecision::max_resolution)
|
||||
.def("__str__", [](const moveit::core::CartesianPrecision& self) {
|
||||
return fmt::format("CartesianPrecision(translational={}, rotational={}, max_resolution={}",
|
||||
self.translational, self.rotational, self.max_resolution);
|
||||
});
|
||||
|
||||
properties::class_<CartesianPath, PlannerInterface>(m, "CartesianPath", R"(
|
||||
Perform linear interpolation between Cartesian poses.
|
||||
Fails on collision along the interpolation path. There is no obstacle avoidance. ::
|
||||
@ -105,15 +122,12 @@ void export_solvers(py::module& m) {
|
||||
# Instantiate Cartesian-space interpolation planner
|
||||
cartesianPlanner = core.CartesianPath()
|
||||
cartesianPlanner.step_size = 0.01
|
||||
cartesianPlanner.jump_threshold = 0.0 # effectively disable jump threshold.
|
||||
cartesianPlanner.precision.translational = 0.001
|
||||
)")
|
||||
.property<double>("step_size", "float: Limit the Cartesian displacement between consecutive waypoints "
|
||||
"In contrast to joint-space interpolation, the Cartesian planner can also "
|
||||
"succeed when only a fraction of the linear path was feasible.")
|
||||
.property<double>(
|
||||
"jump_threshold",
|
||||
"float: Limit joint displacement between consecutive waypoints, thus preventing jumps in joint space. "
|
||||
"This values specifies the fraction of mean acceptable joint motion per step.")
|
||||
.property<moveit::core::CartesianPrecision>("precision", "Cartesian interpolation precision")
|
||||
.property<double>("min_fraction", "float: Fraction of overall distance required to succeed.")
|
||||
.def(py::init<>());
|
||||
|
||||
|
||||
@ -14,10 +14,14 @@ def setUpModule():
|
||||
rclcpp.init()
|
||||
|
||||
|
||||
def tearDownModule():
|
||||
rclcpp.shutdown()
|
||||
|
||||
|
||||
# When py_binding_tools and MTC are compiled with different pybind11 versions,
|
||||
# the corresponding classes are not interoperable.
|
||||
def check_pybind11_incompatibility():
|
||||
rclcpp.init([])
|
||||
rclcpp.init()
|
||||
node = rclcpp.Node("dummy")
|
||||
try:
|
||||
core.PipelinePlanner(node)
|
||||
@ -33,8 +37,8 @@ incompatible_pybind11_msg = "MoveIt and MTC use incompatible pybind11 versions"
|
||||
|
||||
|
||||
class TestPropertyMap(unittest.TestCase):
|
||||
|
||||
def setUp(self):
|
||||
self.node = rclcpp.Node("test_mtc_props")
|
||||
self.props = core.PropertyMap()
|
||||
|
||||
def _check(self, name, value):
|
||||
@ -54,7 +58,8 @@ class TestPropertyMap(unittest.TestCase):
|
||||
|
||||
@unittest.skipIf(incompatible_pybind11, incompatible_pybind11_msg)
|
||||
def test_assign_in_reference(self):
|
||||
planner = core.PipelinePlanner(self.node)
|
||||
node = rclcpp.Node("test_mtc_props")
|
||||
planner = core.PipelinePlanner(node)
|
||||
props = planner.properties
|
||||
|
||||
props["goal_joint_tolerance"] = 3.14
|
||||
|
||||
1
core/src/scope_guard
Submodule
1
core/src/scope_guard
Submodule
@ -0,0 +1 @@
|
||||
Subproject commit 71a04528184db1749dd08ebbbf4daf3b5dca21fd
|
||||
@ -60,7 +60,8 @@ CartesianPath::CartesianPath() {
|
||||
auto& p = properties();
|
||||
p.declare<geometry_msgs::msg::PoseStamped>("ik_frame", "frame to move linearly (use for joint-space target)");
|
||||
p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
|
||||
p.declare<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step");
|
||||
p.declare<moveit::core::CartesianPrecision>("precision", moveit::core::CartesianPrecision(),
|
||||
"precision of linear path");
|
||||
p.declare<double>("min_fraction", 1.0, "fraction of motion required for success");
|
||||
p.declare<kinematics::KinematicsQueryOptions>("kinematics_options", kinematics::KinematicsQueryOptions(),
|
||||
"KinematicsQueryOptions to pass to CartesianInterpolator");
|
||||
@ -120,7 +121,7 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene
|
||||
double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
|
||||
&(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
|
||||
moveit::core::MaxEEFStep(props.get<double>("step_size")),
|
||||
moveit::core::JumpThreshold::relative(props.get<double>("jump_threshold")), is_valid,
|
||||
props.get<moveit::core::CartesianPrecision>("precision"), is_valid,
|
||||
props.get<kinematics::KinematicsQueryOptions>("kinematics_options"),
|
||||
props.get<kinematics::KinematicsBase::IKCostFn>("kinematics_cost_fn"), offset);
|
||||
|
||||
|
||||
@ -102,7 +102,8 @@ StagePrivate::StagePrivate(Stage* me, const std::string& name)
|
||||
, cost_term_{ std::make_unique<CostTerm>() }
|
||||
, total_compute_time_{}
|
||||
, parent_{ nullptr }
|
||||
, introspection_{ nullptr } {}
|
||||
, introspection_{ nullptr }
|
||||
, preempt_requested_{ nullptr } {}
|
||||
|
||||
StagePrivate& StagePrivate::operator=(StagePrivate&& other) {
|
||||
assert(typeid(*this) == typeid(other));
|
||||
|
||||
@ -191,6 +191,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
|
||||
double max_distance = props.get<double>("max_distance");
|
||||
double min_distance = props.get<double>("min_distance");
|
||||
|
||||
// if min_distance == max_distance, resort to moving full distance (negative min_distance)
|
||||
if (max_distance > 0.0 && std::fabs(max_distance - min_distance) < std::numeric_limits<double>::epsilon())
|
||||
min_distance = -1.0;
|
||||
|
||||
const auto& path_constraints = props.get<moveit_msgs::msg::Constraints>("path_constraints");
|
||||
|
||||
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
|
||||
@ -300,7 +305,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
comment = result.message;
|
||||
solution.setPlannerId(planner_->getPlannerId());
|
||||
|
||||
if (robot_trajectory) { // the following requires a robot_trajectory returned from planning
|
||||
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) { // the following requires a robot_trajectory
|
||||
// returned from planning
|
||||
moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
|
||||
reached_state->updateLinkTransforms();
|
||||
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset;
|
||||
@ -335,7 +341,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
}
|
||||
|
||||
// store result
|
||||
if (robot_trajectory) {
|
||||
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
|
||||
scene->setCurrentState(robot_trajectory->getLastWayPoint());
|
||||
if (dir == Interface::BACKWARD)
|
||||
robot_trajectory->reverse();
|
||||
|
||||
@ -46,6 +46,8 @@
|
||||
#include <moveit/robot_model_loader/robot_model_loader.h>
|
||||
#include <moveit/planning_pipeline/planning_pipeline.h>
|
||||
|
||||
#include <scope_guard/scope_guard.hpp>
|
||||
|
||||
#include <functional>
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
@ -214,11 +216,12 @@ void Task::init() {
|
||||
// task expects its wrapped child to push to both ends, this triggers interface resolution
|
||||
stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE }));
|
||||
|
||||
// provide introspection instance to all stages
|
||||
// provide introspection instance and preempt_requested to all stages
|
||||
auto* introspection = impl->introspection_.get();
|
||||
impl->traverseStages(
|
||||
[introspection](Stage& stage, int /*depth*/) {
|
||||
[introspection, impl](Stage& stage, int /*depth*/) {
|
||||
stage.pimpl()->setIntrospection(introspection);
|
||||
stage.pimpl()->setPreemptRequestedMember(&impl->preempt_requested_);
|
||||
return true;
|
||||
},
|
||||
1, UINT_MAX);
|
||||
@ -233,10 +236,17 @@ bool Task::canCompute() const {
|
||||
}
|
||||
|
||||
void Task::compute() {
|
||||
stages()->pimpl()->runCompute();
|
||||
try {
|
||||
stages()->pimpl()->runCompute();
|
||||
} catch (const PreemptStageException& e) {
|
||||
// do nothing, needed for early stop
|
||||
}
|
||||
}
|
||||
|
||||
moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
|
||||
// ensure the preempt request is resetted once this method exits
|
||||
auto guard = sg::make_scope_guard([this]() noexcept { this->resetPreemptRequest(); });
|
||||
|
||||
auto impl = pimpl();
|
||||
init();
|
||||
|
||||
@ -248,7 +258,6 @@ moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
|
||||
explainFailure();
|
||||
return error_code;
|
||||
};
|
||||
impl->preempt_requested_ = false;
|
||||
const double available_time = timeout();
|
||||
const auto start_time = std::chrono::steady_clock::now();
|
||||
while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) {
|
||||
@ -269,6 +278,10 @@ void Task::preempt() {
|
||||
pimpl()->preempt_requested_ = true;
|
||||
}
|
||||
|
||||
void Task::resetPreemptRequest() {
|
||||
pimpl()->preempt_requested_ = false;
|
||||
}
|
||||
|
||||
moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
|
||||
// Add random ID to prevent warnings about multiple publishers within the same node
|
||||
auto node = rclcpp::Node::make_shared("moveit_task_constructor_executor_" +
|
||||
|
||||
@ -65,6 +65,7 @@ struct GeneratorMockup : public Generator
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
bool canCompute() const override;
|
||||
void compute() override;
|
||||
virtual void reset() override { runs_ = 0; };
|
||||
};
|
||||
|
||||
struct MonitoringGeneratorMockup : public MonitoringGenerator
|
||||
@ -81,6 +82,7 @@ struct MonitoringGeneratorMockup : public MonitoringGenerator
|
||||
bool canCompute() const override { return false; }
|
||||
void compute() override {}
|
||||
void onNewSolution(const SolutionBase& s) override;
|
||||
virtual void reset() override { runs_ = 0; };
|
||||
};
|
||||
|
||||
struct ConnectMockup : public Connecting
|
||||
@ -97,6 +99,7 @@ struct ConnectMockup : public Connecting
|
||||
using Connecting::compatible; // make this accessible for testing
|
||||
|
||||
void compute(const InterfaceState& from, const InterfaceState& to) override;
|
||||
virtual void reset() override { runs_ = 0; };
|
||||
};
|
||||
|
||||
struct PropagatorMockup : public PropagatingEitherWay
|
||||
@ -113,6 +116,7 @@ struct PropagatorMockup : public PropagatingEitherWay
|
||||
|
||||
void computeForward(const InterfaceState& from) override;
|
||||
void computeBackward(const InterfaceState& to) override;
|
||||
virtual void reset() override { runs_ = 0; };
|
||||
};
|
||||
|
||||
struct ForwardMockup : public PropagatorMockup
|
||||
|
||||
@ -672,3 +672,48 @@ TEST(Task, timeout) {
|
||||
EXPECT_TRUE(t.plan());
|
||||
EXPECT_EQ(t.solutions().size(), 2u);
|
||||
}
|
||||
|
||||
// https://github.com/moveit/moveit_task_constructor/pull/597
|
||||
// https://github.com/moveit/moveit_task_constructor/pull/598
|
||||
// start planning in another thread, then preempt it in this thread
|
||||
TEST_F(TaskTestBase, preempt) {
|
||||
moveit::core::MoveItErrorCode ec;
|
||||
resetMockupIds();
|
||||
|
||||
auto timeout = std::chrono::milliseconds(10);
|
||||
auto gen1 = add(t, new GeneratorMockup(PredefinedCosts::constant(0.0)));
|
||||
auto fwd1 = add(t, new TimedForwardMockup(timeout));
|
||||
auto fwd2 = add(t, new TimedForwardMockup(timeout));
|
||||
|
||||
// preempt before preempt_request_ is reset in plan()
|
||||
{
|
||||
std::thread thread{ [&ec, this, timeout] {
|
||||
std::this_thread::sleep_for(timeout);
|
||||
ec = t.plan(1);
|
||||
} };
|
||||
t.preempt();
|
||||
thread.join();
|
||||
}
|
||||
|
||||
EXPECT_EQ(ec, moveit::core::MoveItErrorCode::PREEMPTED);
|
||||
EXPECT_EQ(t.solutions().size(), 0u);
|
||||
EXPECT_EQ(gen1->runs_, 0u);
|
||||
EXPECT_EQ(fwd1->runs_, 0u);
|
||||
EXPECT_EQ(fwd2->runs_, 0u);
|
||||
EXPECT_TRUE(t.plan(1)); // make sure the preempt request has been resetted on the previous call to plan()
|
||||
|
||||
t.reset();
|
||||
{
|
||||
std::thread thread{ [&ec, this] { ec = t.plan(1); } };
|
||||
std::this_thread::sleep_for(timeout / 2.0);
|
||||
t.preempt();
|
||||
thread.join();
|
||||
}
|
||||
|
||||
EXPECT_EQ(ec, moveit::core::MoveItErrorCode::PREEMPTED);
|
||||
EXPECT_EQ(t.solutions().size(), 0u);
|
||||
EXPECT_EQ(gen1->runs_, 1u);
|
||||
EXPECT_EQ(fwd1->runs_, 1u);
|
||||
EXPECT_EQ(fwd2->runs_, 0u);
|
||||
EXPECT_TRUE(t.plan(1)); // make sure the preempt request has been resetted on the previous call to plan()
|
||||
}
|
||||
|
||||
@ -1,7 +1,8 @@
|
||||
#include <moveit/task_constructor/container_p.h>
|
||||
#include <moveit/task_constructor/stage_p.h>
|
||||
#include <moveit/task_constructor/task_p.h>
|
||||
#include <moveit/task_constructor/stages/fixed_state.h>
|
||||
#include <moveit/task_constructor/stages/predicate_filter.h>
|
||||
#include <moveit/task_constructor/stages/noop.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
#include "stage_mockups.h"
|
||||
@ -160,6 +161,26 @@ TEST_F(FallbacksFixturePropagate, activeChildReset) {
|
||||
EXPECT_COSTS(fwd2->solutions(), testing::IsEmpty());
|
||||
}
|
||||
|
||||
// https://github.com/moveit/moveit_task_constructor/issues/581#issuecomment-2147985474
|
||||
TEST_F(FallbacksFixturePropagate, filterPropagatesFailures) {
|
||||
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(0.0)));
|
||||
|
||||
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
|
||||
auto add_filtered_fwd = [&fallbacks](double cost, bool accept) {
|
||||
auto fwd = std::make_unique<ForwardMockup>(PredefinedCosts::constant(cost));
|
||||
auto filter = std::make_unique<stages::PredicateFilter>("filter", std::move(fwd));
|
||||
filter->setPredicate([accept](const SolutionBase& /*solution*/, std::string& /*comment*/) { return accept; });
|
||||
fallbacks->add(std::move(filter));
|
||||
};
|
||||
add_filtered_fwd(INF, false); // Propagate fails, filter rejects
|
||||
add_filtered_fwd(2.0, true); // Propagate succeeds, filter accepts
|
||||
fallbacks->add(std::make_unique<stages::NoOp>());
|
||||
t.add(std::move(fallbacks));
|
||||
|
||||
EXPECT_TRUE(t.plan());
|
||||
EXPECT_COSTS(t.solutions(), testing::ElementsAre(2.));
|
||||
}
|
||||
|
||||
using FallbacksFixtureConnect = TaskTestBase;
|
||||
|
||||
TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) {
|
||||
|
||||
@ -34,7 +34,6 @@ int main(int argc, char** argv) {
|
||||
|
||||
// setup solvers
|
||||
auto cartesian = std::make_shared<solvers::CartesianPath>();
|
||||
cartesian->setJumpThreshold(2.0);
|
||||
|
||||
const auto ptp = [&node]() {
|
||||
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner", "PTP") };
|
||||
|
||||
@ -11,7 +11,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
|
||||
geometry_msgs
|
||||
rclcpp
|
||||
tf2_eigen
|
||||
urdfdom
|
||||
urdfdom_headers
|
||||
std_msgs
|
||||
visualization_msgs
|
||||
)
|
||||
|
||||
|
||||
@ -1,8 +1,9 @@
|
||||
#pragma once
|
||||
|
||||
#include <visualization_msgs/msg/marker.hpp>
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <Eigen/Geometry>
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <std_msgs/msg/color_rgba.hpp>
|
||||
#include <visualization_msgs/msg/marker.hpp>
|
||||
|
||||
namespace urdf {
|
||||
class Geometry;
|
||||
|
||||
@ -11,12 +11,15 @@
|
||||
<buildtool_depend>eigen3_cmake_module</buildtool_depend>
|
||||
<buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
|
||||
<build_depend>eigen</build_depend>
|
||||
<build_export_depend>eigen</build_export_depend>
|
||||
|
||||
<build_depend>liburdfdom-headers-dev</build_depend>
|
||||
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>moveit_common</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>tf2_eigen</depend>
|
||||
<depend>urdfdom</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>visualization_msgs</depend>
|
||||
|
||||
<export>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user