fix ranger angular velocity error

README.md

@@ -54,6 +54,15 @@ roslaunch ranger_bringup ranger_teleop_keyboard.launch
 
 ## Examples
 
+### 0. enable can control
+
+```shell
+  robot->Connect("can0");
+  robot->EnableCommandedMode();
+```
+
+
+
 ### 1. set motion mode
 
 see `ranger_ros/ranger_examples/src/change_the_mode.cpp` for more details

ranger_base/include/ranger_base/ranger_params.hpp

@@ -19,8 +19,9 @@ struct RangerParams {
   static constexpr double max_linear_speed = 1.5;      // in m/s
   static constexpr double max_angular_speed = 0.7853;  // in rad/s
   static constexpr double max_speed_cmd = 10.0;        // in rad/s
-  // max central angle: pi/2
-  static constexpr double max_steer_angle_central = 1.5708;
+
+  static constexpr double max_steer_angle_central = 0.53385; //~= 30.58 degree
+  static constexpr double max_steer_angle_slide = 0.69813; // 40 degree
 };
 }  // namespace westonrobot
 #endif  // RANGER_PARAMS_HPP

ranger_base/src/ranger_messenger.cpp

@@ -54,6 +54,9 @@ void RangerROSMessenger::PublishStateToROS() {
   status_msg.control_mode = state.system_state.control_mode;
   status_msg.error_code = state.system_state.error_code;
   status_msg.battery_voltage = state.system_state.battery_voltage;
+  status_msg.current_motion_mode = state.current_motion_mode.motion_mode;
+
+  motion_mode_ = status_msg.current_motion_mode;
 
   // linear_velocity, angular_velocity, central steering_angle
   double l_v = 0.0, a_v = 0.0, phi = 0.0;
@@ -62,17 +65,16 @@ void RangerROSMessenger::PublishStateToROS() {
 
   double phi_i = -state.motion_state.steering_angle / 180.0 * M_PI;
 
-  //  ROS_WARN("%f %f %f %f", state.motion_state.linear_velocity,
-  //           state.motion_state.angular_velocity,
-  //           state.motion_state.lateral_velocity,
-  //           state.motion_state.steering_angle);
-
   switch (motion_mode_) {
     case RangerSetting::MOTION_MODE_ACKERMAN: {
       l_v = state.motion_state.linear_velocity;
-      double r = s / std::tan(phi_i) + s;
+      double r = s / std::tan(std::fabs(phi_i)) + s;
       phi = ConvertInnerAngleToCentral(phi_i);
-      a_v = state.motion_state.linear_velocity / r;
+      if (phi > steer_angle_tolerance) {
+        a_v = state.motion_state.linear_velocity / r;
+      } else {
+        a_v = -state.motion_state.linear_velocity / r;
+      }
       x_v = l_v * std::cos(phi);
       if (l_v >= 0.0) {
         y_v = l_v * std::sin(phi);
@@ -143,13 +145,6 @@ void RangerROSMessenger::GetCurrentMotionCmdForSim(double &linear,
 void RangerROSMessenger::TwistCmdCallback(
     const geometry_msgs::Twist::ConstPtr &msg) {
   double steer_cmd = msg->angular.z;
-  // TODO: different mode have different limit
-  if (steer_cmd > RangerParams::max_steer_angle_central) {
-    steer_cmd = RangerParams::max_steer_angle_central;
-  }
-  if (steer_cmd < -RangerParams::max_steer_angle_central) {
-    steer_cmd = -RangerParams::max_steer_angle_central;
-  }
 
   if (simulated_robot_) {
     std::lock_guard<std::mutex> lg(twist_mutex_);
@@ -159,14 +154,27 @@ void RangerROSMessenger::TwistCmdCallback(
 
   switch (motion_mode_) {
     case RangerSetting::MOTION_MODE_ACKERMAN: {
+      if (steer_cmd > RangerParams::max_steer_angle_central) {
+        steer_cmd = RangerParams::max_steer_angle_central;
+      }
+      if (steer_cmd < -RangerParams::max_steer_angle_central) {
+        steer_cmd = -RangerParams::max_steer_angle_central;
+      }
+
       double phi_i = ConvertCentralAngleToInner(steer_cmd);
 
       double phi_degree = -(phi_i / M_PI * 180.0);
-      // std::cout << "set steering angle: " << phi_degree << std::endl;
       ranger_->SetMotionCommand(msg->linear.x, phi_degree);
       break;
     }
     case RangerSetting::MOTION_MODE_SLIDE: {
+      if (steer_cmd > RangerParams::max_steer_angle_slide) {
+        steer_cmd = RangerParams::max_steer_angle_slide;
+      }
+      if (steer_cmd < -RangerParams::max_steer_angle_slide) {
+        steer_cmd = -RangerParams::max_steer_angle_slide;
+      }
+
       double phi_degree = -(steer_cmd / M_PI * 180.0);
       ranger_->SetMotionCommand(msg->linear.x, phi_degree);
       break;

ranger_examples/src/change_the_mode.cpp

@@ -11,6 +11,7 @@
 #include <nav_msgs/Odometry.h>
 #include <ros/ros.h>
 
+#include <ranger_msgs/RangerSetting.h>
 #include "ranger_base/ranger_messenger.hpp"
 #include "ugv_sdk/ranger_base.hpp"
 
@@ -29,6 +30,12 @@ int main(int argc, char *argv[]) {
   robot->Connect("can0");
   robot->EnableCommandedMode();
 
+  // 0 Arckmann 1 Slide 2 round, 3 Sloping
+  robot->SetMotionMode(0);
+  //  robot->SetMotionMode(1);
+  //  robot->SetMotionMode(2);
+  //  robot->SetMotionMode(3);
+
   // publish robot state at 50Hz while listening to twist commands
   ros::Rate rate(50);
   while (ros::ok()) {

ranger_examples/src/control_the_car.cpp

@@ -17,8 +17,12 @@
 
 using namespace westonrobot;
 
-std::shared_ptr<RangerBase> robot;
+void Ackermann();
+void Slide();
+void Round();
+void Sloping();
 
+std::shared_ptr<RangerBase> robot;
 
 int main(int argc, char *argv[]) {
   // setup ROS node
@@ -31,8 +35,10 @@ int main(int argc, char *argv[]) {
   robot->Connect("can0");
   robot->EnableCommandedMode();
 
-
-
+  Ackermann();
+  // Slide();
+  // Round();
+  // Sloping();
 
   // publish robot state at 50Hz while listening to twist commands
   ros::Rate rate(50);
@@ -42,3 +48,7 @@ int main(int argc, char *argv[]) {
   }
   return 0;
 }
+void Ackermann(){}
+void Slide(){}
+void Round(){}
+void Sloping(){}

ranger_msgs/msg/RangerStatus.msg

@@ -16,6 +16,7 @@ float64 steering_angle
 float64 x_linear_vel
 float64 y_linear_vel
 float64 motion_radius
+uint8 current_motion_mode
 
 # base state
 uint8 vehicle_state
@@ -24,7 +25,7 @@ uint16 error_code
 float64 battery_voltage
 
 # motor state
-RangerActuatorState[4] actuator_states
+RangerActuatorState[8] actuator_states
 
 # light state
 bool light_control_enabled