Merge branch 'master' into ros2

This commit is contained in:
Robert Haschke 2024-09-17 11:32:13 +02:00
commit cc7f9f0585
23 changed files with 180 additions and 37 deletions

View File

@ -59,15 +59,14 @@ jobs:
submodules: recursive submodules: recursive
- name: Cache ccache - name: Cache ccache
uses: rhaschke/cache@main uses: actions/cache@v4
with: with:
save-always: true
path: ${{ env.CCACHE_DIR }} path: ${{ env.CCACHE_DIR }}
key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
restore-keys: | restore-keys: |
ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}
ccache-${{ env.CACHE_PREFIX }} ccache-${{ env.CACHE_PREFIX }}
env:
GHA_CACHE_SAVE: always
- id: ici - id: ici
name: Run industrial_ci name: Run industrial_ci

3
.gitmodules vendored
View File

@ -3,3 +3,6 @@
url = https://github.com/pybind/pybind11 url = https://github.com/pybind/pybind11
branch = smart_holder branch = smart_holder
shallow = true shallow = true
[submodule "core/src/scope_guard"]
path = core/src/scope_guard
url = https://github.com/ricab/scope_guard

View File

@ -29,7 +29,7 @@ repos:
- id: trailing-whitespace - id: trailing-whitespace
- repo: https://github.com/psf/black - repo: https://github.com/psf/black
rev: 22.3.0 rev: 24.8.0
hooks: hooks:
- id: black - id: black
args: ["--line-length", "100"] args: ["--line-length", "100"]

View File

@ -185,8 +185,10 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
exec_traj.effect_on_success = [this, exec_traj.effect_on_success = [this,
&scene_diff = const_cast<::moveit_msgs::msg::PlanningScene&>(sub_traj.scene_diff), &scene_diff = const_cast<::moveit_msgs::msg::PlanningScene&>(sub_traj.scene_diff),
description](const plan_execution::ExecutableMotionPlan* /*plan*/) { 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.joint_state = sensor_msgs::msg::JointState();
scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState(); 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)) { if (!moveit::core::isEmpty(scene_diff)) {
RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description); RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description);

View File

@ -64,9 +64,9 @@ public:
template <typename PropertyType, typename... Extra> template <typename PropertyType, typename... Extra>
class_& property(const char* name, const Extra&... extra) { class_& property(const char* name, const Extra&... extra) {
PropertyConverter<PropertyType>(); // register corresponding property converter PropertyConverter<PropertyType>(); // register corresponding property converter
auto getter = [name](const type_& self) { auto getter = [name](type_& self) -> PropertyType& {
const moveit::task_constructor::PropertyMap& props = self.properties(); moveit::task_constructor::PropertyMap& props = self.properties();
return props.get<PropertyType>(name); return const_cast<PropertyType&>(props.get<PropertyType>(name));
}; };
auto setter = [name](type_& self, const PropertyType& value) { auto setter = [name](type_& self, const PropertyType& value) {
moveit::task_constructor::PropertyMap& props = self.properties(); moveit::task_constructor::PropertyMap& props = self.properties();

View File

@ -39,6 +39,7 @@
#pragma once #pragma once
#include <moveit/task_constructor/solvers/planner_interface.h> #include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/robot_state/cartesian_interpolator.h>
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
@ -57,13 +58,17 @@ public:
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); } void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
void setStepSize(double step_size) { setProperty("step_size", step_size); } 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); } void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }
[[deprecated("Replace with setMaxVelocityScalingFactor")]] // clang-format off [[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 [[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; 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; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, 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, const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override; const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
std::string getPlannerId() const override { return "CartesianPath"; } std::string getPlannerId() const override { return "CartesianPath"; }

View File

