diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 845e3266..cd387f27 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -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 diff --git a/.gitmodules b/.gitmodules index 9339bbb2..d7508c72 100644 --- a/.gitmodules +++ b/.gitmodules @@ -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 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 24733c4d..7a8c292a 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -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"] diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 1bc461e1..565ab247 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -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); diff --git a/core/include/moveit/python/task_constructor/properties.h b/core/include/moveit/python/task_constructor/properties.h index e661fa35..90efd8e1 100644 --- a/core/include/moveit/python/task_constructor/properties.h +++ b/core/include/moveit/python/task_constructor/properties.h @@ -64,9 +64,9 @@ public: template class_& property(const char* name, const Extra&... extra) { PropertyConverter(); // register corresponding property converter - auto getter = [name](const type_& self) { - const moveit::task_constructor::PropertyMap& props = self.properties(); - return props.get(name); + auto getter = [name](type_& self) -> PropertyType& { + moveit::task_constructor::PropertyMap& props = self.properties(); + return const_cast(props.get(name)); }; auto setter = [name](type_& self, const PropertyType& value) { moveit::task_constructor::PropertyMap& props = self.properties(); diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 1d3c37ba..0d4ef8a4 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -39,6 +39,7 @@ #pragma once #include +#include 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 + void setJumpThreshold(double) { + static_assert(std::is_integral::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"; } diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index f3d6ac7e..900853f8 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -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* 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* preempt_requested_; + inline static const rclcpp::Logger LOGGER = rclcpp::get_logger("stage"); }; PIMPL_FUNCTIONS(Stage) diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index ca0cb661..fae0f721 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -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); diff --git a/core/include/moveit/task_constructor/task_p.h b/core/include/moveit/task_constructor/task_p.h index f0e6dc0a..ef42dd34 100644 --- a/core/include/moveit/task_constructor/task_p.h +++ b/core/include/moveit/task_constructor/task_p.h @@ -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 preempt_requested_; // introspection and monitoring std::unique_ptr introspection_; diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index b9635a80..0952d4b5 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include "utils.h" namespace py = pybind11; @@ -96,6 +97,22 @@ void export_solvers(py::module& m) { .property("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_(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_(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("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( - "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("precision", "Cartesian interpolation precision") .property("min_fraction", "float: Fraction of overall distance required to succeed.") .def(py::init<>()); diff --git a/core/python/test/test_mtc.py b/core/python/test/test_mtc.py index 77a57645..8340d8cb 100644 --- a/core/python/test/test_mtc.py +++ b/core/python/test/test_mtc.py @@ -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 diff --git a/core/src/scope_guard b/core/src/scope_guard new file mode 160000 index 00000000..71a04528 --- /dev/null +++ b/core/src/scope_guard @@ -0,0 +1 @@ +Subproject commit 71a04528184db1749dd08ebbbf4daf3b5dca21fd diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 588ba837..0a5494a4 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -60,7 +60,8 @@ CartesianPath::CartesianPath() { auto& p = properties(); p.declare("ik_frame", "frame to move linearly (use for joint-space target)"); 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("precision", moveit::core::CartesianPrecision(), + "precision of linear path"); p.declare("min_fraction", 1.0, "fraction of motion required for success"); p.declare("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("step_size")), - moveit::core::JumpThreshold::relative(props.get("jump_threshold")), is_valid, + props.get("precision"), is_valid, props.get("kinematics_options"), props.get("kinematics_cost_fn"), offset); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 73536cbf..733f61a7 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -102,7 +102,8 @@ StagePrivate::StagePrivate(Stage* me, const std::string& name) , cost_term_{ std::make_unique() } , total_compute_time_{} , parent_{ nullptr } - , introspection_{ nullptr } {} + , introspection_{ nullptr } + , preempt_requested_{ nullptr } {} StagePrivate& StagePrivate::operator=(StagePrivate&& other) { assert(typeid(*this) == typeid(other)); diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 59e0cbb0..46e4208b 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -191,6 +191,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning double max_distance = props.get("max_distance"); double min_distance = props.get("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::epsilon()) + min_distance = -1.0; + const auto& path_constraints = props.get("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(); diff --git a/core/src/task.cpp b/core/src/task.cpp index df850826..9c31d5db 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -46,6 +46,8 @@ #include #include +#include + #include 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_" + diff --git a/core/test/stage_mockups.h b/core/test/stage_mockups.h index fc75c633..35d01e65 100644 --- a/core/test/stage_mockups.h +++ b/core/test/stage_mockups.h @@ -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 diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 876a1e6d..5d7b722c 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -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() +} diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp index d8b07483..49dbc69c 100644 --- a/core/test/test_fallback.cpp +++ b/core/test/test_fallback.cpp @@ -1,7 +1,8 @@ #include #include #include -#include +#include +#include #include #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(PredefinedCosts::single(0.0))); + + auto fallbacks = std::make_unique("Fallbacks"); + auto add_filtered_fwd = [&fallbacks](double cost, bool accept) { + auto fwd = std::make_unique(PredefinedCosts::constant(cost)); + auto filter = std::make_unique("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()); + t.add(std::move(fallbacks)); + + EXPECT_TRUE(t.plan()); + EXPECT_COSTS(t.solutions(), testing::ElementsAre(2.)); +} + using FallbacksFixtureConnect = TaskTestBase; TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) { diff --git a/demo/src/fallbacks_move_to.cpp b/demo/src/fallbacks_move_to.cpp index fb381b13..929dedd7 100644 --- a/demo/src/fallbacks_move_to.cpp +++ b/demo/src/fallbacks_move_to.cpp @@ -34,7 +34,6 @@ int main(int argc, char** argv) { // setup solvers auto cartesian = std::make_shared(); - cartesian->setJumpThreshold(2.0); const auto ptp = [&node]() { auto pp{ std::make_shared(node, "pilz_industrial_motion_planner", "PTP") }; diff --git a/rviz_marker_tools/CMakeLists.txt b/rviz_marker_tools/CMakeLists.txt index 8a8dc572..3311b9de 100644 --- a/rviz_marker_tools/CMakeLists.txt +++ b/rviz_marker_tools/CMakeLists.txt @@ -11,7 +11,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS geometry_msgs rclcpp tf2_eigen - urdfdom + urdfdom_headers + std_msgs visualization_msgs ) diff --git a/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h b/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h index 5533690a..31130e50 100644 --- a/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h +++ b/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h @@ -1,8 +1,9 @@ #pragma once -#include -#include #include +#include +#include +#include namespace urdf { class Geometry; diff --git a/rviz_marker_tools/package.xml b/rviz_marker_tools/package.xml index 0c798180..3805b565 100644 --- a/rviz_marker_tools/package.xml +++ b/rviz_marker_tools/package.xml @@ -11,12 +11,15 @@ eigen3_cmake_module eigen3_cmake_module eigen + eigen + + liburdfdom-headers-dev geometry_msgs moveit_common rclcpp tf2_eigen - urdfdom + std_msgs visualization_msgs