fix(mick_robot_vehicle_interface): 首次支持autoware

README.md

@@ -1,2 +1,59 @@
 # autoware_mickrobot
-该节点是为适配autoware.universe自动驾驶框架而编写的ROS2节点，目前仅支持mick_robot_chassis 开源底盘。
+该节点是为适配autoware.universe自动驾驶框架而编写的ROS2节点，目前仅支持 [mick_robot_chassis](https://github.com/RuPingCen/mick_robot_chassis) 开源底盘。
+
+适配工作参考了Autoware教程 [Creating vehicle interface](https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/creating-vehicle-interface-package/creating-vehicle-interface/)
+
+
+该节点目前实现了以下话题收发：
+
+订阅：
+
+| Topic Name                           | Topic Type                                             | Description                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     |
+| ------------------------------------ | ------------------------------------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| /control/command/control_cmd         | autoware_auto_control_msgs/msg/AckermannControlCommand | This topic includes main topics for controlling our vehicle like a steering tire angle, speed, acceleration, etc.                                                                                                                                                                                                                                                                                                                                                                                                               |
+| /control/command/gear_cmd            | autoware_auto_vehicle_msgs/msg/GearCommand             | This topic includes gear command for autonomous driving, please check message values to make sense of gears values. Please check [the message definition](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/GearCommand.idl) of this type.                                                                                                                                                                                                                                             |
+| /control/command/emergency_cmd       | tier4_vehicle_msgs/msg/VehicleEmergencyStamped         | This topic sends emergency when autoware is on emergency state. Please check [VehicleEmergencyStamped](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_vehicle_msgs/msg/VehicleEmergencyStamped.msg) message type for detailed information.                                                                                                                                                                                                                                                              |
+| /control/command/turn_indicators_cmd | autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand   | This topic indicates a turn signal for your own vehicle. Please check [TurnIndicatorsCommand](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl) message type for detailed information.                                                                                                                                                                                                                                                                      |
+| /control/command/hazard_lights_cmd   | autoware_auto_vehicle_msgs/msg/HazardLightsCommand     | This topic sends command for hazard lights. Please check [HazardLightsCommand](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/HazardLightsCommand.idl)                                                                                                                                                                                                                                                                                                                              |
+
+
+
+
+ 发布：
+ | Topic Name                             | Topic Type                                          | Description                                                                                                                                                                                                                                                                   |
+| -------------------------------------- | --------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| /vehicle/status/control_mode           | autoware_auto_vehicle_msgs/msg/ControlModeReport    | This topic describes the current control mode of vehicle. Please check [ControlModeReport](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/ControlModeReport.idl) message type for detailed information.                           |
+| /vehicle/status/gear_status            | autoware_auto_vehicle_msgs/msg/GearReport           | This topic includes the current gear status of the vehicle. Please check [GearReport](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/GearReport.idl) message type for detailed information.                                       |
+| /vehicle/status/velocity_status        | autoware_auto_vehicle_msgs/msg/VelocityReport       | This topic gives us the velocity status of the vehicle. Please check [VelocityReport](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/VelocityReport.idl) message type for detailed information.                                   |
+| /vehicle/status/hazard_lights_status   | autoware_auto_vehicle_msgs/msg/HazardLightsReport   | This topic describes hazard light status of the vehicle. Please check [HazardLightsReport](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/HazardLightsReport.idl) message type for detailed information.                          |
+| /vehicle/status/turn_indicators_status | autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport | This topic reports the turn indicators status of the vehicle. Please check [TurnIndicatorsReport](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl) message type for detailed information.                 |
+| /vehicle/status/steering_status        | autoware_auto_vehicle_msgs/msg/SteeringReport       | This topic reports the steering status of the vehicle. Please check [SteeringReport](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/SteeringReport.idl) message type for detailed information.                                    |
+
+
+经过实际考察验证，至少需要收发以下话题。此外，针对 [mick_robot_chassis](https://github.com/RuPingCen/mick_robot_chassis) 底盘，部分部件 (如转向灯) 实际不存在, 采用在 interface 内部维护一个变量方式提供。  
+
+至少需要接受以下话题    
+ - 转向、速度、加速度
+    /control/command/control_cmd
+ - 紧急停车
+    /control/command/emergency_cmd
+ - 档位控制
+    /control/command/gear_cmd
+ - 转向灯
+    /control/command/turn_indicators_cmd
+ - 双闪 危险报警闪光灯
+    /control/command/hazard_lights_cmd
+
+至少需要发布以下话题    
+ - 车辆档位状态
+    /vehicle/status/gear_status
+ - 上报车辆控制模式 用于确定是否被人工接管
+    /vehicle/status/control_mode
+ - 上报车速
+    /vehicle/status/velocity_status
+ - 上报转向
+    /vehicle/status/steering_status
+ - 上报转向灯
+    /vehicle/status/turn_indicators_status
+ - 上报双闪灯
+    /vehicle/status/hazard_lights_status

mick_robot_vehicle_interface/CMakeLists.txt

@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 3.8)
+project(mick_robot_vehicle_interface)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+# find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+
+ament_auto_add_executable(mick_robot_vehicle_interface
+  src/mick_robot_vehicle_interface/mick_robot_vehicle_interface.cpp
+  src/mick_robot_vehicle_interface/mick_robot_vehicle_interface_node.cpp
+  src/mick_robot_vehicle_interface/mick_chassis_protocol.cpp
+  # src/async_serial/AsyncSerial.cpp
+  # src/async_serial/BufferedAsyncSerial.cpp
+)
+
+# ament_package()
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)
+

