From 811414e5c230a7bf4d9b1696e26f93dbd44a10c4 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 26 Oct 2023 09:50:37 +0200 Subject: [PATCH] Simplify formatting code with https://github.com/fmtlib --- capabilities/CMakeLists.txt | 3 +- capabilities/package.xml | 1 + .../src/execute_task_solution_capability.cpp | 6 +- core/CMakeLists.txt | 1 + .../include/moveit/task_constructor/stage_p.h | 13 ++- core/package.xml | 1 + core/src/CMakeLists.txt | 2 +- core/src/container.cpp | 42 ++++--- core/src/cost_terms.cpp | 32 ++---- core/src/properties.cpp | 10 +- core/src/stage.cpp | 107 ++++++++---------- core/src/stages/compute_ik.cpp | 4 +- core/src/stages/connect.cpp | 8 +- core/src/stages/fix_collision_objects.cpp | 3 +- core/src/stages/modify_planning_scene.cpp | 6 +- visualization/CMakeLists.txt | 1 + visualization/package.xml | 1 + .../visualization_tools/CMakeLists.txt | 1 + .../src/display_solution.cpp | 11 +- 19 files changed, 122 insertions(+), 131 deletions(-) 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..6249b45e5 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -44,14 +44,19 @@ #include #include +#include #include #include // define pimpl() functions accessing correctly casted pimpl_ pointer -#define PIMPL_FUNCTIONS(Class) \ - const Class##Private* Class::pimpl() const { return static_cast(pimpl_); } \ - Class##Private* Class::pimpl() { return static_cast(pimpl_); } +#define PIMPL_FUNCTIONS(Class) \ + const Class##Private* Class::pimpl() const { \ + return static_cast(pimpl_); \ + } \ + Class##Private* Class::pimpl() { \ + return static_cast(pimpl_); \ + } namespace moveit { namespace task_constructor { @@ -144,7 +149,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..2d4ab6501 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 @@ -763,14 +761,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 { @@ -947,7 +943,7 @@ 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 } diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index fd467ff9b..d9996d2c8 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(); } @@ -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..aeae71ccd 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 { @@ -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..fb784340b 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); 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);