From 774afa08f30437f2cf81186c836f94c5682c7315 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 6 Aug 2026 00:48:07 +0800 Subject: [PATCH 1/2] fix for building on resolute Signed-off-by: Aaron Chong --- core/src/cost_terms.cpp | 16 +++++++--------- core/src/stage.cpp | 5 +++-- 2 files changed, 10 insertions(+), 11 deletions(-) diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 9ddb94ff2..269d13426 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -1,4 +1,4 @@ -/********************************************************************* +/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2020, Hamburg University @@ -253,8 +253,6 @@ Clearance::Clearance(bool with_world, bool cumulative, std::string group_propert , distance_to_cost{ [](double d) { return 1.0 / (d + 1e-5); } } {} double Clearance::operator()(const SubTrajectory& s, std::string& comment) const { - static const std::string PREFIX{ "Clearance: " }; - collision_detection::DistanceRequest request; request.type = cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL; @@ -274,7 +272,7 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const request.acm = &state->scene()->getAllowedCollisionMatrix(); // compute relevant distance data for state & robot - auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) { + auto check_distance{ [=, this](const InterfaceState* state, const moveit::core::RobotState& robot) { collision_detection::DistanceResult result; if (with_world) state->scene()->getCollisionEnv()->distanceRobot(request, result, robot); @@ -297,8 +295,8 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const return result.minimum_distance; } }; - auto collision_comment = [=](const auto& distance) { - return fmt::format(PREFIX + "allegedly valid solution collides between '{}' and '{}'", distance.link_names[0], + auto collision_comment = [](const auto& distance) { + return fmt::format("Clearance: allegedly valid solution collides between '{}' and '{}'", distance.link_names[0], distance.link_names[1]); }; @@ -313,10 +311,10 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const } distance = distance_data.distance; if (!cumulative) - comment = fmt::format(PREFIX + "distance {} between '{}' and '{}'", distance, distance_data.link_names[0], + comment = fmt::format("Clearance: distance {} between '{}' and '{}'", distance, distance_data.link_names[0], distance_data.link_names[1]); else - comment = fmt::format(PREFIX + "cumulative distance {}", distance); + comment = fmt::format("Clearance: 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)); @@ -327,7 +325,7 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const distance += distance_data.distance; } distance /= s.trajectory()->getWayPointCount(); - comment = fmt::format(PREFIX + "average{} distance: {}", (cumulative ? " cumulative" : ""), distance); + comment = fmt::format("Clearance: average{} distance: {}", (cumulative ? " cumulative" : ""), distance); } return distance_to_cost(distance); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index b64ce0ab3..6c8ec5c3e 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -909,8 +909,9 @@ 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(); - auto false_with_debug = [](auto... args) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Connecting"), fmt::format(args...)); + auto false_with_debug = [](const char* format_str, auto&&... args) { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Connecting"), + fmt::format(fmt::runtime(format_str), std::forward(args)...)); return false; }; From ef08ecc6c125374a363bb1ab34bb0c0520be22b0 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Thu, 6 Aug 2026 11:31:08 +0800 Subject: [PATCH 2/2] use string_view instead, pass as argument to fmt since v10 is now consteval Signed-off-by: Aaron Chong --- core/src/cost_terms.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 269d13426..8cf628537 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -253,6 +253,8 @@ Clearance::Clearance(bool with_world, bool cumulative, std::string group_propert , distance_to_cost{ [](double d) { return 1.0 / (d + 1e-5); } } {} double Clearance::operator()(const SubTrajectory& s, std::string& comment) const { + constexpr std::string_view PREFIX{ "Clearance: " }; + collision_detection::DistanceRequest request; request.type = cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL; @@ -272,7 +274,7 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const request.acm = &state->scene()->getAllowedCollisionMatrix(); // compute relevant distance data for state & robot - auto check_distance{ [=, this](const InterfaceState* state, const moveit::core::RobotState& robot) { + auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) { collision_detection::DistanceResult result; if (with_world) state->scene()->getCollisionEnv()->distanceRobot(request, result, robot); @@ -295,9 +297,9 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const return result.minimum_distance; } }; - auto collision_comment = [](const auto& distance) { - return fmt::format("Clearance: allegedly valid solution collides between '{}' and '{}'", distance.link_names[0], - distance.link_names[1]); + auto collision_comment = [=](const auto& distance) { + return fmt::format("{}allegedly valid solution collides between '{}' and '{}'", PREFIX, + distance.link_names[0], distance.link_names[1]); }; double distance{ 0.0 }; @@ -311,10 +313,10 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const } distance = distance_data.distance; if (!cumulative) - comment = fmt::format("Clearance: distance {} between '{}' and '{}'", distance, distance_data.link_names[0], - distance_data.link_names[1]); + comment = fmt::format("{}distance {} between '{}' and '{}'", PREFIX, distance, + distance_data.link_names[0], distance_data.link_names[1]); else - comment = fmt::format("Clearance: cumulative distance {}", distance); + comment = fmt::format("{}cumulative distance {}", PREFIX, distance); } else { // check trajectory for (size_t i = 0; i < s.trajectory()->getWayPointCount(); ++i) { auto distance_data = check_distance(state, s.trajectory()->getWayPoint(i)); @@ -325,7 +327,8 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const distance += distance_data.distance; } distance /= s.trajectory()->getWayPointCount(); - comment = fmt::format("Clearance: average{} distance: {}", (cumulative ? " cumulative" : ""), distance); + comment = fmt::format("{}average{} distance: {}", PREFIX, (cumulative ? " cumulative" : ""), + distance); } return distance_to_cost(distance);