simplify

fixposition · Feb 11, 2025 · b5b3d32 · b5b3d32
1 parent 00285b7
commit b5b3d32
Show file tree

Hide file tree

Showing 5 changed files with 25 additions and 52 deletions.
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp
@@ -29,6 +29,7 @@
 #include <fpsdk_common/time.hpp>
 
 /* PACKAGE */
+#include "fixposition_driver_lib/params.hpp"
 
 namespace fixposition {
 /* ****************************************************************************************************************** */
@@ -198,6 +199,17 @@ inline std::vector<WheelSpeedData> SpeedMsgToWheelspeedData(const RosMsgT& msg)
     return data;
 }
 
+template <typename RosMsgVector3T>
+std::vector<WheelSpeedData> Vector3MsgToWheelspeedData(const RosMsgVector3T& linear_msg, const DriverParams& params_) {
+    std::vector<WheelSpeedData> data;
+    data.push_back(
+        {"RC", params_.converter_use_x_,
+         static_cast<int32_t>(std::round(linear_msg.x * params_.converter_scale_factor_)), params_.converter_use_y_,
+         static_cast<int32_t>(std::round(linear_msg.y * params_.converter_scale_factor_)), params_.converter_use_z_,
+         static_cast<int32_t>(std::round(linear_msg.z * params_.converter_scale_factor_))});
+    return data;
+}
+
 struct NmeaEpochData {
     NmeaEpochData(const fpsdk::common::parser::fpa::FpaEpoch epoch);
 

diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp
@@ -75,28 +75,14 @@ void PublishOdometryData(const OdometryData& data, ros::Publisher& pub);
 void PublishJumpWarning(const JumpDetector& jump_detector, ros::Publisher& pub);
 void PublishFusionEpochData(const FusionEpochData& data, ros::Publisher& pub);
 
-inline std::tuple<double, double, double> getLinearComponents(const geometry_msgs::Twist& msg) {
-    return std::make_tuple(msg.linear.x, msg.linear.y, msg.linear.z);
-}
-
-inline std::tuple<double, double, double> getLinearComponents(const geometry_msgs::TwistWithCovariance& msg) {
-    return std::make_tuple(msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z);
-}
-
-inline std::tuple<double, double, double> getLinearComponents(const nav_msgs::Odometry& msg) {
-    return std::make_tuple(msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z);
-}
-
-template <typename T>
-std::vector<WheelSpeedData> SpeedConverterCallback(const T& msg, const DriverParams& params_) {
-    double x, y, z;
-    std::tie(x, y, z) = getLinearComponents(msg);
-
+template <typename Vector3T>
+std::vector<WheelSpeedData> SpeedConverterCallback(const Vector3T& linear_msg, const DriverParams& params_) {
     std::vector<WheelSpeedData> data;
-    data.push_back({"RC", params_.converter_use_x_,
-                    static_cast<int32_t>(std::round(x * params_.converter_scale_factor_)), params_.converter_use_y_,
-                    static_cast<int32_t>(std::round(y * params_.converter_scale_factor_)), params_.converter_use_z_,
-                    static_cast<int32_t>(std::round(z * params_.converter_scale_factor_))});
+    data.push_back(
+        {"RC", params_.converter_use_x_,
+         static_cast<int32_t>(std::round(linear_msg.x * params_.converter_scale_factor_)), params_.converter_use_y_,
+         static_cast<int32_t>(std::round(linear_msg.y * params_.converter_scale_factor_)), params_.converter_use_z_,
+         static_cast<int32_t>(std::round(linear_msg.z * params_.converter_scale_factor_))});
     return data;
 }
 

diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp
@@ -390,19 +390,19 @@ bool FixpositionDriverNode::StartNode() {
                 case DriverParams::VelTopicType::TWIST:
                     _SUB(ws_conv_sub_, geometry_msgs::Twist, params_.converter_input_topic_, 10,
                          [this](const geometry_msgs::TwistConstPtr& msg) {
-                             driver_.SendWheelspeedData(SpeedConverterCallback(*msg, params_));
+                             driver_.SendWheelspeedData(Vector3MsgToWheelspeedData(msg->linear, params_));
                          });
                     break;
                 case DriverParams::VelTopicType::TWISTWITHCOV:
                     _SUB(ws_conv_sub_, geometry_msgs::TwistWithCovariance, params_.converter_input_topic_, 10,
                          [this](const geometry_msgs::TwistWithCovarianceConstPtr& msg) {
-                             driver_.SendWheelspeedData(SpeedConverterCallback(*msg, params_));
+                             driver_.SendWheelspeedData(Vector3MsgToWheelspeedData(msg->twist.linear, params_));
                          });
                     break;
                 case DriverParams::VelTopicType::ODOMETRY:
                     _SUB(ws_conv_sub_, nav_msgs::Odometry, params_.converter_input_topic_, 10,
                          [this](const nav_msgs::OdometryConstPtr& msg) {
-                             driver_.SendWheelspeedData(SpeedConverterCallback(*msg, params_));
+                             driver_.SendWheelspeedData(Vector3MsgToWheelspeedData(msg->twist.twist.linear, params_));
                          });
                     break;
                 default:

diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp
@@ -102,31 +102,6 @@ void PublishOdometryData(const OdometryData& data, rclcpp::Publisher<nav_msgs::m
 void PublishJumpWarning(const JumpDetector& jump_detector, rclcpp::Publisher<fpmsgs::CovWarn>::SharedPtr& pub);
 void PublishFusionEpochData(const FusionEpochData& data, rclcpp::Publisher<fpmsgs::FusionEpoch>::SharedPtr& pub);
 
-inline std::tuple<double, double, double> getLinearComponents(const geometry_msgs::msg::Twist& msg) {
-    return std::make_tuple(msg.linear.x, msg.linear.y, msg.linear.z);
-}
-
-inline std::tuple<double, double, double> getLinearComponents(const geometry_msgs::msg::TwistWithCovariance& msg) {
-    return std::make_tuple(msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z);
-}
-
-inline std::tuple<double, double, double> getLinearComponents(const nav_msgs::msg::Odometry& msg) {
-    return std::make_tuple(msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z);
-}
-
-template <typename T>
-std::vector<WheelSpeedData> SpeedConverterCallback(const T& msg, const DriverParams& params_) {
-    double x, y, z;
-    std::tie(x, y, z) = getLinearComponents(msg);
-
-    std::vector<WheelSpeedData> data;
-    data.push_back({"RC", params_.converter_use_x_,
-                    static_cast<int32_t>(std::round(x * params_.converter_scale_factor_)), params_.converter_use_y_,
-                    static_cast<int32_t>(std::round(y * params_.converter_scale_factor_)), params_.converter_use_z_,
-                    static_cast<int32_t>(std::round(z * params_.converter_scale_factor_))});
-    return data;
-}
-
 /* ****************************************************************************************************************** */
 }  // namespace fixposition
 #endif  // __FIXPOSITION_DRIVER_ROS2_DATA_TO_ROS2_HPP__
diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp
@@ -416,19 +416,19 @@ bool FixpositionDriverNode::StartNode() {
                 case DriverParams::VelTopicType::TWIST:
                     _SUB(ws_conv_twist_sub_, geometry_msgs::msg::Twist, params_.converter_input_topic_, 10,
                          [this](const geometry_msgs::msg::Twist& msg) {
-                             driver_.SendWheelspeedData(SpeedConverterCallback(msg, params_));
+                             driver_.SendWheelspeedData(Vector3MsgToWheelspeedData(msg.linear, params_));
                          });
                     break;
                 case DriverParams::VelTopicType::TWISTWITHCOV:
                     _SUB(ws_conv_twistcov_sub_, geometry_msgs::msg::TwistWithCovariance, params_.converter_input_topic_,
                          10, [this](const geometry_msgs::msg::TwistWithCovariance& msg) {
-                             driver_.SendWheelspeedData(SpeedConverterCallback(msg, params_));
+                             driver_.SendWheelspeedData(Vector3MsgToWheelspeedData(msg.twist.linear, params_));
                          });
                     break;
                 case DriverParams::VelTopicType::ODOMETRY:
                     _SUB(ws_conv_odom_sub_, nav_msgs::msg::Odometry, params_.converter_input_topic_, 10,
                          [this](const nav_msgs::msg::Odometry& msg) {
-                             driver_.SendWheelspeedData(SpeedConverterCallback(msg, params_));
+                             driver_.SendWheelspeedData(Vector3MsgToWheelspeedData(msg.twist.twist.linear, params_));
                          });
                     break;
                 default: