Merge pull request #8 from husarion/fix-destroy

Added subs destroy and ignore nans
husarion · Feb 14, 2023 · da9adc8 · da9adc8
2 parents f0983ad + ee460f6
commit da9adc8
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 2 deletions.
diff --git a/src/main.cpp b/src/main.cpp
@@ -182,6 +182,15 @@ void wheels_command_callback(const void *msgin) {
             },
             .mode = MPS
         };
+
+        for(auto i = 0u; i < MOTORS_COUNT; ++i){
+            if(isnan(new_speed.speed[i])){
+                float zero_speeds[MOTORS_COUNT] = {0, 0, 0, 0};
+                memcpy(new_speed.speed, zero_speeds, sizeof(new_speed.speed));
+                break;
+            }
+        }
+
         drive.updateTargetSpeed(new_speed);
         last_speed_command_time = odom_watchdog_timer.read_ms();
         is_speed_watchdog_active = false;

diff --git a/src/microros.cpp b/src/microros.cpp
@@ -87,17 +87,19 @@ bool microros_deinit() {
     (void)rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);
 
     RCCHECK(rcl_subscription_fini(&wheels_command_sub, &node));
+    RCCHECK(rcl_subscription_fini(&servos_command_sub, &node));
+    for (auto i = 0u; i < LED_COUNT; ++i) {
+        RCCHECK(rcl_subscription_fini(&led_subs[i], &node));
+    }
     RCCHECK(rcl_publisher_fini(&wheels_state_pub, &node));
     RCCHECK(rcl_publisher_fini(&imu_pub, &node));
     RCCHECK(rcl_publisher_fini(&battery_pub, &node));
     for (auto i = 0u; i < RANGES_COUNT; ++i) {
         RCCHECK(rcl_publisher_fini(&range_pubs[i], &node));
     }
-
     for (auto i = 0u; i < BUTTONS_COUNT; ++i) {
         RCCHECK(rcl_publisher_fini(&buttons_pubs[i], &node));
     }
-
     RCCHECK(rclc_parameter_server_fini(&param_server, &node));
     RCCHECK(rclc_executor_fini(&executor));
     RCCHECK(rcl_node_fini(&node));