Try   HackMD

Updated: 2023/07/22

ROS2 Communication

Communication Interfaces

  • Topic (sensor informations)
    • Publisher: IMU, GPS, Ultrasound, etc.
    • Subscriber: control server or AI compute unit
  • Service (Ying's ID server bridging, time sync and safety server communication)
    • Server:
      • TimeSyncServer: Set time reference server
      • SafetyServer: Manage devices emergency
    • Client:
      • TimeSyncNode: Can be inherited by ros2 node. Inherited from rclcpp::Node, setting Timesync client and operates background.
      • SafetyNode: Can be inherited by ros2 node. Inherited from rclcpp::Node, setting SafetyServer client and operates background.

Topic Message Type (vehicle_interfaces)

  • Header.msg (the header for all message type in vehicle_interfaces)

    • Members
      • priority: declares message priority, from high to low: Control, Sensor and Info
      • devicetype: declares message-sending device type
      • stamptype: declares message timestamp type, e.g., no sync, non-utc synced and utc synced.
      • stamp: message timestamp, including seconds and nano seconds
      • ref_publish_time_ms: the setting of message publish intervals.
    • Definitions
    ​​​​uint8 PRIORITY_CONTROL = 0
    ​​​​uint8 PRIORITY_SENSOR = 1
    ​​​​uint8 PRIORITY_INFO = 2
    
    ​​​​uint8 DEVTYPE_NONE = 0
    ​​​​uint8 DEVTYPE_IDTABLE = 1
    ​​​​uint8 DEVTYPE_JOYSTICK = 2
    ​​​​uint8 DEVTYPE_STEERINGWHEEL = 3
    ​​​​uint8 DEVTYPE_MOTOR = 4
    ​​​​uint8 DEVTYPE_BATTERY = 5
    ​​​​uint8 DEVTYPE_IMU = 6
    ​​​​uint8 DEVTYPE_GPS = 7
    ​​​​uint8 DEVTYPE_ENVIRONMENT = 8
    ​​​​uint8 DEVTYPE_RF = 9
    ​​​​uint8 DEVTYPE_ULTRASONIC = 10
    ​​​​uint8 DEVTYPE_IMAGE = 11
    ​​​​uint8 DEVTYPE_MOTOR_AXLE = 12
    ​​​​uint8 DEVTYPE_MOTOR_STEERING = 13
    ​​​​uint8 DEVTYPE_MOTOR_BRAKING = 14
    
    
    ​​​​uint8 STAMPTYPE_NO_SYNC = 0
    ​​​​uint8 STAMPTYPE_NONE_UTC_SYNC = 1
    ​​​​uint8 STAMPTYPE_UTC_SYNC = 2
    
    ​​​​float32 VERSION = 1.3
    
    • Variables
    ​​​​uint8 priority 2
    ​​​​uint8 device_type 0
    ​​​​string device_id
    ​​​​uint64 frame_id 0
    ​​​​uint8 stamp_type 0
    ​​​​builtin_interfaces/Time stamp
    
    ​​​​float32 ref_publish_time_ms
    
    • Examples
      • C++
      ​​​​​​​​#include "vehicle_interfaces/msg/wheel_state.hpp"
      
      ​​​​​​​​// The remaining code is under rclcpp::Node class
      ​​​​​​​​auto msg = vehicle_interfaces::msg::WheelState();
      ​​​​​​​​msg.header.priority = vehicle_interfaces::msg::Header::PRIORITY_CONTROL;
      ​​​​​​​​msg.header.device_type = vehicle_interfaces::msg::Header::DEVTYPE_JOYSTICK;
      ​​​​​​​​msg.header.device_id = this->nodeName_;
      ​​​​​​​​msg.header.stamp_type = vehicle_interfaces::msg::Header::STAMPTYPE_NONE_UTC_SYNC
      ​​​​​​​​msg.header.stamp = this->get_clock()->now();
      
      • Python3
      ​​​​​​​​from vehicle_interfaces.msg import WheelState
      
      ​​​​​​​​# The remaining code is under rclpy.Node class
      ​​​​​​​​msg = WheelState()
      ​​​​​​​​msg.header.priority = msg.header.PRIORITY_CONTROL
      ​​​​​​​​msg.header.device_type = msg.header.DEVTYPE_JOYSTICK
      ​​​​​​​​msg.header.device_id = self.nodeName_
      ​​​​​​​​msg.header.stamp_type = msg.header.STAMPTYPE_NONE_UTC_SYNC
      ​​​​​​​​msg.header.stamp = self.get_clock().now().to_msg()
      
  • Distance.msg (for distance measurement sensor, e.g., ultrasonic sensor)

    • Definitions
    ​​​​uint8 UNIT_MICRO = 0
    ​​​​uint8 UNIT_MILLI = 1
    ​​​​uint8 UNIT_CENTI = 2
    ​​​​uint8 UNIT_METER = 3
    
    • Variables
    ​​​​vehicle_interfaces/Header header
    ​​​​uint8 unit_type 1
    ​​​​float32 min
    ​​​​float32 max
    ​​​​float32 distance
    
    • Examples
      • C++
      ​​​​​​​​#include "vehicle_interfaces/msg/distance.hpp"
      
      ​​​​​​​​// The remaining code is under rclcpp::Node class
      ​​​​​​​​static u_int64_t frame_id = 0;
      ​​​​​​​​auto msg = vehicle_interfaces::msg::Distance();
      ​​​​​​​​msg.header.priority = vehicle_interfaces::msg::Header::PRIORITY_SENSOR;
      ​​​​​​​​msg.header.device_type = vehicle_interfaces::msg::Header::DEVTYPE_ULTRASONIC;
      ​​​​​​​​msg.header.device_id = this->nodeName_;
      ​​​​​​​​msg.header.frame_id = frame_id++;
      ​​​​​​​​msg.header.stamp_type = vehicle_interfaces::msg::Header::STAMPTYPE_NONE_UTC_SYNC
      ​​​​​​​​msg.header.stamp = this->get_clock()->now();
      
      ​​​​​​​​msg.unit_type = vehicle_interfaces::msg::Distance::UNIT_METER;
      ​​​​​​​​msg.min = 0.2;
      ​​​​​​​​msg.max = 8.0;
      ​​​​​​​​msg.distance = dist;
      
      • Python3
      ​​​​​​​​from vehicle_interfaces.msg import Distance
      
      ​​​​​​​​# The remaining code is under rclpy.Node class
      ​​​​​​​​msg = Distance()
      ​​​​​​​​msg.header.priority = msg.header.PRIORITY_SENSOR
      ​​​​​​​​msg.header.device_type = msg.header.DEVTYPE_ULTRASONIC
      ​​​​​​​​msg.header.device_id = self.nodeName_
      ​​​​​​​​msg.header.frame_id = self.frame_id_# Defined in __init__
      ​​​​​​​​self.frame_id_ += 1
      ​​​​​​​​msg.header.stamp_type = msg.header.STAMPTYPE_NONE_UTC_SYNC
      ​​​​​​​​msg.header.stamp = self.get_clock().now().to_msg()
      ​​​​​​​​
      ​​​​​​​​msg.unit_type = msg.UNIT_METER
      ​​​​​​​​msg.min = 0.2
      ​​​​​​​​msg.max = 8.0
      ​​​​​​​​msg.distance = dist
      
  • Environment.msg (for environment sensor, e.g., temperature, humidity and atmosphere pressure sensors)

    • Definitions
    ​​​​uint8 UNIT_TEMP_CELSIUS = 0
    ​​​​uint8 UNIT_TEMP_FAHRENHEIT = 1
    
    ​​​​uint8 UNIT_PRESS_MBAR = 0
    ​​​​uint8 UNIT_PRESS_HPA = 4
    
    • Variables
    ​​​​vehicle_interfaces/Header header
    ​​​​uint8 unit_type 0
    ​​​​float32 temperature
    ​​​​float32 relative_humidity
    ​​​​float32 pressure
    
    • Examples
      • C++
      ​​​​​​​​#include "vehicle_interfaces/msg/environment.hpp"
      
      ​​​​​​​​// The remaining code is under rclcpp::Node class
      ​​​​​​​​static u_int64_t frame_id = 0;
      ​​​​​​​​auto msg = vehicle_interfaces::msg::Environment();
      ​​​​​​​​msg.header.priority = vehicle_interfaces::msg::Header::PRIORITY_SENSOR;
      ​​​​​​​​msg.header.device_type = vehicle_interfaces::msg::Header::DEVTYPE_ENVIRONMENT;
      ​​​​​​​​msg.header.device_id = this->nodeName_;
      ​​​​​​​​msg.header.frame_id = frame_id++;
      ​​​​​​​​msg.header.stamp_type = vehicle_interfaces::msg::Header::STAMPTYPE_NONE_UTC_SYNC;
      ​​​​​​​​msg.header.stamp = this->get_clock()->now();
      
      ​​​​​​​​msg.unit_type = vehicle_interfaces::msg::Environment::UNIT_TEMP_CELSIUS | vehicle_interfaces::msg::Environment::UNIT_PRESS_MBAR;
      ​​​​​​​​msg.temperature = temp;
      ​​​​​​​​msg.relative_humidity = rh;
      ​​​​​​​​msg.pressure = press;
      
      • Python3
      ​​​​​​​​from vehicle_interfaces.msg import Environment
      
      ​​​​​​​​# The remaining code is under rclpy.Node class
      ​​​​​​​​msg = Environment()
      ​​​​​​​​msg.header.priority = msg.header.PRIORITY_SENSOR
      ​​​​​​​​msg.header.device_type = msg.header.DEVTYPE_ENVIRONMENT
      ​​​​​​​​msg.header.device_id = self.nodeName_
      ​​​​​​​​msg.header.frame_id = self.frame_id_# Defined in __init__
      ​​​​​​​​self.frame_id_ += 1
      ​​​​​​​​msg.header.stamp_type = msg.header.STAMPTYPE_NONE_UTC_SYNC
      ​​​​​​​​msg.header.stamp = self.get_clock().now().to_msg()
      
      ​​​​​​​​msg.unit_type = msg.UNIT_TEMP_CELSIUS | msg.UNIT_PRESS_MBAR
      ​​​​​​​​msg.temperature = temp
      ​​​​​​​​msg.relative_humidity = rh
      ​​​​​​​​msg.pressure = press
      
  • GPS.msg (for GNSS localization modules)

    • Definitions
    ​​​​uint8 GPS_NONE = 0
    ​​​​uint8 GPS_STABLE = 1
    ​​​​uint8 GPS_RTK = 2
    
    • Variables
    ​​​​vehicle_interfaces/Header header
    ​​​​uint8 gps_status 0
    ​​​​float32 latitude 0
    ​​​​float32 longitude 0
    
    • Examples
      • C++
      ​​​​​​​​#include "vehicle_interfaces/msg/gps.hpp"
      
      ​​​​​​​​// The remaining code is under rclcpp::Node class
      ​​​​​​​​static u_int64_t frame_id = 0;
      ​​​​​​​​auto msg = vehicle_interfaces::msg::GPS();
      ​​​​​​​​msg.header.priority = vehicle_interfaces::msg::Header::PRIORITY_SENSOR;
      ​​​​​​​​msg.header.device_type = vehicle_interfaces::msg::Header::DEVTYPE_GPS;
      ​​​​​​​​msg.header.device_id = this->nodeName_;
      ​​​​​​​​msg.header.frame_id = frame_id++;
      ​​​​​​​​msg.header.stamp_type = vehicle_interfaces::msg::Header::STAMPTYPE_NONE_UTC_SYNC;
      ​​​​​​​​msg.header.stamp = this->get_clock()->now();
      
      ​​​​​​​​msg.gps_status = vehicle_interfaces::msg::GPS::GPS_STABLE;
      ​​​​​​​​msg.latitude = lat;
      ​​​​​​​​msg.longitude = lon;
      
      • Python3
      ​​​​​​​​from vehicle_interfaces.msg import GPS
      
      ​​​​​​​​# The remaining code is under rclpy.Node class
      ​​​​​​​​msg = GPS()
      ​​​​​​​​msg.header.priority = msg.header.PRIORITY_SENSOR
      ​​​​​​​​msg.header.device_type = msg.header.DEVTYPE_GPS
      ​​​​​​​​msg.header.device_id = self.nodeName_
      ​​​​​​​​msg.header.frame_id = self.frame_id_# Defined in __init__
      ​​​​​​​​self.frame_id_ += 1
      ​​​​​​​​msg.header.stamp_type = msg.header.STAMPTYPE_NONE_UTC_SYNC
      ​​​​​​​​msg.header.stamp = self.get_clock().now().to_msg()
      
      ​​​​​​​​msg.gps_status = GPS.GPS_STABLE
      ​​​​​​​​msg.latitude = lat
      ​​​​​​​​msg.longitude = lon
      
  • IDTable.msg (for the table retrieved from ID server)

    • Variables
    ​​​​vehicle_interfaces/Header header
    ​​​​string[] idtable
    
    • Examples
      • C++
      ​​​​​​​​#include "vehicle_interfaces/msg/id_table.hpp"
      
      ​​​​​​​​// The remaining code is under rclcpp::Node class
      ​​​​​​​​static u_int64_t frame_id = 0;
      ​​​​​​​​auto msg = vehicle_interfaces::msg::IDTable();
      ​​​​​​​​msg.header.priority = vehicle_interfaces::msg::Header::PRIORITY_INFO;
      ​​​​​​​​msg.header.device_type = vehicle_interfaces::msg::Header::DEVTYPE_IDTABLE;
      ​​​​​​​​msg.header.device_id = this->nodeName_;
      ​​​​​​​​msg.header.frame_id = frame_id++;
      ​​​​​​​​msg.header.stamp_type = vehicle_interfaces::msg::Header::STAMPTYPE_NONE_UTC_SYNC;
      ​​​​​​​​msg.header.stamp = this->get_clock()->now();
      
      ​​​​​​​​msg.idtable = idtable;// std::vector<std::string>
      
      • Python3
      ​​​​​​​​from vehicle_interfaces.msg import IDTable
      
      ​​​​​​​​# The remaining code is under rclpy.Node class
      ​​​​​​​​msg = IDTable()
      ​​​​​​​​msg.header.priority = msg.header.PRIORITY_INFO
      ​​​​​​​​msg.header.device_type = msg.header.DEVTYPE_IDTABLE
      ​​​​​​​​msg.header.device_id = self.nodeName_
      ​​​​​​​​msg.header.frame_id = self.frame_id_# Defined in __init__
      ​​​​​​​​self.frame_id_ += 1
      ​​​​​​​​msg.header.stamp_type = msg.header.STAMPTYPE_NONE_UTC_SYNC
      ​​​​​​​​msg.header.stamp = self.get_clock().now().to_msg()
      
      ​​​​​​​​msg.idtable = table# list[]
      
  • IMU.msg (for IMU sensor, e.g., gyroscope, accelerometer and compass)

    • Definitions
    ​​​​uint8 UNIT_ROT_RAD = 0
    ​​​​uint8 UNIT_ROT_DPS = 1
    
    ​​​​uint8 UNIT_ACC_GS = 0
    ​​​​uint8 UNIT_ACC_MS2 = 4
    
    • Variables
    ​​​​vehicle_interfaces/Header header
    ​​​​uint8 unit_type 0
    ​​​​float32[4] orientation
    ​​​​float32[3] angular_velocity
    ​​​​float32[3] linear_acceleration
    
    • Examples
      • C++
      ​​​​​​​​#include "vehicle_interfaces/msg/imu.hpp"
      
      ​​​​​​​​// The remaining code is under rclcpp::Node class
      ​​​​​​​​static u_int64_t frame_id = 0;
      ​​​​​​​​auto msg = vehicle_interfaces::msg::IMU();
      ​​​​​​​​msg.header.priority = vehicle_interfaces::msg::Header::PRIORITY_SENSOR;
      ​​​​​​​​msg.header.device_type = vehicle_interfaces::msg::Header::DEVTYPE_IMU;
      ​​​​​​​​msg.header.device_id = this->nodeName_;
      ​​​​​​​​msg.header.frame_id = frame_id++;
      ​​​​​​​​msg.header.stamp_type = vehicle_interfaces::msg::Header::STAMPTYPE_NONE_UTC_SYNC;
      ​​​​​​​​msg.header.stamp = this->get_clock()->now();
      
      ​​​​​​​​msg.unit_type = vehicle_interfaces::msg::IMU::UNIT_ACC_GS | vehicle_interfaces::msg::IMU::UNIT_ROT_DPS;
      ​​​​​​​​msg.orientation = quaternion;
      ​​​​​​​​msg.linear_acceleration = acc;
      ​​​​​​​​msg.angular_velocity = gyro;
      
      • Python3
      ​​​​​​​​from vehicle_interfaces.msg import IMU
      
      ​​​​​​​​# The remaining code is under rclpy.Node class
      ​​​​​​​​msg = IMU()
      ​​​​​​​​msg.header.priority = msg.header.PRIORITY_SENSOR
      ​​​​​​​​msg.header.device_type = msg.header.DEVTYPE_IMU
      ​​​​​​​​msg.header.device_id = self.nodeName_
      ​​​​​​​​msg.header.frame_id = self.frame_id_# Defined in __init__
      ​​​​​​​​self.frame_id_ += 1
      ​​​​​​​​msg.header.stamp_type = msg.header.STAMPTYPE_NONE_UTC_SYNC
      ​​​​​​​​msg.header.stamp = self.get_clock().now().to_msg()
      
      ​​​​​​​​msg.unit_type = msg.UNIT_ACC_GS | msg.UNIT_ROT_RAD
      ​​​​​​​​msg.orientation[0] = quaternion[0]
      ​​​​​​​​msg.orientation[1] = quaternion[1]
      ​​​​​​​​msg.orientation[2] = quaternion[2]
      ​​​​​​​​msg.orientation[3] = quaternion[3]
      ​​​​​​​​msg.linear_acceleration[0] = acc['x']
      ​​​​​​​​msg.linear_acceleration[1] = acc['y']
      ​​​​​​​​msg.linear_acceleration[2] = acc['z']
      ​​​​​​​​msg.angular_velocity[0] = gyro['x']
      ​​​​​​​​msg.angular_velocity[1] = gyro['y']
      ​​​​​​​​msg.angular_velocity[2] = gyro['z']
      
  • MotorAxle.msg (for motor status)

    • Definitions
    
    
    • Variables
    ​​​​vehicle_interfaces/Header header
    
    ​​​​int8 dir
    ​​​​float32 pwm
    ​​​​int8 parking
    
    • Examples
      • C++
      ​​​​​​​​#include "vehicle_interfaces/msg/motor_axle.hpp"
      
      ​​​​​​​​// The remaining code is under rclcpp::Node class
      ​​​​​​​​static u_int64_t frame_id = 0;
      ​​​​​​​​auto msg = vehicle_interfaces::msg::MotorAxle();
      ​​​​​​​​msg.header.priority = vehicle_interfaces::msg::Header::PRIORITY_INFO;
      ​​​​​​​​msg.header.device_type = vehicle_interfaces::msg::Header::DEVTYPE_MOTOR_AXLE;
      ​​​​​​​​msg.header.device_id = this->nodeName_;
      ​​​​​​​​msg.header.frame_id = frame_id++;
      ​​​​​​​​msg.header.stamp_type = vehicle_interfaces::msg::Header::STAMPTYPE_NONE_UTC_SYNC;
      ​​​​​​​​msg.header.stamp = this->get_clock()->now();
      
      ​​​​​​​​msg.dir = direction;// Motor spin direction
      ​​​​​​​​msg.pwm = pwm;
      ​​​​​​​​msg.parking = parking;
      
      • Python3
      ​​​​​​​​from vehicle_interfaces.msg import MotorAxle
      
      ​​​​​​​​# The remaining code is under rclpy.Node class
      ​​​​​​​​msg = MotorAxle()
      ​​​​​​​​msg.header.priority = msg.header.PRIORITY_SENSOR
      ​​​​​​​​msg.header.device_type = msg.header.DEVTYPE_MOTOR_AXLE
      ​​​​​​​​msg.header.device_id = self.nodeName_
      ​​​​​​​​msg.header.frame_id = self.frame_id_# Defined in __init__
      ​​​​​​​​self.frame_id_ += 1
      ​​​​​​​​msg.header.stamp_type = msg.header.STAMPTYPE_NO_SYNC
      ​​​​​​​​msg.header.stamp = self.get_clock().now().to_msg()
      
      ​​​​​​​​RunClient.ros2DictLock.acquire()
      ​​​​​​​​tmp = RunClient.axleDict# Motor status dictionary in RunClient.py
      ​​​​​​​​RunClient.ros2DictLock.release()
      ​​​​​​​​
      ​​​​​​​​msg.dir = tmp["dir"]
      ​​​​​​​​msg.pwm = float(tmp["pwm"])
      ​​​​​​​​msg.parking = tmp["parking"]
      
  • MotorSteering.msg (for steering motor status)

    • Definitions
    ​​​​uint8 UNIT_ANG = 0
    ​​​​uint8 UNIT_DIST = 1
    
    • Variables
    ​​​​vehicle_interfaces/Header header
    ​​​​uint8 unit_type 0
    ​​​​float32 min
    ​​​​float32 max
    ​​​​float32 center
    ​​​​float32 value
    
    • Examples
      • C++
      ​​​​​​​​#include "vehicle_interfaces/msg/motor_steering.hpp"
      
      ​​​​​​​​// The remaining code is under rclcpp::Node class
      ​​​​​​​​static u_int64_t frame_id = 0;
      ​​​​​​​​auto msg = vehicle_interfaces::msg::MotorSteering();
      ​​​​​​​​msg.header.priority = vehicle_interfaces::msg::Header::PRIORITY_INFO;
      ​​​​​​​​msg.header.device_type = vehicle_interfaces::msg::Header::DEVTYPE_MOTOR_STEERING;
      ​​​​​​​​msg.header.device_id = this->nodeName_;
      ​​​​​​​​msg.header.frame_id = frame_id++;
      ​​​​​​​​msg.header.stamp_type = vehicle_interfaces::msg::Header::STAMPTYPE_NONE_UTC_SYNC;
      ​​​​​​​​msg.header.stamp = this->get_clock()->now();
      
      ​​​​​​​​msg.unit_type = unit_type;// Motor spin direction
      ​​​​​​​​msg.min = min;
      ​​​​​​​​msg.max = max;
      ​​​​​​​​msg.center = center;
      ​​​​​​​​msg.value = value;
      
      • Python3
      ​​​​​​​​from vehicle_interfaces.msg import MotorSteering
      
      ​​​​​​​​# The remaining code is under rclpy.Node class
      ​​​​​​​​msg = MotorSteering()
      ​​​​​​​​msg.header.priority = msg.header.PRIORITY_SENSOR
      ​​​​​​​​msg.header.device_type = msg.header.DEVTYPE_MOTOR_STEERING
      ​​​​​​​​msg.header.device_id = self.nodeName_
      ​​​​​​​​msg.header.frame_id = self.frame_id_# Defined in __init__
      ​​​​​​​​self.frame_id_ += 1
      ​​​​​​​​msg.header.stamp_type = msg.header.STAMPTYPE_NO_SYNC
      ​​​​​​​​msg.header.stamp = self.get_clock().now().to_msg()
      
      ​​​​​​​​RunClient.ros2DictLock.acquire()
      ​​​​​​​​tmp = RunClient.steeringDict# Steering motor status dictionary in RunClient.py
      ​​​​​​​​RunClient.ros2DictLock.release()
      ​​​​​​​​
      ​​​​​​​​msg.unit_type = msg.UNIT_DIST
      ​​​​​​​​msg.min = float(tmp["min"])
      ​​​​​​​​msg.max = float(tmp["max"])
      ​​​​​​​​msg.center = float(tmp["center"])
      ​​​​​​​​msg.value = float(tmp["value"])
      
  • WheelState.msg (for steering wheel and pedal control signal)

    • Definitions
    ​​​​uint8 GEAR_PARK = 0
    ​​​​uint8 GEAR_REVERSE = 1
    ​​​​uint8 GEAR_NEUTRAL = 2
    ​​​​uint8 GEAR_DRIVE = 3
    
    • Variables
    ​​​​vehicle_interfaces/Header header
    ​​​​uint8 gear 0
    ​​​​int16 steering 0
    ​​​​int16 pedal_throttle 0
    ​​​​int16 pedal_brake 0
    ​​​​int16 pedal_clutch 0
    ​​​​
    ​​​​uint8 button 0
    ​​​​uint8 func 0
    
    • Examples
      • C++
      ​​​​​​​​#include "vehicle_interfaces/msg/wheel_state.hpp"
      
      ​​​​​​​​// The remaining code is under rclcpp::Node class
      ​​​​​​​​static u_int64_t frame_id = 0;
      ​​​​​​​​auto msg = vehicle_interfaces::msg::WheelState();
      ​​​​​​​​msg.header.priority = vehicle_interfaces::msg::Header::PRIORITY_CONTROL;
      ​​​​​​​​msg.header.device_type = vehicle_interfaces::msg::Header::DEVTYPE_STEERINGWHEEL;
      ​​​​​​​​msg.header.device_id = this->nodeName_;
      ​​​​​​​​msg.header.frame_id = frame_id++;
      ​​​​​​​​msg.header.stamp_type = vehicle_interfaces::msg::Header::STAMPTYPE_NONE_UTC_SYNC;
      ​​​​​​​​msg.header.stamp = this->get_clock()->now();
      
      ​​​​​​​​msg.gear = gear;
      ​​​​​​​​msg.steering = wheelState;
      ​​​​​​​​msg.pedal_throttle = throttle;
      ​​​​​​​​msg.pedal_brake = brake;
      ​​​​​​​​msg.pedal_clutch = clutch;
      ​​​​​​​​msg.button = btn;
      ​​​​​​​​msg.func = func;
      
      • Python3
      ​​​​​​​​from vehicle_interfaces.msg import WheelState
      
      ​​​​​​​​# The remaining code is under rclpy.Node class
      ​​​​​​​​msg = WheelState()
      ​​​​​​​​msg.header.priority = msg.header.PRIORITY_CONTROL
      ​​​​​​​​msg.header.device_type = msg.header.DEVTYPE_STEERINGWHEEL
      ​​​​​​​​msg.header.device_id = self.nodeName_
      ​​​​​​​​msg.header.frame_id = self.frame_id_# Defined in __init__
      ​​​​​​​​self.frame_id_ += 1
      ​​​​​​​​msg.header.stamp_type = msg.header.STAMPTYPE_NONE_UTC_SYNC
      ​​​​​​​​msg.header.stamp = self.get_clock().now().to_msg()
      
      ​​​​​​​​msg.gear = gear
      ​​​​​​​​msg.steering = wheelState
      ​​​​​​​​msg.pedal_throttle = throttle
      ​​​​​​​​msg.pedal_brake = brake
      ​​​​​​​​msg.pedal_clutch = clutch
      ​​​​​​​​msg.button = btn
      ​​​​​​​​msg.func = func
      

Service .srv Type (vehicle_interfaces)

  • IDServer.srv (communicate with ID server)

    • Request and response structure
    ​​​​string request_code
    ​​​​string content1
    ​​​​string content2
    ​​​​---
    ​​​​bool response
    ​​​​string content
    
    • Examples
    ​​​​// This example demonstrates requesting ID table from ID server node
    ​​​​auto clientNode = rclcpp::Node::make_shared("service_client");
    ​​​​auto client = 
    ​​​​    clientNode->create_client<vehicle_interfaces::srv::IDServer>(SERVICE_SERVER);
    ​​​​auto request = std::make_shared<vehicle_interfaces::srv::IDServer::Request>();
    ​​​​auto response = std::make_shared<vehicle_interfaces::srv::IDServer::Response>();
    ​​​​request->request_code
    
    ​​​​request->request_code = "getTable";
    ​​​​auto result = client->async_send_request(request);
    
    ​​​​if (rclcpp::spin_until_future_complete(clientNode, result) == rclcpp::FutureReturnCode::SUCCESS)
    ​​​​{
    ​​​​    RCLCPP_INFO(clientNode->get_logger(), "Response: %d\n\t%s", result.get()->response, result.get()->content.c_str());
    ​​​​}
    ​​​​else
    ​​​​{
    ​​​​    RCLCPP_ERROR(clientNode->get_logger(), "Failed to call service");
    ​​​​}
    
    ​​​​// If request succeed, the ID table will be retrieved from ID table topic which subscribed by IDTable.msg message type
    
  • SafetyReg.srv (deivce registered to safety server)

    • Request and response structure
    ​​​​# Passing device_id to safety server is necessary for control server to identify the place where emergency occurred
    ​​​​string device_id
    
    ​​​​# emergency_percentage from 0 to 1 represents emergency level from low to high
    ​​​​float32 emergency_percentage
    
    ​​​​---
    ​​​​# response is true if value accepted, otherwise server ignore the request and response false
    ​​​​bool response
    
    • Examples
    ​​​​// This example demonstrates safety server registration in a class
    
    ​​​​// SafetyNode and safety service implementation in safety.h
    ​​​​#include "vehicle_interfaces/safety.h"
    
    ​​​​// Class members
    ​​​​std::shared_ptr<Params> params_;
    ​​​​std::shared_ptr<SafetyNode> safetyNode_;
    ​​​​rclcpp::executors::SingleThreadedExecutor* safetyExec_;
    ​​​​std::thread safetyNodeTh_;
    
    ​​​​// Using SingleThreadedExecutor create thread for SafetyNode
    ​​​​this->safetyNode_ = std::make_shared<SafetyNode>(params->mainNodeName, params->service_SafetyServer_serviceName);
    ​​​​this->safetyExec_ = new rclcpp::executors::SingleThreadedExecutor();
    ​​​​this->safetyExec_->add_node(this->safetyNode_);
    ​​​​this->safetyNodeTh_ = std::thread(SpinNodeExecutor, this->safetyExec_, params->mainNodeName + "_safety");
    
    ​​​​// Set emergency value to safety server
    ​​​​this->safetyNode_->setEmergency(this->params_->mainNodeName, em);
    
  • SafetyReq.srv (deivce request emergencies from safety server)

    • Request and response structure
    ​​​​# Get device_id emergency percentage, return all emergencies if device_id set to "all"
    ​​​​string device_id
    
    ​​​​---
    ​​​​# response is true if value accepted, otherwise server ignore the request and response false
    ​​​​bool response
    
    ​​​​# Get device_id vector
    ​​​​string[] device_ids
    
    ​​​​# Get emergency_percentage vector corresponds to device_id vector
    ​​​​float32[] emergency_percentages
    
    • Examples
    ​​​​// This example demonstrates safety server requesting in a class
    
    ​​​​// SafetyNode and safety service implementation in safety.h
    ​​​​#include "vehicle_interfaces/safety.h"
    
    ​​​​// TODO: add safety server requesting demonstration
    
  • TimeSync.srv (synchronize UTC time from time server)

    • Request and response structure
    ​​​​uint8 request_code
    ​​​​builtin_interfaces/Time request_time
    ​​​​---
    ​​​​uint8 response_code
    ​​​​builtin_interfaces/Time request_time
    ​​​​builtin_interfaces/Time response_time
    
    • Examples
    ​​​​// This example demonstrates requesting UTC timestamp from time server
    ​​​​// The remaining code is under rclcpp::Node class
    
    ​​​​auto request = std::make_shared<vehicle_interfaces::srv::TimeSync::Request>();
    ​​​​request->request_code = this->timeStampType_;
    ​​​​request->request_time = this->get_clock()->now();
    ​​​​auto result = this->client_->async_send_request(request);
    ​​​​// Wait for the result.
    ​​​​if (rclcpp::spin_until_future_complete(this->clientNode_, result, 10ms) == rclcpp::FutureReturnCode::SUCCESS)
    ​​​​{
    ​​​​    rclcpp::Time nowTime = this->get_clock()->now();
    ​​​​    auto response = result.get();
    ​​​​    this->initTime_ = response->request_time;
    ​​​​    this->refTime_ = (rclcpp::Time)response->response_time - (nowTime - this->initTime_) * 0.5;
    ​​​​    this->timeStampType_ = response->response_code;
    ​​​​}
    ​​​​else
    ​​​​{
    ​​​​    RCLCPP_ERROR(this->clientNode_->get_logger(), "Failed to call service");
    ​​​​}
    

Instant Communication via Ying's ID Server (Deprecated)

For all topic message communicate with others via Ying's ID server, the only content we send are the variables in message. For the definitions such as UNIT, both devices needs the same .msg file to decode unit_type.

Ying's ID server provided 1450 bytes of payload size, including description_id, device_length and message_length which occupies 14 bytes. Considering the device name length, the conservative message length will be less then 1350 bytes.

The message can be considered as two part: header_field and data_field. The header_field has fixed size for describing the message type and timestamp. The data_field filled with message data which sizes is variant depend on different message type, such as IMU, Ultrasonic, etc..

Header Field

The header_field data size and order is reference to Header.msg. The header_field will only filled with variables from Header.msg. The header_field size is fixed at 75 bytes, including priority, device_type, device_id, stamp_type and stamp. Users can easily determine whether or not the message will be used.

Variables

# Variables from Header.msg
uint8 priority 2
uint8 device_type 0
string device_id
uint8 stamp_type 0
builtin_interfaces/Time stamp

# Header field variables
uint8 priority          (1 byte)
uint8 device_type       (1 byte)
string device_id        (64 bytes)
uint8 stamp_type        (1 byte)
double stamp_sec        (8 bytes)

Order

# Order
{header_field} = <priority><device_type><device_id><stamp_type><stamp_sec>

# Size (total 75 bytes)
{header_field} = <1><1><64><1><8>

Examples (Big-Endian Unconfirmed)

// This example shows the IMU sensor header format
// Definitions decoding refers to Header.msg
# include <cstring>
# include <string>

typedef unsigned char UCHAR;

// Variable settings from data server
uint8_t priority = 1;
uint8_t device_type = 5;
std::string device_id = "Topic_IMU";
uint8_t stamp_type = 0;
double stamp_sec = 1665981835.921912432;

// Encoding
UCHAR* stampBytes = reinterpret_cast<UCHAR*>(&stamp_sec);// 8 bytes

UCHAR header_field[75];
memset(header_field, 0, 75);
UCHAR* ptr = header_field;

*ptr++ = priority;
*ptr++ = device_type;
memcpy(ptr, device_id.c_str(), device_id.length()); ptr += 64;
*ptr++ = 0;
memcpy(ptr, stampBytes, 8); ptr += 8;
printf("buff: %s\nptr pos: %ld\n", header_field, ptr - header_field);

//Decoding
UCHAR* recvBuf = header_field;
UCHAR* ptr2 = recvBuf;
printf("priority: %d\n", *ptr2++);
printf("device_type: %d\n", *ptr2++);
std::string deviceStr(reinterpret_cast<char*>(ptr2)); ptr2 += 64;
printf("device_id: %s\n", deviceStr.c_str());
printf("stamp_type: %d\n", *ptr2++);
double *stampPtr = reinterpret_cast<double*>(ptr2); ptr2 += 8;
printf("stamp_sec: %.9f\n", *stampPtr);

Data_Field

The data_field content is variant with difference messages type. The convenient way to recognized each type of message, is reference from relative .msg file. The data_field filling and reading order must follow the .msg file from up to down, and each variables must follow the size of data type.

For the basic data type, like int or double, the data must slice and store according to Big-Endian sequence.

Convert ROS2 Message To JSON Format

The ROS2 data converts to JSON format using timestamp to group the topics messages. The number of topics and types will be variant in different timestamp moment, but the same type of topic exist same message structure, while the message order may not be the same.

Data Structure

{
    "TIMESTAMP_0": {
        "TOPIC_A": {
            "NUMERICAL": 100, 
            "STRING": "Example", 
            "NUMERICAL_ARRAY": [1, 2, 3, 4], 
            "STRING_ARRAY": ["a", "b", "c"]
        }
    }
}

Under The Topic

Our JSON format will only stores the message variables and header. The header variables will be extracted to the same hierarchy with message variables.

  • ROS2 topic message example
    ​​​​# IMU topic variables
    ​​​​vehicle_interfaces/Header header
    ​​​​uint8 unit_type 0
    ​​​​float32[4] orientation
    ​​​​float32[3] angular_velocity
    ​​​​float32[3] linear_acceleration
    
  • ROS2 topic message to JSON example
    ​​​​// IMU topic to JSON
    ​​​​"topic_IMU":{
    ​​​​    "priority": 1, 
    ​​​​    "device_type": 6, 
    ​​​​    "device_id": "imu_publisher_node", 
    ​​​​    "frame_id": 0, 
    ​​​​    "stamp_type": 0, 
    ​​​​    "stamp": 1666699999.999999, 
    ​​​​    "ref_publish_time_ms": 50.0, 
    ​​​​    "unit_type": 4, 
    ​​​​    "orientation": [0, 0, 0, 0], 
    ​​​​    "angular_velocity": [0, 0, 0], 
    ​​​​    "linear_acceleration": [0, 0, 0], 
    ​​​​    "msg_type": 5
    ​​​​}
    
    • The stamp is converted to double format
    • The msg_type is added by dataserver, not supported yet