Advanced wheels spin

robotics-laboratory · Apr 7, 2024 · c652b1f · c652b1f
1 parent cf0f204
commit c652b1f
Show file tree

Hide file tree

Showing 2 changed files with 36 additions and 22 deletions.
diff --git a/packages/visualization/src/visualization_node.cpp b/packages/visualization/src/visualization_node.cpp
@@ -278,8 +278,30 @@ visualization_msgs::msg::Marker makeMeshMarker(int id, const std_msgs::msg::Head
 void VisualizationNode::updateWheelsSpin() {
     const auto now_seconds = now().seconds();
     const auto time = now_seconds - cache_.last_ego_update_second;
-    const auto velocity = state_.telemetry->current_rps * model_->gearRatio() * M_2PI;
-    cache_.wheels_spin_angle = std::fmod(cache_.wheels_spin_angle + velocity * time, M_2PI);
+    const auto rear_twist = model::Twist {
+        state_.telemetry->rear_curvature,
+        state_.telemetry->rear_velocity
+    };
+    const auto velocities = model_->rearTwistToWheelVelocity(rear_twist);
+    for (auto wheel : kAllWheels) {
+        const double velocity = [&]() {
+            switch (wheel) {
+                case WheelIndex::kFrontLeft:
+                    return velocities.left.radians();
+                case WheelIndex::kFrontRight:
+                    return -velocities.right.radians();
+                case WheelIndex::kRearLeft:
+                    return velocities.rear.radians();
+                case WheelIndex::kRearRight:
+                default:
+                    return -velocities.rear.radians();
+            }
+        }();
+
+        const double angle = cache_.wheel_spin_angles[wheel] + velocity * time;
+        cache_.wheel_spin_angles[wheel] = std::fmod(angle, M_2PI);
+    }
+
     cache_.last_ego_update_second = now_seconds;
 }
 
@@ -306,25 +328,17 @@ void VisualizationNode::publishEgo() const {
     msg_array.markers.push_back(body_msg);
 
     for (auto wheel : kAllWheels) {
-        double y_angle, z_angle;
-        switch (wheel) {
-            case WheelIndex::kFrontLeft:
-                y_angle = cache_.wheels_spin_angle;
-                z_angle = state_.telemetry->current_left_steering;
-                break;
-            case WheelIndex::kRearLeft:
-                y_angle = cache_.wheels_spin_angle;
-                z_angle = 0.0;
-                break;
-            case WheelIndex::kFrontRight:
-                y_angle = -cache_.wheels_spin_angle;
-                z_angle = state_.telemetry->current_right_steering;
-                break;
-            case WheelIndex::kRearRight:
-                y_angle = -cache_.wheels_spin_angle;
-                z_angle = 0.0;
-                break;
-        }
+        const double y_angle = cache_.wheel_spin_angles[wheel];
+        const double z_angle = [&]() {
+            switch (wheel) {
+                case WheelIndex::kFrontLeft:
+                    return state_.telemetry->current_left_steering;
+                case WheelIndex::kFrontRight:
+                    return state_.telemetry->current_right_steering;
+                default:
+                    return 0.0;
+            }
+        }();
 
         auto rotation = tf2::Quaternion::getIdentity();
         rotation.setRPY(0, y_angle, z_angle);

diff --git a/packages/visualization/src/visualization_node.h b/packages/visualization/src/visualization_node.h
@@ -117,7 +117,7 @@ class VisualizationNode : public rclcpp::Node {
     struct Cache {
         tf2::Transform body_base_tf;
         std::array<tf2::Transform, 4> wheel_base_tfs;
-        double wheels_spin_angle = 0.0;
+        double wheel_spin_angles[4] = {0.0};
         double last_ego_update_second;
     } cache_;