@ -57,6 +57,13 @@
namespace moveit { namespace moveit {
namespace task_constructor { namespace task_constructor {
/// exception thrown by StagePrivate::runCompute()
class PreemptStageException : public std::exception
{
public:
explicit PreemptStageException() {}
};
class ContainerBase; class ContainerBase;
class StagePrivate class StagePrivate
{ {
@ -146,6 +153,10 @@ public:
bool storeFailures() const { return introspection_ != nullptr; } bool storeFailures() const { return introspection_ != nullptr; }
void runCompute() { void runCompute() {
RCLCPP_DEBUG_STREAM(LOGGER, fmt::format("Computing stage '{}'", name())); RCLCPP_DEBUG_STREAM(LOGGER, fmt::format("Computing stage '{}'", name()));
if (preempted())
throw PreemptStageException();
auto compute_start_time = std::chrono::steady_clock::now(); auto compute_start_time = std::chrono::steady_clock::now();
try { try {
compute(); compute();
@ -159,6 +170,11 @@ public:
/** compute cost for solution through configured CostTerm */ /** compute cost for solution through configured CostTerm */
void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution); 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: protected:
StagePrivate& operator=(StagePrivate&& other); StagePrivate& operator=(StagePrivate&& other);
@ -197,6 +213,8 @@ private:
InterfaceWeakPtr next_starts_; // interface to be used for sendForward() InterfaceWeakPtr next_starts_; // interface to be used for sendForward()
Introspection* introspection_; // task's introspection instance Introspection* introspection_; // task's introspection instance
const std::atomic<bool>* preempt_requested_;
inline static const rclcpp::Logger LOGGER = rclcpp::get_logger("stage"); inline static const rclcpp::Logger LOGGER = rclcpp::get_logger("stage");
}; };
PIMPL_FUNCTIONS(Stage) PIMPL_FUNCTIONS(Stage)

View File

@ -129,8 +129,9 @@ public:
/// 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
moveit::core::MoveItErrorCode plan(size_t max_solutions = 0); moveit::core::MoveItErrorCode plan(size_t max_solutions = 0);
/// interrupt current planning (or execution) /// interrupt current planning
void preempt(); void preempt();
void resetPreemptRequest();
/// execute solution, return the result /// execute solution, return the result
moveit::core::MoveItErrorCode execute(const SolutionBase& s); moveit::core::MoveItErrorCode execute(const SolutionBase& s);

View File

@ -63,7 +63,7 @@ private:
std::string ns_; std::string ns_;
robot_model_loader::RobotModelLoaderPtr robot_model_loader_; robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
moveit::core::RobotModelConstPtr robot_model_; moveit::core::RobotModelConstPtr robot_model_;
bool preempt_requested_; std::atomic<bool> preempt_requested_;
// introspection and monitoring // introspection and monitoring
std::unique_ptr<Introspection> introspection_; std::unique_ptr<Introspection> introspection_;

View File

@ -38,6 +38,7 @@
#include <moveit/task_constructor/solvers/joint_interpolation.h> #include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/task_constructor/solvers/multi_planner.h> #include <moveit/task_constructor/solvers/multi_planner.h>
#include <moveit_msgs/msg/workspace_parameters.hpp> #include <moveit_msgs/msg/workspace_parameters.hpp>
#include <fmt/core.h>
#include "utils.h" #include "utils.h"
namespace py = pybind11; 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") .property<double>("max_step", "float: Limit any (single) joint change between two waypoints to this amount")
.def(py::init<>()); .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"( properties::class_<CartesianPath, PlannerInterface>(m, "CartesianPath", R"(
Perform linear interpolation between Cartesian poses. Perform linear interpolation between Cartesian poses.
Fails on collision along the interpolation path. There is no obstacle avoidance. :: 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 # Instantiate Cartesian-space interpolation planner
cartesianPlanner = core.CartesianPath() cartesianPlanner = core.CartesianPath()
cartesianPlanner.step_size = 0.01 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 " .property<double>("step_size", "float: Limit the Cartesian displacement between consecutive waypoints "
"In contrast to joint-space interpolation, the Cartesian planner can also " "In contrast to joint-space interpolation, the Cartesian planner can also "
"succeed when only a fraction of the linear path was feasible.") "succeed when only a fraction of the linear path was feasible.")
.property<double>( .property<moveit::core::CartesianPrecision>("precision", "Cartesian interpolation precision")
"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<double>("min_fraction", "float: Fraction of overall distance required to succeed.") .property<double>("min_fraction", "float: Fraction of overall distance required to succeed.")
.def(py::init<>()); .def(py::init<>());

View File

@ -14,10 +14,14 @@ def setUpModule():
rclcpp.init() rclcpp.init()
def tearDownModule():
rclcpp.shutdown()
# When py_binding_tools and MTC are compiled with different pybind11 versions, # When py_binding_tools and MTC are compiled with different pybind11 versions,
# the corresponding classes are not interoperable. # the corresponding classes are not interoperable.
def check_pybind11_incompatibility(): def check_pybind11_incompatibility():
rclcpp.init([]) rclcpp.init()
node = rclcpp.Node("dummy") node = rclcpp.Node("dummy")
try: try:
core.PipelinePlanner(node) core.PipelinePlanner(node)
@ -33,8 +37,8 @@ incompatible_pybind11_msg = "MoveIt and MTC use incompatible pybind11 versions"
class TestPropertyMap(unittest.TestCase): class TestPropertyMap(unittest.TestCase):
def setUp(self): def setUp(self):
self.node = rclcpp.Node("test_mtc_props")
self.props = core.PropertyMap() self.props = core.PropertyMap()
def _check(self, name, value): def _check(self, name, value):
@ -54,7 +58,8 @@ class TestPropertyMap(unittest.TestCase):
@unittest.skipIf(incompatible_pybind11, incompatible_pybind11_msg) @unittest.skipIf(incompatible_pybind11, incompatible_pybind11_msg)
def test_assign_in_reference(self): 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 = planner.properties
props["goal_joint_tolerance"] = 3.14 props["goal_joint_tolerance"] = 3.14

1
core/src/scope_guard Submodule

@ -0,0 +1 @@
Subproject commit 71a04528184db1749dd08ebbbf4daf3b5dca21fd

View File

@ -60,7 +60,8 @@ CartesianPath::CartesianPath() {
auto& p = properties(); auto& p = properties();
p.declare<geometry_msgs::msg::PoseStamped>("ik_frame", "frame to move linearly (use for joint-space target)"); 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>("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<double>("min_fraction", 1.0, "fraction of motion required for success");
p.declare<kinematics::KinematicsQueryOptions>("kinematics_options", kinematics::KinematicsQueryOptions(), p.declare<kinematics::KinematicsQueryOptions>("kinematics_options", kinematics::KinematicsQueryOptions(),
"KinematicsQueryOptions to pass to CartesianInterpolator"); "KinematicsQueryOptions to pass to CartesianInterpolator");
@ -120,7 +121,7 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene
double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath( double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
&(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true, &(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
moveit::core::MaxEEFStep(props.get<double>("step_size")), 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::KinematicsQueryOptions>("kinematics_options"),
props.get<kinematics::KinematicsBase::IKCostFn>("kinematics_cost_fn"), offset); props.get<kinematics::KinematicsBase::IKCostFn>("kinematics_cost_fn"), offset);

View File

@ -102,7 +102,8 @@ StagePrivate::StagePrivate(Stage* me, const std::string& name)
, cost_term_{ std::make_unique<CostTerm>() } , cost_term_{ std::make_unique<CostTerm>() }
, total_compute_time_{} , total_compute_time_{}
, parent_{ nullptr } , parent_{ nullptr }
, introspection_{ nullptr } {} , introspection_{ nullptr }
, preempt_requested_{ nullptr } {}
StagePrivate& StagePrivate::operator=(StagePrivate&& other) { StagePrivate& StagePrivate::operator=(StagePrivate&& other) {
assert(typeid(*this) == typeid(other)); assert(typeid(*this) == typeid(other));

View File

@ -191,6 +191,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
double max_distance = props.get<double>("max_distance"); double max_distance = props.get<double>("max_distance");
double min_distance = props.get<double>("min_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"); const auto& path_constraints = props.get<moveit_msgs::msg::Constraints>("path_constraints");
robot_trajectory::RobotTrajectoryPtr robot_trajectory; robot_trajectory::RobotTrajectoryPtr robot_trajectory;
@ -300,7 +305,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
comment = result.message; comment = result.message;
solution.setPlannerId(planner_->getPlannerId()); 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(); moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
reached_state->updateLinkTransforms(); reached_state->updateLinkTransforms();
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset; 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 // store result
if (robot_trajectory) { if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
scene->setCurrentState(robot_trajectory->getLastWayPoint()); scene->setCurrentState(robot_trajectory->getLastWayPoint());
if (dir == Interface::BACKWARD) if (dir == Interface::BACKWARD)
robot_trajectory->reverse(); robot_trajectory->reverse();

View File

@ -46,6 +46,8 @@
#include <moveit/robot_model_loader/robot_model_loader.h> #include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_pipeline/planning_pipeline.h> #include <moveit/planning_pipeline/planning_pipeline.h>
#include <scope_guard/scope_guard.hpp>
#include <functional> #include <functional>
using namespace std::chrono_literals; 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 // task expects its wrapped child to push to both ends, this triggers interface resolution
stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE })); 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(); auto* introspection = impl->introspection_.get();
impl->traverseStages( impl->traverseStages(
[introspection](Stage& stage, int /*depth*/) { [introspection, impl](Stage& stage, int /*depth*/) {
stage.pimpl()->setIntrospection(introspection); stage.pimpl()->setIntrospection(introspection);
stage.pimpl()->setPreemptRequestedMember(&impl->preempt_requested_);
return true; return true;
}, },
1, UINT_MAX); 1, UINT_MAX);
@ -233,10 +236,17 @@ bool Task::canCompute() const {
} }
void Task::compute() { void Task::compute() {
try {
stages()->pimpl()->runCompute(); stages()->pimpl()->runCompute();
} catch (const PreemptStageException& e) {
// do nothing, needed for early stop
}
} }
moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) { 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(); auto impl = pimpl();
init(); init();
@ -248,7 +258,6 @@ moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
explainFailure(); explainFailure();
return error_code; return error_code;
}; };
impl->preempt_requested_ = false;
const double available_time = timeout(); const double available_time = timeout();
const auto start_time = std::chrono::steady_clock::now(); const auto start_time = std::chrono::steady_clock::now();
while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) { while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) {
@ -269,6 +278,10 @@ void Task::preempt() {
pimpl()->preempt_requested_ = true; pimpl()->preempt_requested_ = true;
} }
void Task::resetPreemptRequest() {
pimpl()->preempt_requested_ = false;
}
moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) { moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
// Add random ID to prevent warnings about multiple publishers within the same node // Add random ID to prevent warnings about multiple publishers within the same node
auto node = rclcpp::Node::make_shared("moveit_task_constructor_executor_" + auto node = rclcpp::Node::make_shared("moveit_task_constructor_executor_" +

View File

@ -65,6 +65,7 @@ struct GeneratorMockup : public Generator
void init(const moveit::core::RobotModelConstPtr& robot_model) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override; bool canCompute() const override;
void compute() override; void compute() override;
virtual void reset() override { runs_ = 0; };
}; };
struct MonitoringGeneratorMockup : public MonitoringGenerator struct MonitoringGeneratorMockup : public MonitoringGenerator
@ -81,6 +82,7 @@ struct MonitoringGeneratorMockup : public MonitoringGenerator
bool canCompute() const override { return false; } bool canCompute() const override { return false; }
void compute() override {} void compute() override {}
void onNewSolution(const SolutionBase& s) override; void onNewSolution(const SolutionBase& s) override;
virtual void reset() override { runs_ = 0; };
}; };
struct ConnectMockup : public Connecting struct ConnectMockup : public Connecting
@ -97,6 +99,7 @@ struct ConnectMockup : public Connecting
using Connecting::compatible; // make this accessible for testing using Connecting::compatible; // make this accessible for testing
void compute(const InterfaceState& from, const InterfaceState& to) override; void compute(const InterfaceState& from, const InterfaceState& to) override;
virtual void reset() override { runs_ = 0; };
}; };
struct PropagatorMockup : public PropagatingEitherWay struct PropagatorMockup : public PropagatingEitherWay
@ -113,6 +116,7 @@ struct PropagatorMockup : public PropagatingEitherWay
void computeForward(const InterfaceState& from) override; void computeForward(const InterfaceState& from) override;
void computeBackward(const InterfaceState& to) override; void computeBackward(const InterfaceState& to) override;
virtual void reset() override { runs_ = 0; };
}; };
struct ForwardMockup : public PropagatorMockup struct ForwardMockup : public PropagatorMockup

