From c751db9f38e077267c24e32f64c8080afb8be986 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= <bsowa123@gmail.com>
Date: Fri, 5 Jul 2024 16:33:34 +0200
Subject: [PATCH] Fix wheel topics functionality (#10)

---
 main.cpp | 22 ++++++++++++++--------
 1 file changed, 14 insertions(+), 8 deletions(-)

diff --git a/main.cpp b/main.cpp
index 78bd904..39ece17 100644
--- a/main.cpp
+++ b/main.cpp
@@ -127,10 +127,10 @@ struct WheelWrapper {
   ros::Subscriber<std_msgs::Float32, WheelWrapper> cmd_vel_sub_;
 };
 
-static WheelWrapper wheel_FL_wrapper(controller->wheel_FL, "FL");
-static WheelWrapper wheel_RL_wrapper(controller->wheel_RL, "RL");
-static WheelWrapper wheel_FR_wrapper(controller->wheel_FR, "FR");
-static WheelWrapper wheel_RR_wrapper(controller->wheel_RR, "RR");
+static WheelWrapper *wheel_FL_wrapper;
+static WheelWrapper *wheel_RL_wrapper;
+static WheelWrapper *wheel_FR_wrapper;
+static WheelWrapper *wheel_RR_wrapper;
 
 class ServoWrapper {
  public:
@@ -230,10 +230,10 @@ void initROS() {
   nh.advertiseService(board_type_srv);
   nh.advertiseService(reset_board_srv);
 
-  wheel_FL_wrapper.initROS();
-  wheel_RL_wrapper.initROS();
-  wheel_FR_wrapper.initROS();
-  wheel_RR_wrapper.initROS();
+  wheel_FL_wrapper->initROS();
+  wheel_RL_wrapper->initROS();
+  wheel_FR_wrapper->initROS();
+  wheel_RR_wrapper->initROS();
 
   servo1_wrapper.initROS();
   servo2_wrapper.initROS();
@@ -259,6 +259,12 @@ void setup() {
   } else {
     controller = new diff_drive_lib::DiffDriveController(ROBOT_CONFIG);
   }
+
+  wheel_FL_wrapper = new WheelWrapper(controller->wheel_FL, "FL");
+  wheel_RL_wrapper = new WheelWrapper(controller->wheel_RL, "RL");
+  wheel_FR_wrapper = new WheelWrapper(controller->wheel_FR, "FR");
+  wheel_RR_wrapper = new WheelWrapper(controller->wheel_RR, "RR");
+
   initROS();
 
   if (imu_receiver.init()) {