diff --git a/cpp_test/test_ros_conversions.cpp b/cpp_test/test_ros_conversions.cpp index e8aa097..69c7714 100644 --- a/cpp_test/test_ros_conversions.cpp +++ b/cpp_test/test_ros_conversions.cpp @@ -109,22 +109,6 @@ TEST(to_pose_msgs, vector_of_euler) { ASSERT_EQ(out.size(), 3); } -template -concept AcceptsRosToPoseVec = requires(const T& t) { - vortex::utils::ros_conversions::ros_to_pose_vec(t); -}; - -static_assert(AcceptsRosToPoseVec); -static_assert(AcceptsRosToPoseVec); -static_assert( - AcceptsRosToPoseVec); -static_assert(AcceptsRosToPoseVec); - -static_assert(!AcceptsRosToPoseVec); -static_assert(!AcceptsRosToPoseVec); -static_assert(!AcceptsRosToPoseVec); -static_assert(!AcceptsRosToPoseVec); - TEST(ros_to_pose_vec, pose) { geometry_msgs::msg::Pose p; p.position.x = 1; diff --git a/include/vortex/utils/ros_conversions.hpp b/include/vortex/utils/ros_conversions.hpp index be58750..3dc7a31 100644 --- a/include/vortex/utils/ros_conversions.hpp +++ b/include/vortex/utils/ros_conversions.hpp @@ -20,42 +20,6 @@ namespace vortex::utils::ros_conversions { -/** - * @brief Helper concept to check if two types are the same - * after removing cv-ref qualifiers. - * - * @tparam T First type. - * @tparam U Second type. - */ -template -concept same_bare_as = std::same_as, U>; - -/** - * @brief Concept describing ROS pose message types supported by - * ros_to_eigen6d(). - * - * Supported types are: - * - * - `geometry_msgs::msg::Pose` - * - * - `geometry_msgs::msg::PoseStamped` - * - * - `geometry_msgs::msg::PoseWithCovariance` - * - * - `geometry_msgs::msg::PoseWithCovarianceStamped` - * - * - `geometry_msgs::msg::PoseArray` - * - * @tparam T The candidate type to test. - */ -template -concept ROSPoseLike = - same_bare_as || - same_bare_as || - same_bare_as || - same_bare_as || - same_bare_as; - /** * @brief Converts a PoseLike object to a ROS geometry_msgs::msg::Pose. * @@ -136,49 +100,63 @@ inline vortex::utils::types::Pose ros_pose_to_pose( } /** - * @brief Extracts one or more internal Pose objects from a ROS pose message. - * - * Supported input types are: - * - * - `geometry_msgs::msg::Pose` - * - * - `geometry_msgs::msg::PoseStamped` - * - * - `geometry_msgs::msg::PoseWithCovariance` - * - * - `geometry_msgs::msg::PoseWithCovarianceStamped` - * - * - `geometry_msgs::msg::PoseArray` - * - * Messages containing a single pose produce a vector with one element. - * PoseArray messages produce a vector with one element per pose. - * - * @tparam T ROS message type satisfying ROSPoseLike - * @param msg ROS pose message - * @return std::vector Extracted internal poses + * @brief Converts a ROS geometry_msgs::msg::Pose to an internal Pose vector + * type. + * @param pose geometry_msgs::msg::Pose + * @return std::vector Internal pose representation + */ +inline std::vector ros_to_pose_vec( + const geometry_msgs::msg::Pose& msg) { + return {ros_pose_to_pose(msg)}; +} + +/** + * @brief Converts a ROS geometry_msgs::msg::PoseStamped to an internal Pose + * vector type. + * @param pose geometry_msgs::msg::PoseStamped + * @return std::vector Internal pose representation + */ +inline std::vector ros_to_pose_vec( + const geometry_msgs::msg::PoseStamped& msg) { + return {ros_pose_to_pose(msg.pose)}; +} + +/** + * @brief Converts a ROS geometry_msgs::msg::PoseWithCovariance to an internal + * Pose vector type. + * @param pose geometry_msgs::msg::PoseWithCovariance + * @return std::vector Internal pose representation + */ +inline std::vector ros_to_pose_vec( + const geometry_msgs::msg::PoseWithCovariance& msg) { + return {ros_pose_to_pose(msg.pose)}; +} + +/** + * @brief Converts a ROS geometry_msgs::msg::PoseWithCovarianceStamped to an + * internal Pose vector type. + * @param pose geometry_msgs::msg::PoseWithCovarianceStamped + * @return std::vector Internal pose representation + */ +inline std::vector ros_to_pose_vec( + const geometry_msgs::msg::PoseWithCovarianceStamped& msg) { + return {ros_pose_to_pose(msg.pose.pose)}; +} + +/** + * @brief Converts a ROS geometry_msgs::msg::PoseArray to an internal Pose + * vector type. + * @param pose geometry_msgs::msg::PoseArray + * @return std::vector Internal pose representation */ -template -std::vector ros_to_pose_vec(const T& msg) { - if constexpr (same_bare_as) { - return {ros_pose_to_pose(msg)}; - } else if constexpr (same_bare_as) { - return {ros_pose_to_pose(msg.pose)}; - } else if constexpr (same_bare_as) { - return {ros_pose_to_pose(msg.pose)}; - } else if constexpr (same_bare_as< - T, - geometry_msgs::msg::PoseWithCovarianceStamped>) { - return {ros_pose_to_pose(msg.pose.pose)}; - } else if constexpr (same_bare_as) { - std::vector poses; - poses.reserve(msg.poses.size()); - - for (const auto& pose : msg.poses) { - poses.push_back(ros_pose_to_pose(pose)); - } - return poses; +inline std::vector ros_to_pose_vec( + const geometry_msgs::msg::PoseArray& msg) { + std::vector poses; + poses.reserve(msg.poses.size()); + for (const auto& pose : msg.poses) { + poses.push_back(ros_pose_to_pose(pose)); } + return poses; } } // namespace vortex::utils::ros_conversions