View File

@ -672,3 +672,48 @@ TEST(Task, timeout) {
EXPECT_TRUE(t.plan()); EXPECT_TRUE(t.plan());
EXPECT_EQ(t.solutions().size(), 2u); 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()
}

View File

@ -1,7 +1,8 @@
#include <moveit/task_constructor/container_p.h> #include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/stage_p.h> #include <moveit/task_constructor/stage_p.h>
#include <moveit/task_constructor/task_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 <moveit/planning_scene/planning_scene.h>
#include "stage_mockups.h" #include "stage_mockups.h"
@ -160,6 +161,26 @@ TEST_F(FallbacksFixturePropagate, activeChildReset) {
EXPECT_COSTS(fwd2->solutions(), testing::IsEmpty()); 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; using FallbacksFixtureConnect = TaskTestBase;
TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) { TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) {

View File

@ -34,7 +34,6 @@ int main(int argc, char** argv) {
// setup solvers // setup solvers
auto cartesian = std::make_shared<solvers::CartesianPath>(); auto cartesian = std::make_shared<solvers::CartesianPath>();
cartesian->setJumpThreshold(2.0);
const auto ptp = [&node]() { const auto ptp = [&node]() {
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner", "PTP") }; auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner", "PTP") };

View File

@ -11,7 +11,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
geometry_msgs geometry_msgs
rclcpp rclcpp
tf2_eigen tf2_eigen
urdfdom urdfdom_headers
std_msgs
visualization_msgs visualization_msgs
) )

View File

@ -1,8 +1,9 @@
#pragma once #pragma once
#include <visualization_msgs/msg/marker.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <Eigen/Geometry> #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 { namespace urdf {
class Geometry; class Geometry;

View File

@ -11,12 +11,15 @@
<buildtool_depend>eigen3_cmake_module</buildtool_depend> <buildtool_depend>eigen3_cmake_module</buildtool_depend>
<buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend> <buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
<build_depend>eigen</build_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>geometry_msgs</depend>
<depend>moveit_common</depend> <depend>moveit_common</depend>
<depend>rclcpp</depend> <depend>rclcpp</depend>
<depend>tf2_eigen</depend> <depend>tf2_eigen</depend>
<depend>urdfdom</depend> <depend>std_msgs</depend>
<depend>visualization_msgs</depend> <depend>visualization_msgs</depend>
<export> <export>