mick_robot_vehicle_interface/README.md

@@ -0,0 +1,7 @@
+解析函数的结构是这样的
+
+| Prev frame |     0XAE     |  0xEA | length |  cmd  |  data |       |       |                           |  data |       |       |       |       |       |       |       |       |       |       | checksum |  0XEF |  0xFE | Next frame |
+|:----------:|:------------:|:-----:|:------:|:-----:|:-----:|:-----:|:-----:|:-------------------------:|:-----:|:-----:|:-----:|:-----:|:-----:|:-----:|:-----:|:-----:|:-----:|:-----:|:-----:|:--------:|:-----:|:-----:|:----------:|
+|            | ^ buffer_loc |       |        |       |       |       |       | ^ i = 4 + buffer_loc; i++ |       |       |       |       |       |       |       |       |       |       |       |          |       |       |            |
+|    1Byte   |     1Byte    | 1Byte |  1Byte | 1Byte | 1Byte | 1Byte | 1Byte |           1Byte           | 1Byte | 1Byte | 1Byte | 1Byte | 1Byte | 1Byte | 1Byte | 1Byte | 1Byte | 1Byte | 1Byte |   1Byte  | 1Byte | 1Byte |    1Byte   |
+

mick_robot_vehicle_interface/config/mick_robot.param.yaml

