Refactoring

packages/model/src/model.cpp

@@ -164,13 +164,12 @@ WheelVelocity Model::rearTwistToWheelVelocity(Twist rear_twist) const {
     const double rear_curvature_squared = squared(rear_twist.curvature);
 
     const double rear_ratio = rear_twist.curvature * cache_.width_half;
-    const double front_ratio_base_squared =
-        1 + rear_curvature_squared * cache_.width_half_squared
-        + rear_curvature_squared * cache_.length_squared;
-    const double front_left_ratio = sqrt(front_ratio_base_squared
-        - rear_twist.curvature * params_.wheel_base.width);
-    const double front_right_ratio = sqrt(front_ratio_base_squared
-        + rear_twist.curvature * params_.wheel_base.width);
+    const double front_ratio_base_squared = 1 + rear_curvature_squared * cache_.width_half_squared
+                                            + rear_curvature_squared * cache_.length_squared;
+    const double front_left_ratio =
+        sqrt(front_ratio_base_squared - rear_twist.curvature * params_.wheel_base.width);
+    const double front_right_ratio =
+        sqrt(front_ratio_base_squared + rear_twist.curvature * params_.wheel_base.width);
 
     return WheelVelocity{
         geom::Angle{(1 - rear_ratio) * rear_spin_velocity},

packages/model/src/pybind.cpp

@@ -10,12 +10,12 @@ std::string to_string(const T& obj);
 
 template<>
 std::string to_string<double>(const double& obj) {
-    return boost::str(boost::format("%.5d") % obj);
+    return boost::str(boost::format("%.5f") % obj);
 }
 
 template<>
 std::string to_string<geom::Angle>(const geom::Angle& obj) {
-    return boost::str(boost::format("Angle(%.1d deg)") % obj.degrees());
+    return boost::str(boost::format("Angle(%.5f deg)") % obj.degrees());
 }
 
 template<>
@@ -35,7 +35,7 @@ std::string to_string<model::WheelVelocity>(const model::WheelVelocity& obj) {
 template<>
 std::string to_string<model::Twist>(const model::Twist& obj) {
     return boost::str(
-        boost::format("Twist(curvature=%.5d, velocity=%.5d)") % obj.curvature % obj.velocity);
+        boost::format("Twist(curvature=%.5f, velocity=%.5f)") % obj.curvature % obj.velocity);
 }
 
 template<>

packages/simulator_2d/include/simulator_2d/truck_state.h

@@ -23,7 +23,7 @@ class TruckState {
     const std::vector<float>& lidarRanges() const;
     double currentMotorRps() const;
     double targetMotorRps() const;
-    model::WheelVelocity wheelVelocity() const;
+    const& model::WheelVelocity wheelVelocity() const;
     geom::Vec3 gyroAngularVelocity() const;
     geom::Vec3 accelLinearAcceleration() const;
 
@@ -38,9 +38,9 @@ class TruckState {
     TruckState& lidarRanges(std::vector<float> lidar_ranges);
     TruckState& currentMotorRps(double current_rps);
     TruckState& targetMotorRps(double target_rps);
-    TruckState& wheelVelocity(model::WheelVelocity wheel_velocity);
-    TruckState& gyroAngularVelocity(geom::Vec3 angular_velocity);
-    TruckState& accelLinearAcceleration(geom::Vec3 linear_acceleration);
+    TruckState& wheelVelocity(const model::WheelVelocity& wheel_velocity);
+    TruckState& gyroAngularVelocity(const geom::Vec3& angular_velocity);
+    TruckState& accelLinearAcceleration(const geom::Vec3& linear_acceleration);
 
   private:
     struct Cache {

packages/simulator_2d/src/truck_state.cpp

@@ -27,7 +27,7 @@ double TruckState::currentMotorRps() const { return cache_.current_motor_rps; }
 
 double TruckState::targetMotorRps() const { return cache_.target_motor_rps; }
 
-model::WheelVelocity TruckState::wheelVelocity() const { return cache_.wheel_velocity; }
+const& model::WheelVelocity TruckState::wheelVelocity() const { return cache_.wheel_velocity; }
 
 geom::Vec3 TruckState::gyroAngularVelocity() const { return cache_.gyro_angular_velocity; }
 
@@ -88,17 +88,17 @@ TruckState& TruckState::targetMotorRps(double target_rps) {
     return *this;
 }
 
-TruckState& TruckState::wheelVelocity(model::WheelVelocity wheel_velocity) {
+TruckState& TruckState::wheelVelocity(const model::WheelVelocity& wheel_velocity) {
     cache_.wheel_velocity = wheel_velocity;
     return *this;
 }
 
-TruckState& TruckState::gyroAngularVelocity(geom::Vec3 angular_velocity) {
+TruckState& TruckState::gyroAngularVelocity(const geom::Vec3& angular_velocity) {
     cache_.gyro_angular_velocity = angular_velocity;
     return *this;
 }
 
-TruckState& TruckState::accelLinearAcceleration(geom::Vec3 linear_acceleration) {
+TruckState& TruckState::accelLinearAcceleration(const geom::Vec3& linear_acceleration) {
     cache_.accel_linear_acceleration = linear_acceleration;
     return *this;
 }

packages/visualization/src/visualization_node.cpp

@@ -159,7 +159,6 @@ void VisualizationNode::initializeCacheWheelBaseTfs() {
 void VisualizationNode::initializeCache() {
     initializeCacheBodyBaseTf();
     initializeCacheWheelBaseTfs();
-    cache_.last_ego_update_second = now().seconds();
 }
 
 void VisualizationNode::handleTelemetry(truck_msgs::msg::HardwareTelemetry::ConstSharedPtr msg) {
@@ -266,7 +265,9 @@ visualization_msgs::msg::Marker makeMeshMarker(
 
 void VisualizationNode::updateWheelsSpin() {
     const auto now_seconds = now().seconds();
-    const auto time = now_seconds - cache_.last_ego_update_second;
+    const auto before = state_.last_ego_update_second.value_or(now_seconds);
+    const auto time = now_seconds - before;
+
     const model::WheelVelocity wheel_velocity = {
         geom::Angle(state_.telemetry->rear_left_wheel_velocity),
         geom::Angle(state_.telemetry->rear_right_wheel_velocity),
@@ -288,11 +289,11 @@ void VisualizationNode::updateWheelsSpin() {
             }
         }();
 
-        const double angle = cache_.wheel_spin_angles[wheel] + velocity * time;
-        cache_.wheel_spin_angles[wheel] = std::fmod(angle, M_2PI);
+        const double angle = state_.wheel_spin_angles[wheel] + velocity * time;
+        state_.wheel_spin_angles[wheel] = std::fmod(angle, M_2PI);
     }
 
-    cache_.last_ego_update_second = now_seconds;
+    state_.last_ego_update_seconds = now_seconds;
 }
 
 void VisualizationNode::updateEgo() {
@@ -318,7 +319,7 @@ void VisualizationNode::publishEgo() const {
     msg_array.markers.push_back(body_msg);
 
     for (auto wheel : kAllWheels) {
-        const double y_angle = cache_.wheel_spin_angles[wheel];
+        const double y_angle = state_.wheel_spin_angles[wheel];
         const double z_angle = [&]() {
             switch (wheel) {
                 case WheelIndex::kFrontLeft:

packages/visualization/src/visualization_node.h

@@ -22,6 +22,7 @@
 #include <rclcpp/time.hpp>
 
 #include <string>
+#include <optional>
 
 namespace truck::visualization {
 
@@ -107,8 +108,6 @@ class VisualizationNode : public rclcpp::Node {
     struct Cache {
         tf2::Transform body_base_tf;
         std::array<tf2::Transform, 4> wheel_base_tfs;
-        double wheel_spin_angles[4] = {0.0};
-        double last_ego_update_second;
     } cache_;
 
     std::unique_ptr<model::Model> model_ = nullptr;
@@ -124,6 +123,8 @@ class VisualizationNode : public rclcpp::Node {
         truck_msgs::msg::Waypoints::ConstSharedPtr waypoints = nullptr;
         truck_msgs::msg::NavigationMesh::ConstSharedPtr navigation_mesh = nullptr;
         truck_msgs::msg::NavigationRoute::ConstSharedPtr navigation_route = nullptr;
+        double wheel_spin_angles[4] = {0.0};
+        std::optional<double> last_ego_update_seconds = nullptr;
     } state_;
 
     struct Slots {