change the calc of r

wangzheqie · Apr 23, 2021 · 5393ee3 · 5393ee3
1 parent cbcee14
commit 5393ee3
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 3 deletions.
diff --git 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
@@ -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);