diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index 3058626b9..a40899e8d 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -16,8 +16,8 @@ jobs: - uses: actions/checkout@v4 with: submodules: recursive - - name: Install clang-format-10 - run: sudo apt-get install clang-format-10 + - name: Install clang-format-12 + run: sudo apt-get install clang-format-12 - uses: rhaschke/install-catkin_lint-action@v1.0 with: distro: noetic diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 1c9190e56..aa133c806 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -39,7 +39,7 @@ repos: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format-10 + entry: clang-format-12 language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ["-fallback-style=none", "-i"] diff --git a/capabilities/CMakeLists.txt b/capabilities/CMakeLists.txt index d0503a91f..a5cf37ce0 100644 --- a/capabilities/CMakeLists.txt +++ b/capabilities/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_task_constructor_capabilities) +find_package(fmt REQUIRED) find_package(catkin REQUIRED COMPONENTS actionlib moveit_core @@ -32,7 +33,7 @@ add_library(${PROJECT_NAME} ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} fmt::fmt) install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/capabilities/package.xml b/capabilities/package.xml index 98487b18b..28390cb00 100644 --- a/capabilities/package.xml +++ b/capabilities/package.xml @@ -12,6 +12,7 @@ catkin + fmt moveit_core moveit_ros_move_group actionlib diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 3c127d5dd..2a3afa9ed 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -43,6 +43,7 @@ #include #include #include +#include namespace { @@ -152,8 +153,9 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr if (!joint_names.empty()) { group = findJointModelGroup(*model, joint_names); if (!group) { - ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "Could not find JointModelGroup that actuates {" - << boost::algorithm::join(joint_names, ", ") << "}"); + ROS_ERROR_STREAM_NAMED( + "ExecuteTaskSolution", + fmt::format("Could not find JointModelGroup that actuates {{{}}}", fmt::join(joint_names, ", "))); return false; } ROS_DEBUG_NAMED("ExecuteTaskSolution", "Using JointModelGroup '%s' for execution", diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 1dd5f0f6a..12af7292b 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_task_constructor_core) find_package(Boost REQUIRED) +find_package(fmt REQUIRED) find_package(catkin REQUIRED COMPONENTS roslint tf2_eigen diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 858d51ea8..5d359b5c6 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -44,6 +44,7 @@ #include #include +#include #include #include @@ -144,7 +145,7 @@ class StagePrivate void newSolution(const SolutionBasePtr& solution); bool storeFailures() const { return introspection_ != nullptr; } void runCompute() { - ROS_DEBUG_STREAM_NAMED("Stage", "Computing stage '" << name() << "'"); + ROS_DEBUG_STREAM_NAMED("Stage", fmt::format("Computing stage '{}'", name())); auto compute_start_time = std::chrono::steady_clock::now(); try { compute(); diff --git a/core/package.xml b/core/package.xml index 1078fb066..dae50cb97 100644 --- a/core/package.xml +++ b/core/package.xml @@ -19,6 +19,7 @@ roslint roscpp + fmt tf2_eigen geometry_msgs moveit_core diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index 45cefe57c..135b0c986 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -37,7 +37,7 @@ add_library(${PROJECT_NAME} solvers/pipeline_planner.cpp solvers/multi_planner.cpp ) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} fmt::fmt) target_include_directories(${PROJECT_NAME} PUBLIC $ PUBLIC $ diff --git a/core/src/container.cpp b/core/src/container.cpp index 0fe38c795..f0feeaf91 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include using namespace std::placeholders; @@ -232,7 +232,7 @@ inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Prio } void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { - ROS_DEBUG_STREAM_NAMED("Pruning", "'" << child.name() << "' generated a failure"); + ROS_DEBUG_STREAM_NAMED("Pruning", fmt::format("'{}' generated a failure", child.name())); switch (child.pimpl()->interfaceFlags()) { case GENERATE: // just ignore: the pair of (new) states isn't known to us anyway @@ -503,8 +503,8 @@ struct SolutionCollector }; void SerialContainer::onNewSolution(const SolutionBase& current) { - ROS_DEBUG_STREAM_NAMED("SerialContainer", "'" << this->name() << "' received solution of child stage '" - << current.creator()->name() << "'"); + ROS_DEBUG_STREAM_NAMED("SerialContainer", fmt::format("'{}' received solution of child stage '{}'", this->name(), + current.creator()->name())); // failures should never trigger this callback assert(!current.isFailure()); @@ -573,10 +573,10 @@ void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) else if ((flags1 & READS_END) && (flags2 & WRITES_PREV_END)) stage2.setPrevEnds(stage1.ends()); else { - boost::format desc("cannot connect end interface of '%1%' (%2%) to start interface of '%3%' (%4%)"); - desc % stage1.name() % flowSymbol(flags1); - desc % stage2.name() % flowSymbol(flags2); - throw InitStageException(*me(), desc.str()); + throw InitStageException( + *me(), fmt::format("cannot connect end interface of '{}' ({}) to start interface of '{}' ({})", // + stage1.name(), flowSymbol(flags1), // + stage2.name(), flowSymbol(flags2))); } } @@ -586,12 +586,10 @@ void SerialContainerPrivate::validateInterface(const StagePrivate& child, Interf if (required == UNKNOWN) return; // cannot yet validate InterfaceFlags child_interface = child.interfaceFlags() & mask; - if (required != child_interface) { - boost::format desc("%1% interface (%3%) of '%2%' does not match mine (%4%)"); - desc % (mask == START_IF_MASK ? "start" : "end") % child.name(); - desc % flowSymbol(child_interface) % flowSymbol(required); - throw InitStageException(*me_, desc.str()); - } + if (required != child_interface) + throw InitStageException(*me_, fmt::format("{0} interface ({2}) of '{1}' does not match mine ({3})", + (mask == START_IF_MASK ? "start" : "end"), child.name(), + flowSymbol(child_interface), flowSymbol(required))); } // called by parent asking for pruning of this' interface @@ -649,8 +647,8 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) { exceptions.append(e); } - required_interface_ = (first.pimpl()->interfaceFlags() & START_IF_MASK) | // clang-format off - (last.pimpl()->interfaceFlags() & END_IF_MASK); // clang-format off + required_interface_ = (first.pimpl()->interfaceFlags() & START_IF_MASK) | // + (last.pimpl()->interfaceFlags() & END_IF_MASK); if (exceptions) throw exceptions; @@ -705,7 +703,6 @@ void SerialContainer::compute() { } } - ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name) : ContainerBasePrivate(me, name) {} @@ -744,11 +741,11 @@ void ParallelContainerBasePrivate::initializeExternalInterfaces() { // States received by the container need to be copied to all children's pull interfaces. if (requiredInterface() & READS_START) starts() = std::make_shared([this](Interface::iterator external, Interface::UpdateFlags updated) { - this->propagateStateToAllChildren(external, updated); + this->propagateStateToAllChildren(external, updated); }); if (requiredInterface() & READS_END) ends() = std::make_shared([this](Interface::iterator external, Interface::UpdateFlags updated) { - this->propagateStateToAllChildren(external, updated); + this->propagateStateToAllChildren(external, updated); }); } @@ -763,14 +760,12 @@ void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, valid = valid & ((external & mask) == (child_interface & mask)); } - if (!valid) { - boost::format desc("interface of '%1%' (%3% %4%) does not match %2% (%5% %6%)."); - desc % child.name(); - desc % (first ? "external one" : "other children's"); - desc % flowSymbol(child_interface) % flowSymbol(child_interface); - desc % flowSymbol(external) % flowSymbol(external); - throw InitStageException(*me_, desc.str()); - } + if (!valid) + throw InitStageException( + *me_, fmt::format("interface of '{0}' ({2} {3}) does not match {1} ({4} {5}).", child.name(), + (first ? "external one" : "other children's"), // + flowSymbol(child_interface), flowSymbol(child_interface), + flowSymbol(external), flowSymbol(external))); } void ParallelContainerBasePrivate::validateConnectivity() const { @@ -784,7 +779,8 @@ void ParallelContainerBasePrivate::validateConnectivity() const { } template -void ParallelContainerBasePrivate::propagateStateToAllChildren(Interface::iterator external, Interface::UpdateFlags updated) { +void ParallelContainerBasePrivate::propagateStateToAllChildren(Interface::iterator external, + Interface::UpdateFlags updated) { for (const Stage::pointer& stage : children()) copyState(external, stage->pimpl()->pullInterface(), updated); } @@ -794,8 +790,8 @@ ParallelContainerBase::ParallelContainerBase(const std::string& name) : ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {} void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) { - pimpl()->liftSolution(std::make_shared(this, &solution, cost, std::move(comment)), - solution.start(), solution.end()); + pimpl()->liftSolution(std::make_shared(this, &solution, cost, std::move(comment)), solution.start(), + solution.end()); } void ParallelContainerBase::spawn(InterfaceState&& state, SubTrajectory&& t) { @@ -876,7 +872,7 @@ void Fallbacks::onNewSolution(const SolutionBase& s) { } inline void Fallbacks::replaceImpl() { - FallbacksPrivate *impl = pimpl(); + FallbacksPrivate* impl = pimpl(); if (pimpl()->interfaceFlags() == pimpl()->requiredInterface()) return; switch (pimpl()->requiredInterface()) { @@ -899,11 +895,10 @@ inline void Fallbacks::replaceImpl() { pimpl_ = impl; } -FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name) - : ParallelContainerBasePrivate(me, name) {} +FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} FallbacksPrivate::FallbacksPrivate(FallbacksPrivate&& other) -: ParallelContainerBasePrivate(static_cast(other.me()), "") { + : ParallelContainerBasePrivate(static_cast(other.me()), "") { // move contents of other this->ParallelContainerBasePrivate::operator=(std::move(other)); } @@ -933,8 +928,8 @@ void FallbacksPrivateCommon::reset() { } bool FallbacksPrivateCommon::canCompute() const { - while(current_ != children().end() && // not completely exhausted - !(*current_)->pimpl()->canCompute()) // but current child cannot compute + while (current_ != children().end() && // not completely exhausted + !(*current_)->pimpl()->canCompute()) // but current child cannot compute return const_cast(this)->nextJob(); // advance to next job // return value: current child is well defined and thus can compute? @@ -947,12 +942,13 @@ void FallbacksPrivateCommon::compute() { inline void FallbacksPrivateCommon::nextChild() { if (std::next(current_) != children().end()) - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*current_)->name() << "' failed, trying next one."); + ROS_DEBUG_STREAM_NAMED("Fallbacks", fmt::format("Child '{}' failed, trying next one.", (*current_)->name())); ++current_; // advance to next child } -FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old) - : FallbacksPrivateCommon(std::move(old)) { FallbacksPrivateCommon::reset(); } +FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old) : FallbacksPrivateCommon(std::move(old)) { + FallbacksPrivateCommon::reset(); +} bool FallbacksPrivateGenerator::nextJob() { assert(current_ != children().end() && !(*current_)->pimpl()->canCompute()); @@ -963,27 +959,27 @@ bool FallbacksPrivateGenerator::nextJob() { return false; } - do { nextChild(); } - while (current_ != children().end() && !(*current_)->pimpl()->canCompute()); + do { + nextChild(); + } while (current_ != children().end() && !(*current_)->pimpl()->canCompute()); // return value shall indicate current_->canCompute() return current_ != children().end(); } - FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old) - : FallbacksPrivateCommon(std::move(old)) { + : FallbacksPrivateCommon(std::move(old)) { switch (requiredInterface()) { - case PROPAGATE_FORWARDS: - dir_ = Interface::FORWARD; - starts() = std::make_shared(); - break; - case PROPAGATE_BACKWARDS: - dir_ = Interface::BACKWARD; - ends() = std::make_shared(); - break; - default: - assert(false); + case PROPAGATE_FORWARDS: + dir_ = Interface::FORWARD; + starts() = std::make_shared(); + break; + case PROPAGATE_BACKWARDS: + dir_ = Interface::BACKWARD; + ends() = std::make_shared(); + break; + default: + assert(false); } FallbacksPrivatePropagator::reset(); } @@ -1003,8 +999,8 @@ bool FallbacksPrivatePropagator::nextJob() { assert(current_ != children().end() && !(*current_)->pimpl()->canCompute()); const auto jobs = pullInterface(dir_); - if (job_ != jobs->end()) { // current job exists, but is exhausted on current child - if (!job_has_solutions_) // job didn't produce solutions -> feed to next child + if (job_ != jobs->end()) { // current job exists, but is exhausted on current child + if (!job_has_solutions_) // job didn't produce solutions -> feed to next child nextChild(); else current_ = children().end(); // indicate that this job is exhausted on all children @@ -1022,9 +1018,9 @@ bool FallbacksPrivatePropagator::nextJob() { // pick next job if needed and possible if (job_ == jobs->end()) { // need to pick next job if (!jobs->empty() && jobs->front()->priority().enabled()) - job_ = jobs->begin(); + job_ = jobs->begin(); else - return false; // no more jobs available + return false; // no more jobs available } // When arriving here, we have a valid job_ and a current_ child to feed it. Let's do that. @@ -1032,13 +1028,11 @@ bool FallbacksPrivatePropagator::nextJob() { return true; } - -FallbacksPrivateConnect::FallbacksPrivateConnect(FallbacksPrivate&& old) - : FallbacksPrivate(std::move(old)) { - starts_ = std::make_shared( - std::bind(&FallbacksPrivateConnect::propagateStateUpdate, this, std::placeholders::_1, std::placeholders::_2)); - ends_ = std::make_shared( - std::bind(&FallbacksPrivateConnect::propagateStateUpdate, this, std::placeholders::_1, std::placeholders::_2)); +FallbacksPrivateConnect::FallbacksPrivateConnect(FallbacksPrivate&& old) : FallbacksPrivate(std::move(old)) { + starts_ = std::make_shared(std::bind(&FallbacksPrivateConnect::propagateStateUpdate, + this, std::placeholders::_1, std::placeholders::_2)); + ends_ = std::make_shared(std::bind(&FallbacksPrivateConnect::propagateStateUpdate, + this, std::placeholders::_1, std::placeholders::_2)); FallbacksPrivateConnect::reset(); } @@ -1055,7 +1049,7 @@ void FallbacksPrivateConnect::propagateStateUpdate(Interface::iterator external, } bool FallbacksPrivateConnect::canCompute() const { - for (auto it=children().begin(), end=children().end(); it!=end; ++it) + for (auto it = children().begin(), end = children().end(); it != end; ++it) if ((*it)->pimpl()->canCompute()) { active_ = it; return true; @@ -1072,7 +1066,8 @@ void FallbacksPrivateConnect::compute() { void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { // expect failure to be reported from active child - assert(active_ != children().end() && active_->get() == &child); (void)child; + assert(active_ != children().end() && active_->get() == &child); + (void)child; // ... thus we can use std::next(active_) to find the next child auto next = std::next(active_); @@ -1093,7 +1088,6 @@ void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceSt parent()->pimpl()->onNewFailure(*me(), from, to); } - MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} void MergerPrivate::resolveInterface(InterfaceFlags expected) { @@ -1115,7 +1109,7 @@ void MergerPrivate::resolveInterface(InterfaceFlags expected) { Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) { properties().declare("time_parameterization", - std::make_shared()); + std::make_shared()); } void Merger::reset() { @@ -1292,8 +1286,9 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions, oss << "Invalid waypoint(s): "; if (invalid_index.size() == merged->getWayPointCount()) oss << "all"; - else for (size_t i : invalid_index) - oss << i << ", "; + else + for (size_t i : invalid_index) + oss << i << ", "; t.setComment(oss.str()); } else { // accumulate costs and markers diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index fd467ff9b..8ba3aac91 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -44,7 +44,7 @@ #include -#include +#include #include namespace moveit { @@ -206,9 +206,7 @@ double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) cons return 0.0; if (!traj->getWayPoint(0).knowsFrameTransform(link_name)) { - boost::format desc("LinkMotionCost: frame '%1%' unknown in trajectory"); - desc % link_name; - comment = desc.str(); + comment = fmt::format("LinkMotionCost: frame '{}' unknown in trajectory", link_name); return std::numeric_limits::infinity(); } @@ -243,8 +241,8 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const auto& state_properties{ state->properties() }; auto& stage_properties{ s.creator()->properties() }; request.group_name = state_properties.hasProperty(group_property) ? - state_properties.get(group_property) : - stage_properties.get(group_property); + state_properties.get(group_property) : + stage_properties.get(group_property); // look at all forbidden collisions involving group_name request.enableGroup(state->scene()->getRobotModel()); @@ -274,11 +272,10 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const return result.minimum_distance; } }; - auto collision_comment{ [=](const auto& distance) { - boost::format desc{ PREFIX + "allegedly valid solution collides between '%1%' and '%2%'" }; - desc % distance.link_names[0] % distance.link_names[1]; - return desc.str(); - } }; + auto collision_comment = [=](const auto& distance) { + return fmt::format(PREFIX + "allegedly valid solution collides between '{}' and '{}'", distance.link_names[0], + distance.link_names[1]); + }; double distance{ 0.0 }; @@ -290,13 +287,11 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const return std::numeric_limits::infinity(); } distance = distance_data.distance; - if (!cumulative) { - boost::format desc{ PREFIX + "distance %1% between '%2%' and '%3%'" }; - desc % distance % distance_data.link_names[0] % distance_data.link_names[1]; - comment = desc.str(); - } else { - comment = PREFIX + "cumulative distance " + std::to_string(distance); - } + if (!cumulative) + comment = fmt::format(PREFIX + "distance {} between '{}' and '{}'", distance, distance_data.link_names[0], + distance_data.link_names[1]); + else + comment = fmt::format(PREFIX + "cumulative distance {}", distance); } else { // check trajectory for (size_t i = 0; i < s.trajectory()->getWayPointCount(); ++i) { auto distance_data = check_distance(state, s.trajectory()->getWayPoint(i)); @@ -307,10 +302,7 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const distance += distance_data.distance; } distance /= s.trajectory()->getWayPointCount(); - - boost::format desc(PREFIX + "average%1% distance: %2%"); - desc % (cumulative ? " cumulative" : "") % distance; - comment = desc.str(); + comment = fmt::format(PREFIX + "average{} distance: {}", (cumulative ? " cumulative" : ""), distance); } return distance_to_cost(distance); diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 95858704b..95bd2570c 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -37,7 +37,8 @@ */ #include -#include +#include +#include #include #include @@ -290,8 +291,8 @@ void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMa } catch (const Property::undefined&) { } - ROS_DEBUG_STREAM_NAMED(LOGNAME, pair.first << ": " << p.initialized_from_ << " -> " << source << ": " - << Property::serialize(value)); + ROS_DEBUG_STREAM_NAMED(LOGNAME, fmt::format("{}: {} -> {}: {}", pair.first, p.initialized_from_, source, + Property::serialize(value))); p.setCurrentValue(value); p.initialized_from_ = source; } @@ -317,9 +318,8 @@ Property::undefined::undefined(const std::string& name, const std::string& msg) setName(name); } -static boost::format TYPE_ERROR_FMT("type (%1%) doesn't match property's declared type (%2%)"); Property::type_error::type_error(const std::string& current_type, const std::string& declared_type) - : Property::error(boost::str(TYPE_ERROR_FMT % current_type % declared_type)) {} + : Property::error(fmt::format("type {} doesn't match property's declared type {}", current_type, declared_type)) {} } // namespace task_constructor } // namespace moveit diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 171497ebb..5eec3e7fc 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -42,8 +42,7 @@ #include #include - -#include +#include #include #include @@ -149,12 +148,11 @@ void StagePrivate::validateConnectivity() const { // check that the required interface is provided InterfaceFlags required = requiredInterface(); InterfaceFlags actual = interfaceFlags(); - if ((required & actual) != required) { - boost::format desc("actual interface %1% %2% does not match required interface %3% %4%"); - desc % flowSymbol(actual) % flowSymbol(actual); - desc % flowSymbol(required) % flowSymbol(required); - throw InitStageException(*me(), desc.str()); - } + if ((required & actual) != required) + throw InitStageException(*me(), + fmt::format("actual interface {} {} does not match required interface {} {}", + flowSymbol(actual), flowSymbol(actual), + flowSymbol(required), flowSymbol(required))); } bool StagePrivate::storeSolution(const SolutionBasePtr& solution, const InterfaceState* from, @@ -358,7 +356,7 @@ void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) { impl->properties_.reset(); if (impl->parent()) { try { - ROS_DEBUG_STREAM_NAMED("Properties", "init '" << name() << "'"); + ROS_DEBUG_STREAM_NAMED("Properties", fmt::format("init '{}'", name())); impl->properties_.performInitFrom(PARENT, impl->parent()->properties()); } catch (const Property::error& e) { std::ostringstream oss; @@ -548,17 +546,16 @@ void PropagatingEitherWayPrivate::resolveInterface(InterfaceFlags expected) { dir = PropagatingEitherWay::FORWARD; else if ((expected & START_IF_MASK) == WRITES_PREV_END || (expected & END_IF_MASK) == READS_END) dir = PropagatingEitherWay::BACKWARD; - else { - boost::format desc("propagator cannot satisfy expected interface %1% %2%"); - desc % flowSymbol(expected) % flowSymbol(expected); - throw InitStageException(*me(), desc.str()); - } - if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_) { - boost::format desc("configured interface (%1% %2%) does not match expected one (%3% %4%)"); - desc % flowSymbol(required_interface_) % flowSymbol(required_interface_); - desc % flowSymbol(expected) % flowSymbol(expected); - throw InitStageException(*me(), desc.str()); - } + else + throw InitStageException(*me(), + fmt::format("propagator cannot satisfy expected interface {} {}", + flowSymbol(expected), flowSymbol(expected))); + if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_) + throw InitStageException(*me(), + fmt::format("configured interface ({} {}) does not match expected one ({} {})", + flowSymbol(required_interface_), + flowSymbol(required_interface_), + flowSymbol(expected), flowSymbol(expected))); initInterface(dir); } @@ -915,38 +912,33 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta const planning_scene::PlanningSceneConstPtr& from = from_state.scene(); const planning_scene::PlanningSceneConstPtr& to = to_state.scene(); - if (from->getWorld()->size() != to->getWorld()->size()) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different number of collision objects"); + auto false_with_debug = [](auto... args) { + ROS_DEBUG_STREAM_NAMED("Connecting", fmt::format(args...)); return false; - } + }; + + if (from->getWorld()->size() != to->getWorld()->size()) + return false_with_debug("{}: different number of collision objects", name()); // both scenes should have the same set of collision objects, at the same location for (const auto& from_object_pair : *from->getWorld()) { const std::string& from_object_name = from_object_pair.first; const collision_detection::World::ObjectPtr& from_object = from_object_pair.second; const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_name); - if (!to_object) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object_name); - return false; - } + if (!to_object) + return false_with_debug("{}: object missing: {}", name(), from_object_name); - if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4)) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object_name); - return false; // transforms do not match - } + if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4)) + return false_with_debug("{}: different object pose: {}", name(), from_object_name); - if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object_name); - return false; // shapes not matching - } + if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) + return false_with_debug("{}: different object shapes: {}", name(), from_object_name); for (auto from_it = from_object->shape_poses_.cbegin(), from_end = from_object->shape_poses_.cend(), to_it = to_object->shape_poses_.cbegin(); from_it != from_end; ++from_it, ++to_it) - if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different shape pose: " << from_object_name); - return false; // transforms do not match - } + if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) + return false_with_debug("{}: different shape pose: {}", name(), from_object_name); } // Also test for attached objects which have a different storage @@ -954,41 +946,32 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta std::vector to_attached; from->getCurrentState().getAttachedBodies(from_attached); to->getCurrentState().getAttachedBodies(to_attached); - if (from_attached.size() != to_attached.size()) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different number of objects"); - return false; - } + if (from_attached.size() != to_attached.size()) + return false_with_debug("{}: different number of objects", name()); for (const moveit::core::AttachedBody* from_object : from_attached) { auto it = std::find_if(to_attached.cbegin(), to_attached.cend(), [from_object](const moveit::core::AttachedBody* object) { return object->getName() == from_object->getName(); }); - if (it == to_attached.cend()) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object->getName()); - return false; - } + if (it == to_attached.cend()) + return false_with_debug("{}: object missing: {}", name(), from_object->getName()); + const moveit::core::AttachedBody* to_object = *it; - if (from_object->getAttachedLink() != to_object->getAttachedLink()) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different attach links: " << from_object->getName() - << " attached to " << from_object->getAttachedLinkName() << " / " - << to_object->getAttachedLinkName()); - return false; // links not matching - } - if (from_object->getShapes().size() != to_object->getShapes().size()) { - ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object->getName()); - return false; // shapes not matching - } + if (from_object->getAttachedLink() != to_object->getAttachedLink()) + return false_with_debug("{}: different attach links: {} attached to {} vs. {}", // + name(), from_object->getName(), // + from_object->getAttachedLink()->getName(), to_object->getAttachedLink()->getName()); + + if (from_object->getShapes().size() != to_object->getShapes().size()) + return false_with_debug("{}: different object shapes: {}", name(), from_object->getName()); auto from_it = from_object->getShapePosesInLinkFrame().cbegin(); auto from_end = from_object->getShapePosesInLinkFrame().cend(); auto to_it = to_object->getShapePosesInLinkFrame().cbegin(); for (; from_it != from_end; ++from_it, ++to_it) - if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) { - ROS_DEBUG_STREAM_NAMED("Connecting", - name() << ": different pose of attached object shape: " << from_object->getName()); - return false; // transforms do not match - } + if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) + return false_with_debug("{}: different pose of attached object shape: {}", name(), from_object->getName()); } return true; } diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 738a66178..4822998ad 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -48,6 +48,7 @@ #include #include #include +#include namespace moveit { namespace task_constructor { @@ -286,7 +287,7 @@ void ComputeIK::compute() { if (value.empty()) { // property undefined // determine IK link from eef/group if (!(link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second) : - jmg->getOnlyOneEndEffectorTip())) { + jmg->getOnlyOneEndEffectorTip())) { ROS_WARN_STREAM_NAMED("ComputeIK", "Failed to derive IK target link"); return; } @@ -298,7 +299,8 @@ void ComputeIK::compute() { tf2::fromMsg(ik_pose_msg.pose, ik_pose); if (!scene->getCurrentState().knowsFrameTransform(ik_pose_msg.header.frame_id)) { - ROS_WARN_STREAM_NAMED("ComputeIK", "ik frame unknown in robot: '" << ik_pose_msg.header.frame_id << "'"); + ROS_WARN_STREAM_NAMED("ComputeIK", + fmt::format("ik frame unknown in robot: '{}'", ik_pose_msg.header.frame_id)); return; } ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose; diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 11053eb3b..c235463b3 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -42,6 +42,8 @@ #include #include +#include +#include using namespace trajectory_processing; @@ -95,7 +97,7 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) { try { merged_jmg_.reset(task_constructor::merge(groups)); } catch (const std::runtime_error& e) { - ROS_INFO_STREAM_NAMED("Connect", this->name() << ": " << e.what() << ". Disabling merging."); + ROS_INFO_STREAM_NAMED("Connect", fmt::format("{}: {}. Disabling merging.", this->name(), e.what())); } } @@ -126,8 +128,8 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState& Eigen::Map positions_from(from.getJointPositions(jm), num); Eigen::Map positions_to(to.getJointPositions(jm), num); if (!(positions_from - positions_to).isZero(1e-4)) { - ROS_INFO_STREAM_NAMED("Connect", "Deviation in joint " << jm->getName() << ": [" << positions_from.transpose() - << "] != [" << positions_to.transpose() << "]"); + ROS_INFO_STREAM_NAMED("Connect", fmt::format("Deviation in joint {}: [{}] != [{}]", jm->getName(), + positions_from.transpose(), positions_to.transpose())); return false; } } diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index cea34e0a1..f371191f0 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -46,6 +46,7 @@ #include #include #include +#include namespace vm = visualization_msgs; namespace cd = collision_detection; @@ -79,7 +80,7 @@ bool computeCorrection(const std::vector& contacts, Eigen::Vector3d for (const cd::Contact& c : contacts) { if ((c.body_type_1 != cd::BodyTypes::WORLD_OBJECT && c.body_type_2 != cd::BodyTypes::WORLD_OBJECT)) { ROS_WARN_STREAM_NAMED("FixCollisionObjects", - "Cannot fix collision between " << c.body_name_1 << " and " << c.body_name_2); + fmt::format("Cannot fix collision between {} and {}", c.body_name_1, c.body_name_2)); return false; } if (c.body_type_1 == cd::BodyTypes::WORLD_OBJECT) diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index c24a18a1d..3a5389b35 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -41,6 +41,7 @@ #include #include +#include namespace moveit { namespace task_constructor { @@ -58,8 +59,9 @@ void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& void ModifyPlanningScene::addObject(const moveit_msgs::CollisionObject& collision_object) { if (collision_object.operation != moveit_msgs::CollisionObject::ADD) { - ROS_ERROR_STREAM_NAMED("ModifyPlanningScene", name() << ": addObject is called with object's operation not set " - "to ADD -- ignoring the object"); + ROS_ERROR_STREAM_NAMED( + "ModifyPlanningScene", + fmt::format("{}: addObject is called with object's operation not set to ADD -- ignoring the object", name())); return; } collision_objects_.push_back(collision_object); @@ -110,7 +112,7 @@ void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene& scene, if (invert) attach = !attach; obj.object.operation = attach ? static_cast(moveit_msgs::CollisionObject::ADD) : - static_cast(moveit_msgs::CollisionObject::REMOVE); + static_cast(moveit_msgs::CollisionObject::REMOVE); for (const std::string& name : pair.second.first) { obj.object.id = name; scene.processAttachedCollisionObjectMsg(obj); diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index 85ed2fdf8..8268eb855 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -129,7 +129,7 @@ void SimpleGraspBase::setup(std::unique_ptr&& generator, bool forward) { const std::string& eef = p.get("eef"); moveit_msgs::AttachedCollisionObject obj; obj.object.operation = forward ? static_cast(moveit_msgs::CollisionObject::ADD) : - static_cast(moveit_msgs::CollisionObject::REMOVE); + static_cast(moveit_msgs::CollisionObject::REMOVE); obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second; obj.object.id = p.get("object"); scene->processAttachedCollisionObjectMsg(obj); diff --git a/core/test/test_stage.cpp b/core/test/test_stage.cpp index 1a81b4387..298d8c3fb 100644 --- a/core/test/test_stage.cpp +++ b/core/test/test_stage.cpp @@ -119,7 +119,7 @@ void attachObject(PlanningScene& scene, const std::string& object, const std::st moveit_msgs::AttachedCollisionObject obj; obj.link_name = link; obj.object.operation = attach ? static_cast(moveit_msgs::CollisionObject::ADD) : - static_cast(moveit_msgs::CollisionObject::REMOVE); + static_cast(moveit_msgs::CollisionObject::REMOVE); obj.object.id = object; scene.processAttachedCollisionObjectMsg(obj); } diff --git a/visualization/CMakeLists.txt b/visualization/CMakeLists.txt index 9185ab3b7..d72aaebf7 100644 --- a/visualization/CMakeLists.txt +++ b/visualization/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_task_constructor_visualization) +find_package(fmt REQUIRED) find_package(catkin REQUIRED COMPONENTS moveit_core moveit_ros_visualization diff --git a/visualization/package.xml b/visualization/package.xml index 12c1304a6..3dfb14689 100644 --- a/visualization/package.xml +++ b/visualization/package.xml @@ -10,6 +10,7 @@ catkin + fmt qtbase5-dev moveit_core moveit_task_constructor_msgs diff --git a/visualization/visualization_tools/CMakeLists.txt b/visualization/visualization_tools/CMakeLists.txt index 58cb29f1a..491cfb776 100644 --- a/visualization/visualization_tools/CMakeLists.txt +++ b/visualization/visualization_tools/CMakeLists.txt @@ -25,6 +25,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} + fmt::fmt ) target_include_directories(${MOVEIT_LIB_NAME} PUBLIC include) target_include_directories(${MOVEIT_LIB_NAME} SYSTEM diff --git a/visualization/visualization_tools/src/display_solution.cpp b/visualization/visualization_tools/src/display_solution.cpp index bfb704c3b..498875d42 100644 --- a/visualization/visualization_tools/src/display_solution.cpp +++ b/visualization/visualization_tools/src/display_solution.cpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include namespace moveit_rviz_plugin { @@ -88,11 +88,10 @@ const MarkerVisualizationPtr DisplaySolution::markers(const DisplaySolution::Ind void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& start_scene, const moveit_task_constructor_msgs::Solution& msg) { - if (msg.start_scene.robot_model_name != start_scene->getRobotModel()->getName()) { - static boost::format fmt("Solution for model '%s' but model '%s' was expected"); - fmt % msg.start_scene.robot_model_name.c_str() % start_scene->getRobotModel()->getName().c_str(); - throw std::invalid_argument(fmt.str()); - } + if (msg.start_scene.robot_model_name != start_scene->getRobotModel()->getName()) + throw std::invalid_argument(fmt::format("Solution for model '{}' but model '{}' was expected", + msg.start_scene.robot_model_name, + start_scene->getRobotModel()->getName())); // initialize parent scene from solution's start scene start_scene->setPlanningSceneMsg(msg.start_scene);