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);