Merge branch 'main' into sloretz__cache_ik_link_issues

moveit · Aug 14, 2023 · 9ce1be6 · 9ce1be6
2 parents 449e7fe + 01ccced
commit 9ce1be6
Show file tree

Hide file tree

Showing 19 changed files with 41 additions and 22 deletions.
diff --git a/.github/mergify.yml b/.github/mergify.yml
@@ -26,6 +26,15 @@ pull_request_rules:
         branches:
           - humble
 
+  - name: backport to iron at reviewers discretion
+    conditions:
+      - base=main
+      - "label=backport-iron"
+    actions:
+      backport:
+        branches:
+          - iron
+
   - name: ask to resolve conflict
     conditions:
       - conflict

diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt
@@ -9,6 +9,7 @@ moveit_package()
 include(ConfigExtras.cmake)
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
+  fmt
   ament_cmake
   moveit_core
   moveit_ros_occupancy_map_monitor
@@ -64,8 +65,8 @@ ament_target_dependencies(move_group  ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
 target_link_libraries(move_group moveit_move_group_capabilities_base)
 
 add_executable(list_move_group_capabilities src/list_capabilities.cpp)
-ament_target_dependencies(list_move_group_capabilities ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
-target_link_libraries(list_move_group_capabilities moveit_move_group_capabilities_base)
+ament_target_dependencies(list_move_group_capabilities ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_link_libraries(list_move_group_capabilities moveit_move_group_capabilities_base fmt)
 
 install(
   TARGETS

diff --git a/moveit_ros/move_group/package.xml b/moveit_ros/move_group/package.xml
@@ -31,6 +31,7 @@
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>fmt</depend>
 
   <exec_depend>moveit_kinematics</exec_depend>
 

diff --git a/moveit_ros/move_group/src/list_capabilities.cpp b/moveit_ros/move_group/src/list_capabilities.cpp
@@ -36,7 +36,7 @@
 
 #include <moveit/move_group/move_group_capability.h>
 #include <pluginlib/class_loader.hpp>
-#include <boost/algorithm/string/join.hpp>
+#include <fmt/format.h>
 
 int main(int /*argc*/, char** /*argv*/)
 {
@@ -45,7 +45,7 @@ int main(int /*argc*/, char** /*argv*/)
     pluginlib::ClassLoader<move_group::MoveGroupCapability> capability_plugin_loader("moveit_ros_move_group",
                                                                                      "move_group::MoveGroupCapability");
     std::cout << "Available capabilities:\n"
-              << boost::algorithm::join(capability_plugin_loader.getDeclaredClasses(), "\n") << '\n';
+              << fmt::format("{}", fmt::join(capability_plugin_loader.getDeclaredClasses(), "\n")) << '\n';
   }
   catch (pluginlib::PluginlibException& ex)
   {

diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt
@@ -6,6 +6,7 @@ find_package(moveit_common REQUIRED)
 moveit_package()
 
 find_package(ament_cmake REQUIRED)
+find_package(fmt REQUIRED)
 find_package(generate_parameter_library REQUIRED)
 find_package(moveit_msgs REQUIRED)
 # find_package(moveit_ros_perception REQUIRED)

diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml
@@ -41,6 +41,7 @@
   <depend>tf2_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>eigen</depend>
+  <depend>fmt</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
@@ -39,7 +39,7 @@
 #include <moveit/collision_detection/collision_tools.h>
 #include <moveit/trajectory_processing/trajectory_tools.h>
 #include <boost/tokenizer.hpp>
-#include <boost/algorithm/string/join.hpp>
+#include <fmt/format.h>
 #include <sstream>
 
 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning.planning_pipeline");
@@ -124,7 +124,7 @@ void planning_pipeline::PlanningPipeline::configure()
 
   if (planner_plugin_name_.empty())
   {
-    std::string classes_str = boost::algorithm::join(classes, ", ");
+    std::string classes_str = fmt::format("{}", fmt::join(classes, ", "));
     throw std::runtime_error("Planning plugin name is empty. Please choose one of the available plugins: " +
                              classes_str);
   }
@@ -140,7 +140,7 @@ void planning_pipeline::PlanningPipeline::configure()
   }
   catch (pluginlib::PluginlibException& ex)
   {
-    std::string classes_str = boost::algorithm::join(classes, ", ");
+    std::string classes_str = fmt::format("{}", fmt::join(classes, ", "));
     RCLCPP_FATAL(LOGGER,
                  "Exception while loading planner '%s': %s"
                  "Available plugins: %s",

diff --git a/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt b/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt
@@ -15,7 +15,7 @@ ament_target_dependencies(moveit_planning_scene_monitor
   urdf
   pluginlib
   rclcpp
-  Boost
+  fmt
   moveit_msgs
 )
 target_link_libraries(moveit_planning_scene_monitor
@@ -27,7 +27,7 @@ add_executable(demo_scene demos/demo_scene.cpp)
 ament_target_dependencies(demo_scene
   PUBLIC
   rclcpp
-  Boost
+  fmt
   moveit_msgs
   urdf
   message_filters

diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
@@ -45,7 +45,7 @@
 #include <tf2_eigen/tf2_eigen.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
-#include <boost/algorithm/string/join.hpp>
+#include <fmt/format.h>
 #include <memory>
 
 #include <std_msgs/msg/string.hpp>
@@ -1454,7 +1454,7 @@ void PlanningSceneMonitor::updateSceneWithCurrentState()
     if (!current_state_monitor_->haveCompleteState(missing) &&
         (time - current_state_monitor_->getMonitorStartTime()).seconds() > 1.0)
     {
-      std::string missing_str = boost::algorithm::join(missing, ", ");
+      std::string missing_str = fmt::format("{}", fmt::join(missing, ", "));
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wold-style-cast"
       RCLCPP_WARN_THROTTLE(LOGGER, steady_clock, 1000, "The complete state of the robot is not yet known.  Missing %s",

diff --git a/moveit_ros/warehouse/CMakeLists.txt b/moveit_ros/warehouse/CMakeLists.txt
@@ -6,6 +6,7 @@ find_package(moveit_common REQUIRED)
 moveit_package()
 
 find_package(ament_cmake REQUIRED)
+find_package(fmt REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(moveit_core REQUIRED)
 find_package(warehouse_ros REQUIRED)
@@ -19,6 +20,7 @@ include(ConfigExtras.cmake)
 include_directories(include)
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
+  fmt
   Boost
   moveit_core
   rclcpp
@@ -52,7 +54,7 @@ target_link_libraries(moveit_warehouse_broadcast moveit_warehouse)
 
 add_executable(moveit_save_to_warehouse src/save_to_warehouse.cpp)
 ament_target_dependencies(moveit_save_to_warehouse ${THIS_PACKAGE_INCLUDE_DEPENDS})
-target_link_libraries(moveit_save_to_warehouse moveit_warehouse)
+target_link_libraries(moveit_save_to_warehouse moveit_warehouse fmt)
 
 add_executable(moveit_warehouse_import_from_text src/import_from_text.cpp)
 ament_target_dependencies(moveit_warehouse_import_from_text ${THIS_PACKAGE_INCLUDE_DEPENDS})

diff --git a/moveit_ros/warehouse/package.xml b/moveit_ros/warehouse/package.xml
@@ -25,6 +25,7 @@
   <depend>moveit_ros_planning</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_ros</depend>
+  <depend>fmt</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

diff --git a/moveit_ros/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/src/save_to_warehouse.cpp
@@ -38,11 +38,11 @@
 #include <moveit/warehouse/constraints_storage.h>
 #include <moveit/warehouse/state_storage.h>
 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
-#include <boost/algorithm/string/join.hpp>
 #include <boost/program_options/cmdline.hpp>
 #include <boost/program_options/options_description.hpp>
 #include <boost/program_options/parsers.hpp>
 #include <boost/program_options/variables_map.hpp>
+#include <fmt/format.h>
 #include <tf2_ros/transform_listener.h>
 #include <rclcpp/executors.hpp>
 #include <rclcpp/experimental/buffers/intra_process_buffer.hpp>
@@ -221,7 +221,7 @@ int main(int argc, char** argv)
 
   std::vector<std::string> topics;
   psm.getMonitoredTopics(topics);
-  RCLCPP_INFO_STREAM(LOGGER, "Listening for scene updates on topics " << boost::algorithm::join(topics, ", "));
+  RCLCPP_INFO_STREAM(LOGGER, "Listening for scene updates on topics " << fmt::format("{}", fmt::join(topics, ", ")));
   RCLCPP_INFO_STREAM(LOGGER, "Listening for planning requests on topic " << mplan_req_sub->get_topic_name());
   RCLCPP_INFO_STREAM(LOGGER, "Listening for named constraints on topic " << constr_sub->get_topic_name());
   RCLCPP_INFO_STREAM(LOGGER, "Listening for states on topic " << state_sub->get_topic_name());

diff --git a/...tup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launch_bundle.hpp b/...tup_assistant/moveit_setup_app_plugins/include/moveit_setup_app_plugins/launch_bundle.hpp
@@ -139,9 +139,9 @@ class LaunchBundle
     bool write() override
     {
       // Add function name as a TemplateVariable, then remove it
-      variables_.push_back(TemplateVariable("FUNCTION_NAME", function_name_));
+      variables.push_back(TemplateVariable("FUNCTION_NAME", function_name_));
       bool ret = TemplatedGeneratedFile::write();
-      variables_.pop_back();
+      variables.pop_back();
       return ret;
     }
 

diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/src/configuration_files.cpp b/moveit_setup_assistant/moveit_setup_core_plugins/src/configuration_files.cpp
@@ -47,7 +47,7 @@ void ConfigurationFiles::onInit()
 
 void ConfigurationFiles::loadTemplateVariables()
 {
-  auto& variables = TemplatedGeneratedFile::variables_;
+  auto& variables = TemplatedGeneratedFile::variables;
   variables.clear();
   for (const auto& config : config_data_->getConfigured())
   {

diff --git a/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt
@@ -8,6 +8,7 @@ moveit_package()
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_ros REQUIRED)
 find_package(ament_index_cpp REQUIRED)
+find_package(fmt REQUIRED)
 find_package(moveit_core REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_ros_visualization REQUIRED)
@@ -83,6 +84,7 @@ install(TARGETS moveit_setup_framework
         INCLUDES DESTINATION include/moveit_setup_framework
 )
 
+ament_export_dependencies(fmt)
 ament_export_dependencies(rclcpp)
 ament_export_dependencies(Qt5Core)
 ament_export_dependencies(Qt5Widgets)

diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/templates.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/templates.hpp
@@ -69,7 +69,7 @@ class TemplatedGeneratedFile : public GeneratedFile
 
   bool write() override;
 
-  static std::vector<TemplateVariable> variables_;
+  static std::vector<TemplateVariable> variables;
 };
 
 }  // namespace moveit_setup
diff --git a/moveit_setup_assistant/moveit_setup_framework/package.xml b/moveit_setup_assistant/moveit_setup_framework/package.xml
@@ -20,6 +20,7 @@
   <depend>rviz_rendering</depend>
   <depend>srdfdom</depend>
   <depend>urdf</depend>
+  <depend>fmt</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_clang_format</test_depend>

diff --git a/moveit_setup_assistant/moveit_setup_framework/src/templates.cpp b/moveit_setup_assistant/moveit_setup_framework/src/templates.cpp
@@ -40,7 +40,7 @@
 
 namespace moveit_setup
 {
-std::vector<TemplateVariable> TemplatedGeneratedFile::variables_;
+std::vector<TemplateVariable> TemplatedGeneratedFile::variables;
 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_setup.templates");
 
 bool TemplatedGeneratedFile::write()
@@ -71,7 +71,7 @@ bool TemplatedGeneratedFile::write()
   template_stream.close();
 
   // Replace keywords in string ------------------------------------------------------------
-  for (const auto& variable : variables_)
+  for (const auto& variable : variables)
   {
     std::string key_with_brackets = "[" + variable.key + "]";
     boost::replace_all(template_string, key_with_brackets, variable.value);

diff --git a/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp b/moveit_setup_assistant/moveit_setup_framework/src/urdf_config.cpp
@@ -35,7 +35,7 @@
 #include <moveit_setup_framework/data/urdf_config.hpp>
 #include <moveit_setup_framework/utilities.hpp>
 #include <moveit/rdf_loader/rdf_loader.h>
-#include <boost/algorithm/string/join.hpp>
+#include <fmt/format.h>
 
 namespace moveit_setup
 {
@@ -85,7 +85,7 @@ void URDFConfig::loadFromPath(const std::filesystem::path& urdf_file_path, const
 {
   urdf_path_ = urdf_file_path;
   xacro_args_vec_ = xacro_args;
-  xacro_args_ = boost::algorithm::join(xacro_args_vec_, " ");
+  xacro_args_ = fmt::format("{}", fmt::join(xacro_args_vec_, " "));
   setPackageName();
   load();
 }