diff --git a/src/radio_ctrl/src/radio_ctrl.cpp b/src/radio_ctrl/src/radio_ctrl.cpp index 0c8e43d..4c3eb2f 100644 --- a/src/radio_ctrl/src/radio_ctrl.cpp +++ b/src/radio_ctrl/src/radio_ctrl.cpp @@ -75,14 +75,15 @@ private: for (int i = 0; i < 10; ++i) { ch_data[i] = uart_handler_->get_channel_value(i); - // printf("ch[%d]:%d ", i, ch_data[i]); + printf("ch[%d]:%d ", i, ch_data[i]); } - // printf("\n"); + printf("\n"); - uint16_t ctrl = ch_data[4]; // 手刹 - uint16_t gear = ch_data[5]; // 挡位 - uint16_t sweep = ch_data[6]; // 清扫 - uint16_t dump = ch_data[7]; // 垃圾倾倒 + uint16_t ctrl = ch_data[4]; // 手刹 + uint16_t gear = ch_data[5]; // 挡位 + uint16_t sweep = ch_data[6]; // 清扫 + uint16_t dump = ch_data[7]; // 垃圾倾倒 + printf("sweep=%d\n", sweep); int16_t speed = ch_data[1] - 992; //[-800,800] if (!initialized) @@ -213,25 +214,25 @@ private: // 发布控制消息 pub_->publish(msg); - RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:" - << "\n brake: " << static_cast(msg.brake) // uint8 - << "\n gear: " << static_cast(msg.gear) // uint8 - << "\n rpm: " << static_cast(msg.rpm) // uint8 - << "\n ehb_enable: " << static_cast(msg.ehb_anable) // bool - << "\n ehb_brake_pressure: " << static_cast(msg.ehb_brake_pressure) // float32 - << "\n angle: " << msg.angle // float32 - << "\n angle_speed: " << msg.angle_speed // uint16 - << "\n edge_brush_lift: " << static_cast(msg.edge_brush_lift) // bool - << "\n sweep_ctrl: " << static_cast(msg.sweep_ctrl) // bool - << "\n spray: " << static_cast(msg.spray) // bool - << "\n mud_flap: " << static_cast(msg.mud_flap) // bool - << "\n dust_shake: " << static_cast(msg.dust_shake) // bool - << "\n left_light: " << static_cast(msg.left_light) // bool - << "\n right_light: " << static_cast(msg.right_light) // bool - << "\n brake_light: " << static_cast(msg.brake_light) // bool - << "\n headlight: " << static_cast(msg.headlight) // bool - << "\n dump: " << static_cast(msg.dump) // uint8 - ); + // RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:" + // << "\n brake: " << static_cast(msg.brake) // uint8 + // << "\n gear: " << static_cast(msg.gear) // uint8 + // << "\n rpm: " << static_cast(msg.rpm) // uint8 + // << "\n ehb_enable: " << static_cast(msg.ehb_anable) // bool + // << "\n ehb_brake_pressure: " << static_cast(msg.ehb_brake_pressure) // float32 + // << "\n angle: " << msg.angle // float32 + // << "\n angle_speed: " << msg.angle_speed // uint16 + // << "\n edge_brush_lift: " << static_cast(msg.edge_brush_lift) // bool + // << "\n sweep_ctrl: " << static_cast(msg.sweep_ctrl) // bool + // << "\n spray: " << static_cast(msg.spray) // bool + // << "\n mud_flap: " << static_cast(msg.mud_flap) // bool + // << "\n dust_shake: " << static_cast(msg.dust_shake) // bool + // << "\n left_light: " << static_cast(msg.left_light) // bool + // << "\n right_light: " << static_cast(msg.right_light) // bool + // << "\n brake_light: " << static_cast(msg.brake_light) // bool + // << "\n headlight: " << static_cast(msg.headlight) // bool + // << "\n dump: " << static_cast(msg.dump) // uint8 + // ); } else {