From 5393ee387d29b0e42fe72d468a26de9c209b6e0c Mon Sep 17 00:00:00 2001
From: wangzheqie <wangzheqie@qq.com>
Date: Fri, 23 Apr 2021 16:35:14 +0800
Subject: [PATCH] change the calc of r

---
 ranger_base/include/ranger_base/ranger_messenger.hpp | 5 +++--
 ranger_base/src/ranger_messenger.cpp                 | 2 +-
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/ranger_base/include/ranger_base/ranger_messenger.hpp b/ranger_base/include/ranger_base/ranger_messenger.hpp
index 5fa6b63..187a252 100644
--- a/ranger_base/include/ranger_base/ranger_messenger.hpp
+++ b/ranger_base/include/ranger_base/ranger_messenger.hpp
@@ -11,6 +11,8 @@
 #define RANGER_MESSENGER_HPP
 
 #include <geometry_msgs/Twist.h>
+#include <ranger_msgs/RangerSetting.h>
+#include <ranger_msgs/RangerStatus.h>
 #include <ros/console.h>
 #include <ros/ros.h>
 #include <tf2_ros/transform_broadcaster.h>
@@ -19,8 +21,6 @@
 #include "ascent/Utility.h"
 #include "ranger_base/ranger_params.hpp"
 #include "ugv_sdk/ranger_base.hpp"
-#include <ranger_msgs/RangerStatus.h>
-#include <ranger_msgs/RangerSetting.h>
 
 using namespace ros;
 using namespace ros::master;
@@ -81,6 +81,7 @@ class RangerROSMessenger {
 
   static constexpr double l = RangerParams::wheelbase;
   static constexpr double w = RangerParams::track;
+  static constexpr double s = w / 2.0;                    // half of track
   static constexpr double steer_angle_tolerance = 0.005;  // ~+-0.287 degrees
 
   // speed variables
diff --git a/ranger_base/src/ranger_messenger.cpp b/ranger_base/src/ranger_messenger.cpp
index 8bf4b82..262658c 100644
--- a/ranger_base/src/ranger_messenger.cpp
+++ b/ranger_base/src/ranger_messenger.cpp
@@ -70,7 +70,7 @@ void RangerROSMessenger::PublishStateToROS() {
   switch (motion_mode_) {
     case RangerSetting::MOTION_MODE_ACKERMAN: {
       l_v = state.motion_state.linear_velocity;
-      double r = l / std::tan(phi_i) + w;
+      double r = s / std::tan(phi_i) + s;
       phi = ConvertInnerAngleToCentral(phi_i);
       a_v = state.motion_state.linear_velocity / r;
       x_v = l_v * std::cos(phi);