Fix core warnings

moveit_core/CMakeLists.txt

@@ -7,8 +7,10 @@ if(NOT CMAKE_CXX_STANDARD)
 endif()
 
 # Warnings
+# Remove -Wcast-qual to avoid warnings in tf2/LinearMath/Vector3.h.
+# TODO: Add -Wcast-qual back when PR(https://github.com/ros2/geometry2/pull/193) is merged.
 add_compile_options(-Wall -Wextra
-  -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual
+  -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls
   -Wno-unused-parameter -Wno-unused-function)
 if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
   # This too often has false-positives

moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp

@@ -1,5 +1,5 @@
 #include <moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 namespace collision_detection
 {

moveit_core/collision_distance_field/src/collision_robot_distance_field.cpp

@@ -299,7 +299,7 @@ bool CollisionRobotDistanceField::getSelfCollisions(const collision_detection::C
           }
 
           RCLCPP_DEBUG(LOGGER_COLLISION_ROBOT_DISTANCE_FIELD,
-                       ("Self collision detected for link %s ", con.body_name_1).c_str());
+                       "Self collision detected for link %s ", con.body_name_1.c_str());
 
           con.body_type_2 = collision_detection::BodyTypes::ROBOT_LINK;
           con.body_name_2 = "self";
@@ -890,7 +890,7 @@ DistanceFieldCacheEntryPtr CollisionRobotDistanceField::generateDistanceFieldCac
     {
       dfce->state_check_indices_.push_back(dfce->state_values_.size() - 1);
       RCLCPP_DEBUG(LOGGER_COLLISION_ROBOT_DISTANCE_FIELD,
-                   ("Non-group joint %p will be checked for state changes", *name_iter).c_str());
+                   "Non-group joint %p will be checked for state changes", (*name_iter).c_str());
     }
   }
 
@@ -1058,7 +1058,7 @@ void CollisionRobotDistanceField::addLinkBodyDecompositions(
     {
       RCLCPP_WARN(
           LOGGER_COLLISION_ROBOT_DISTANCE_FIELD,
-          ("Skipping model generation for link %s since it contains no geometries", link_models[i]->getName()).c_str());
+          "Skipping model generation for link %s since it contains no geometries", link_models[i]->getName().c_str());
       continue;
     }
 

moveit_core/collision_distance_field/src/collision_world_distance_field.cpp

@@ -551,7 +551,7 @@ void CollisionWorldDistanceField::updateDistanceObject(const std::string& id, Di
   else
   {
     RCLCPP_DEBUG(LOGGER_COLLISION_WORLD_DISTANCE_FIELD,
-                 ("Removing Object '%s' from CollisionWorldDistanceField", id).c_str());
+                 "Removing Object '%s' from CollisionWorldDistanceField", id.c_str());
     dfce->posed_body_point_decompositions_.erase(id);
   }
 }

moveit_core/distance_field/test/test_distance_field.cpp

@@ -819,7 +819,7 @@ TEST(TestSignedPropagationDistanceField, TestPerformance)
 
   dt = std::chrono::system_clock::now();
   worstdfu.addPointsToField(bad_vec);
-  auto wd = std::chrono::system_clock::now() - dt;
+  std::chrono::duration<double> wd = std::chrono::system_clock::now() - dt;
   printf("Time for unsigned adding %u uniform points is %g average %g\n", (unsigned int)bad_vec.size(), wd.count(),
          wd.count() / (bad_vec.size() * 1.0));
   dt = std::chrono::system_clock::now();

moveit_core/kinematics_base/CMakeLists.txt

@@ -4,10 +4,6 @@ set(MOVEIT_LIB_NAME moveit_kinematics_base)
 add_library(${MOVEIT_LIB_NAME} SHARED src/kinematics_base.cpp)
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
-# Avoid warnings about deprecated members of KinematicsBase when building KinematicsBase itself.
-# TODO: remove when deperecated variables (tip_frame_, search_discretization_) are finally removed.
-# target_compile_options(${MOVEIT_LIB_NAME} PRIVATE -Wno-deprecated-declarations)
-
 # This line is needed to ensure that messages are done being built before this is built
 ament_target_dependencies(${MOVEIT_LIB_NAME} urdf urdfdom_headers)
 

moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h

@@ -328,21 +328,6 @@ class KinematicsBase
   virtual bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
                              std::vector<geometry_msgs::msg::Pose>& poses) const = 0;
 
-  /**
-   * @brief Set the parameters for the solver, for use with kinematic chain IK solvers
-   * @param robot_description This parameter can be used as an identifier for the robot kinematics it is computed for;
-   * For example, the name of the ROS parameter that contains the robot description;
-   * @param group_name The group for which this solver is being configured
-   * @param base_frame The base frame in which all input poses are expected.
-   * This may (or may not) be the root frame of the chain that the solver operates on
-   * @param tip_frame The tip of the chain
-   * @param search_discretization The discretization of the search when the solver steps through the redundancy
-   */
-  /* Replace by tip_frames-based method! */
-  MOVEIT_DEPRECATED virtual void setValues(const std::string& robot_description, const std::string& group_name,
-                                           const std::string& base_frame, const std::string& tip_frame,
-                                           double search_discretization);
-
   /**
    * @brief Set the parameters for the solver, for use with non-chain IK solvers
    * @param robot_description This parameter can be used as an identifier for the robot kinematics it is computed for;
@@ -357,24 +342,6 @@ class KinematicsBase
                          const std::string& base_frame, const std::vector<std::string>& tip_frames,
                          double search_discretization);
 
-  /**
-   * @brief  Initialization function for the kinematics, for use with kinematic chain IK solvers
-   * @param robot_description This parameter can be used as an identifier for the robot kinematics it is computed for;
-   * For example, the name of the ROS parameter that contains the robot description;
-   * @param group_name The group for which this solver is being configured
-   * @param base_frame The base frame in which all input poses are expected.
-   * This may (or may not) be the root frame of the chain that the solver operates on
-   * @param tip_frame The tip of the chain
-   * @param search_discretization The discretization of the search when the solver steps through the redundancy
-   * @return True if initialization was successful, false otherwise
-   *
-   * Instead of this method, use the method passing in a RobotModel!
-   * Default implementation returns false, indicating that this API is not supported.
-   */
-  MOVEIT_DEPRECATED virtual bool initialize(const std::string& robot_description, const std::string& group_name,
-                                            const std::string& base_frame, const std::string& tip_frame,
-                                            double search_discretization);
-
   /**
    * @brief  Initialization function for the kinematics, for use with non-chain IK solvers
    * @param robot_description This parameter can be used as an identifier for the robot kinematics is computed for;
@@ -589,12 +556,6 @@ class KinematicsBase
   std::string base_frame_;
   std::vector<std::string> tip_frames_;
 
-  // The next two variables still exists for backwards compatibility
-  // with previously generated custom ik solvers like IKFast
-  // Replace tip_frame_ -> tip_frames_[0], search_discretization_ -> redundant_joint_discretization_
-  MOVEIT_DEPRECATED std::string tip_frame_;
-  MOVEIT_DEPRECATED double search_discretization_;
-
   double default_timeout_;
   std::vector<unsigned int> redundant_joint_indices_;
   std::map<int, double> redundant_joint_discretization_;

moveit_core/kinematics_base/src/kinematics_base.cpp

@@ -62,7 +62,6 @@ void KinematicsBase::storeValues(const moveit::core::RobotModel& robot_model, co
   for (const std::string& name : tip_frames)
     tip_frames_.push_back(removeSlash(name));
   setSearchDiscretization(search_discretization);
-  search_discretization_ = search_discretization;
 }
 
 void KinematicsBase::setValues(const std::string& robot_description, const std::string& group_name,
@@ -77,50 +76,23 @@ void KinematicsBase::setValues(const std::string& robot_description, const std::
   for (const std::string& name : tip_frames)
     tip_frames_.push_back(removeSlash(name));
   setSearchDiscretization(search_discretization);
-
-  // store deprecated values for backwards compatibility
-  search_discretization_ = search_discretization;
-  if (tip_frames_.size() == 1)
-    tip_frame_ = tip_frames_[0];
-  else
-    tip_frame_.clear();
-}
-
-void KinematicsBase::setValues(const std::string& robot_description, const std::string& group_name,
-                               const std::string& base_frame, const std::string& tip_frame,
-                               double search_discretization)
-{
-  setValues(robot_description, group_name, base_frame, std::vector<std::string>({ tip_frame }), search_discretization);
-}
-
-bool KinematicsBase::initialize(const std::string& robot_description, const std::string& group_name,
-                                const std::string& base_frame, const std::string& tip_frame,
-                                double search_discretization)
-{
-  return false;  // default implementation returns false
 }
 
 bool KinematicsBase::initialize(const std::string& robot_description, const std::string& group_name,
                                 const std::string& base_frame, const std::vector<std::string>& tip_frames,
                                 double search_discretization)
 {
-  // For IK solvers that do not support multiple tip frames, fall back to single pose call
-  if (tip_frames.size() == 1)
-  {
-    return initialize(robot_description, group_name, base_frame, tip_frames[0], search_discretization);
-  }
-
-  RCLCPP_ERROR(LOGGER_KINEMATICS_BASE, "This solver does not support multiple tip frames");
+  RCLCPP_ERROR(LOGGER_KINEMATICS_BASE, "IK plugin for group '%s' relies on deprecated API.",
+               group_name.c_str());
   return false;
 }
 
 bool KinematicsBase::initialize(const moveit::core::RobotModel& robot_model, const std::string& group_name,
                                 const std::string& base_frame, const std::vector<std::string>& tip_frames,
                                 double search_discretization)
 {
-  RCLCPP_WARN(LOGGER_KINEMATICS_BASE, "IK plugin for group '%s' relies on deprecated API. "
-                                      "Please implement initialize(RobotModel, ...).",
-              group_name.c_str());
+  RCLCPP_ERROR(LOGGER_KINEMATICS_BASE, "IK plugin for group '%s' relies on deprecated API.",
+               group_name.c_str());
   return false;
 }
 
@@ -175,11 +147,7 @@ bool KinematicsBase::supportsGroup(const moveit::core::JointModelGroup* jmg, std
 }
 
 KinematicsBase::KinematicsBase()
-  : tip_frame_("DEPRECATED")
-  // help users understand why this variable might not be set
-  // (if multiple tip frames provided, this variable will be unset)
-  , search_discretization_(DEFAULT_SEARCH_DISCRETIZATION)
-  , default_timeout_(DEFAULT_TIMEOUT)
+  :default_timeout_(DEFAULT_TIMEOUT)
 {
   supported_methods_.push_back(DiscretizationMethods::NO_DISCRETIZATION);
 }

moveit_core/package.xml

@@ -56,6 +56,7 @@
   -->
 
   <export>
+    <build_type>ament_cmake</build_type>
     <moveit_core plugin="${prefix}/collision_detector_fcl_description.xml"/>
   </export>
 </package>

moveit_core/profiler/src/profiler.cpp

@@ -40,11 +40,15 @@
 #include <vector>
 #include <algorithm>
 #include <sstream>
+#include <rclcpp/rclcpp.hpp>
 
 namespace moveit
 {
 namespace tools
 {
+
+rclcpp::Logger LOGGER_MOVEIT_PROFILER = rclcpp::get_logger("moveit_profiler");
+
 Profiler& Profiler::instance()
 {
   static Profiler p(false, false);
@@ -174,7 +178,7 @@ void Profiler::console()
   std::stringstream ss;
   ss << std::endl;
   status(ss, true);
-  RCUTILS_LOG_INFO("profiler", ss.str().c_str());
+  RCLCPP_INFO(LOGGER_MOVEIT_PROFILER, ss.str().c_str());
 }
 
 /// @cond IGNORE