Simplify formatting code with https://github.com/fmtlib

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}

capabilities/package.xml

@@ -12,6 +12,7 @@
 
 	<buildtool_depend>catkin</buildtool_depend>
 
+	<depend>fmt</depend>
 	<depend>moveit_core</depend>
 	<depend>moveit_ros_move_group</depend>
 	<depend>actionlib</depend>

capabilities/src/execute_task_solution_capability.cpp

@@ -43,6 +43,7 @@
 #include <moveit/robot_state/conversions.h>
 #include <moveit/utils/message_checks.h>
 #include <moveit/utils/moveit_error_code.h>
+#include <fmt/format.h>
 
 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",

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

core/include/moveit/task_constructor/stage_p.h

@@ -44,14 +44,19 @@
 #include <moveit/task_constructor/cost_queue.h>
 
 #include <ros/console.h>
+#include <fmt/core.h>
 
 #include <ostream>
 #include <chrono>
 
 // define pimpl() functions accessing correctly casted pimpl_ pointer
-#define PIMPL_FUNCTIONS(Class)                                                                       \
-	const Class##Private* Class::pimpl() const { return static_cast<const Class##Private*>(pimpl_); } \
-	Class##Private* Class::pimpl() { return static_cast<Class##Private*>(pimpl_); }
+#define PIMPL_FUNCTIONS(Class)                           \
+	const Class##Private* Class::pimpl() const {          \
+		return static_cast<const Class##Private*>(pimpl_); \
+	}                                                     \
+	Class##Private* Class::pimpl() {                      \
+		return static_cast<Class##Private*>(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();

core/package.xml

@@ -19,6 +19,7 @@
 	<build_depend>roslint</build_depend>
 	<exec_depend>roscpp</exec_depend>
 
+	<depend>fmt</depend>
 	<depend>tf2_eigen</depend>
 	<depend>geometry_msgs</depend>
 	<depend>moveit_core</depend>

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 $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
 	PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>

core/src/container.cpp

@@ -46,7 +46,7 @@
 #include <iostream>
 #include <algorithm>
 #include <boost/range/adaptor/reversed.hpp>
-#include <boost/format.hpp>
+#include <fmt/core.h>
 #include <functional>
 
 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<END_IF_MASK>(flags1);
-		desc % stage2.name() % flowSymbol<START_IF_MASK>(flags2);
-		throw InitStageException(*me(), desc.str());
+		throw InitStageException(
+		    *me(), fmt::format("cannot connect end interface of '{}' ({}) to start interface of '{}' ({})",  //
+		                       stage1.name(), flowSymbol<END_IF_MASK>(flags1),  //
+		                       stage2.name(), flowSymbol<START_IF_MASK>(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<mask>(child_interface) % flowSymbol<mask>(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<mask>(child_interface), flowSymbol<mask>(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<START_IF_MASK>(child_interface) % flowSymbol<END_IF_MASK>(child_interface);
-		desc % flowSymbol<START_IF_MASK>(external) % flowSymbol<END_IF_MASK>(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<START_IF_MASK>(child_interface), flowSymbol<END_IF_MASK>(child_interface),
+		                                           flowSymbol<START_IF_MASK>(external), flowSymbol<END_IF_MASK>(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
 }
 

core/src/cost_terms.cpp

@@ -44,7 +44,7 @@
 
 #include <Eigen/Geometry>
 
-#include <boost/format.hpp>
+#include <fmt/core.h>
 #include <utility>
 
 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<double>::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<double>::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);

core/src/properties.cpp

@@ -37,7 +37,8 @@
 */
 
 #include <moveit/task_constructor/properties.h>
-#include <boost/format.hpp>
+#include <fmt/core.h>
+#include <fmt/ostream.h>
 #include <functional>
 #include <ros/console.h>
 
@@ -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