Fix wheel topics functionality (#10)

LeoRover · Jul 5, 2024 · c751db9 · c751db9
1 parent 2f6dd14
commit c751db9
Showing 1 changed file with 14 additions and 8 deletions.
diff --git 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()) {