@@ -0,0 +1,15 @@
+/**:
+  ros__parameters:
+    base_frame_id: "base_link"
+    command_timeout_ms: 1000
+    loop_rate: 30.0
+    use_external_emergency_brake: true # 遥控器上切换拨杆可以实现紧急停车
+    # serial communication
+    serial_dev: "/dev/serial/by-id/usb-Prolific_Technology_Inc._USB-Serial_Controller-if00-port0"
+    baud: 115200
+
+    joy_ptopic: "/mick_robot/rc_remotes/joy"
+    chassis_type: 0 # default to differential drive  0: Differential 1: Mecanum 2: Ackermann 3: 4WS4WD
+    rc_k: 1
+    rc_min: 0
+    rc_max: 2500

mick_robot_vehicle_interface/include/mick_robot_vehicle_interface/mick_chassis_msg.h

@@ -0,0 +1,73 @@
+#ifndef CHASSIS_MICK_MSG_H
+#define CHASSIS_MICK_MSG_H
+
+#include <stdint.h>
+#include <vector>
+typedef struct
+{
+    uint8_t  available;    // 是否数据是否有效可用
+    uint32_t counter;
+
+    int32_t angle;         // abs angle range:[0,8191] 电机转角绝对值
+    int32_t last_angle;    // abs angle range:[0,8191]
+    int32_t speed_rpm;     // 转速
+    int16_t given_current; // 实际的转矩电流
+
+    uint32_t offset_angle; // 电机启动时候的零偏角度
+    int32_t  round_cnt;    // 电机转动圈数
+    int32_t  total_angle;  // 电机转动的总角度
+
+    uint8_t Temp;          // 温度
+} moto_measure_t;
+
+typedef struct
+{
+    uint8_t  available;    // 是否数据是否有效可用
+    uint32_t counter;
+    int16_t  ax, ay, az;
+    int16_t  gx, gy, gz;
+    int16_t  mx, my, mz;
+    float    pitch, roll, yaw;
+    float    pitch_rad, roll_rad, yaw_rad;
+    float    qw, qx, qy, qz;
+} imu_measure_t;
+
+typedef struct
+{
+    uint16_t ch1, ch2, ch3, ch4;
+    uint16_t ch5, ch6, ch7, ch8, ch9, ch10, ch11, ch12, ch13, ch14, ch15, ch16, ch17, ch18;
+    uint8_t  sw1, sw2, sw3, sw4;
+    uint16_t ch1_offset, ch2_offset, ch3_offset, ch4_offset;
+
+    uint8_t type;               // 1 DJI-DBUS   2 SBUS 遥控器类型
+
+    uint8_t          status;
+    volatile uint8_t update;    // 表示接收到了一个新数据
+    volatile uint8_t available; // 是否数据是否有效可用
+
+} rc_info_t;
+
+typedef struct
+{
+    char  available;
+    float vx, vy, wz;
+    float px, py, pz;
+    float roll, pitch, yaw;
+    float qx, qy, qz, qw;
+} odom_measure_t;
+
+struct chassis_measure_t
+{
+    int8_t chassis_type;       // 0:X4  1:M4  2: Ackermann  3:4WS4WD
+    int8_t chassis_motor_type; // 0 M3508    1: 安普斯电机
+
+    odom_measure_t odom_measurements;
+    // CHENG TODO 这里的初始化要全部赋值，否则编译警告
+    moto_measure_t moto_measurements[4]     = {0};
+    moto_measure_t moto_rmd_measurements[4] = {0};
+
+    volatile rc_info_t    rc;
+    imu_measure_t         imu_measurements; // IMU 数据
+    std::vector<uint16_t> ultrasonic;       // 超声波数据
+};
+#endif

mick_robot_vehicle_interface/include/mick_robot_vehicle_interface/mick_chassis_protocol.hpp

@@ -0,0 +1,70 @@
+#ifndef MICK_CHASSIS_PROTOCOL_HPP
+#define MICK_CHASSIS_PROTOCOL_HPP
+
+#include <chrono>
+#include <fstream>
+#include <iostream>
+#include <math.h>
+#include <sstream>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <boost/asio.hpp>
+#include <boost/asio/serial_port.hpp>
+
+// #include "serial/serial.h"
+
+#define WHEEL_RATIO              (19.0)  // 麦克纳母轮模式 减速比 3508电机减速比为1:19
+#define WHEEL_K                  (0.355) // 麦克纳母轮模式
+
+#define WHEEL_L                  (0.4)   // 左右轮子的间距
+#define WHEEL_D                  (0.17)  // 轮子直径  6.5寸的轮子
+// #define  WHEEL_R    WHEEL_D/2.0 			//轮子半径
+#define WHEEL_PI                 (3.141693) // pi
+
+// 协议为大端模式, 高位低地址
+//  ----------------------- 协议固定数据 -----------------------
+#define FRAME_HEADER1            (0xAE) // 帧头标识
+#define FRAME_HEADER2            (0xEA) // 帧头标识（2字节，0xAEEA）
+#define FRAME_FOOTER1            (0xEF) // 帧尾标识
+#define FRAME_FOOTER2            (0xFE) // 帧尾标识（2字节，0xEFFE）
+#define MAX_DATA_LENGTH          (0xF)  // 最大数据长度(15字节)
+#define CHECKSUM_SIZE            (1)    // 校验位（1字节）
+#define FRAME_DATA_LEN_LOC       (2)    // 数据长度位置 2 个偏移量
+// ----------------------- 命令类型定义 -----------------------
+#define CMD_TYPE_DIFF_MOTO_DATA  (0x07) // 4轮差速底盘-电机数据（MickV3）
+#define CMD_TYPE_DIFF_MOTO_STATE (0x08) // 4轮差速底盘-电机状态（MickV3）
+#define CMD_TYPE_RC_DATA         (0xA3) // 遥控器数据
+#define CMD_TYPE_ODOM_VEL        (0xA7) // 里程计之速度信息
+#define CMD_TYPE_GPIO            (0xAC) // GPIO
+// ----------------------- 接收状态机 -----------------------
+typedef enum
+{
+    STATE_WAIT_FOR_HEADER, // 等待帧头
+    STATE_READING_FRAME,   // 读取帧内容
+    STATE_ERROR            // 错误状态
+} mcpf_parser_state;       // mick chassis frame parser state
+
+// ----------------------- 宏函数 -----------------------
+#define FRAME_MINIMUM_SIZE (7)           // when data length == 0
+#define FRAME_SIZE(D_LEN)  ((D_LEN) + 4) // data length + frame header length + frame footer length
+
+void send_speed_to_chassis(std::shared_ptr<boost::asio::serial_port> ser_port_fd, int chassis_type, float speed_x, float speed_y, float speed_w);
+
+void send_rpm_to_chassis(std::shared_ptr<boost::asio::serial_port> ser_port_fd, int w1, int w2, int w3, int w4);
+
+void send_speed_to_X4chassis(std::shared_ptr<boost::asio::serial_port> ser_port_fd, float x, float y, float w);
+
+void send_speed_to_4WS4WDchassis(std::shared_ptr<boost::asio::serial_port> ser_port_fd, float x, float y, float w);
+
+void send_speed_to_Ackerchassis(std::shared_ptr<boost::asio::serial_port> ser_port_fd, float x, float w);
+
+void send_rpm_to_4WS4WDchassis(std::shared_ptr<boost::asio::serial_port> ser_port_fd, std::vector<float> vw);
+
+void clear_odometry_chassis(std::shared_ptr<boost::asio::serial_port> ser_port_fd);
+
+#endif