Updated: 2023/07/22
Header.msg (the header for all message type in vehicle_interfaces)
Control
, Sensor
and Info
ββββ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
ββββ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
ββββββββ#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();
ββββββββ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)
ββββuint8 UNIT_MICRO = 0
ββββuint8 UNIT_MILLI = 1
ββββuint8 UNIT_CENTI = 2
ββββuint8 UNIT_METER = 3
ββββvehicle_interfaces/Header header
ββββuint8 unit_type 1
ββββfloat32 min
ββββfloat32 max
ββββfloat32 distance
ββββββββ#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;
ββββββββ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)
ββββuint8 UNIT_TEMP_CELSIUS = 0
ββββuint8 UNIT_TEMP_FAHRENHEIT = 1
ββββuint8 UNIT_PRESS_MBAR = 0
ββββuint8 UNIT_PRESS_HPA = 4
ββββvehicle_interfaces/Header header
ββββuint8 unit_type 0
ββββfloat32 temperature
ββββfloat32 relative_humidity
ββββfloat32 pressure
ββββββββ#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;
ββββββββ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)
ββββuint8 GPS_NONE = 0
ββββuint8 GPS_STABLE = 1
ββββuint8 GPS_RTK = 2
ββββvehicle_interfaces/Header header
ββββuint8 gps_status 0
ββββfloat32 latitude 0
ββββfloat32 longitude 0
ββββββββ#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;
ββββββββ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)
ββββvehicle_interfaces/Header header
ββββstring[] idtable
ββββββββ#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>
ββββββββ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)
ββββuint8 UNIT_ROT_RAD = 0
ββββuint8 UNIT_ROT_DPS = 1
ββββuint8 UNIT_ACC_GS = 0
ββββuint8 UNIT_ACC_MS2 = 4
ββββvehicle_interfaces/Header header
ββββuint8 unit_type 0
ββββfloat32[4] orientation
ββββfloat32[3] angular_velocity
ββββfloat32[3] linear_acceleration
ββββββββ#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;
ββββββββ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)
ββββvehicle_interfaces/Header header
ββββint8 dir
ββββfloat32 pwm
ββββint8 parking
ββββββββ#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;
ββββββββ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)
ββββuint8 UNIT_ANG = 0
ββββuint8 UNIT_DIST = 1
ββββvehicle_interfaces/Header header
ββββuint8 unit_type 0
ββββfloat32 min
ββββfloat32 max
ββββfloat32 center
ββββfloat32 value
ββββββββ#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;
ββββββββ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)
ββββuint8 GEAR_PARK = 0
ββββuint8 GEAR_REVERSE = 1
ββββuint8 GEAR_NEUTRAL = 2
ββββuint8 GEAR_DRIVE = 3
ββββ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
ββββββββ#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;
ββββββββ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
IDServer.srv (communicate with ID server)
ββββstring request_code
ββββstring content1
ββββstring content2
ββββ---
ββββbool response
ββββstring content
ββββ// 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)
ββββ# 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
ββββ// 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)
ββββ# 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
ββββ// 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)
ββββuint8 request_code
ββββbuiltin_interfaces/Time request_time
ββββ---
ββββuint8 response_code
ββββbuiltin_interfaces/Time request_time
ββββbuiltin_interfaces/Time response_time
ββββ// 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");
ββββ}
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..
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 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
{header_field} = <priority><device_type><device_id><stamp_type><stamp_sec>
# Size (total 75 bytes)
{header_field} = <1><1><64><1><8>
// 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);
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.
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.
{
"TIMESTAMP_0": {
"TOPIC_A": {
"NUMERICAL": 100,
"STRING": "Example",
"NUMERICAL_ARRAY": [1, 2, 3, 4],
"STRING_ARRAY": ["a", "b", "c"]
}
}
}
Our JSON format will only stores the message variables and header. The header variables will be extracted to the same hierarchy with message variables.
ββββ# IMU topic variables
ββββvehicle_interfaces/Header header
ββββuint8 unit_type 0
ββββfloat32[4] orientation
ββββfloat32[3] angular_velocity
ββββfloat32[3] linear_acceleration
ββββ// 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
ββββ}
stamp
is converted to double formatmsg_type
is added by dataserver, not supported yet