cvci挑战赛接口测试

就不具体到算法了，先看数据接收和发布部分：
首先是gps和path的msg的解析部分：
网站上的附件给出的内容是有点问题的，不过不影响：
Trajectory.msg

Header header
uint64 frame_number
bool is_valid_frame
TrajectoryInfo trajectoryinfo

TrajectoryInfo.msg

int32 path_id
float32 total_path_length
float32 total_path_time
int8 decision_type
int8 light_type
string[] lane_ids
TrajectoryPoint[] trajectorypoints

TrajectoryPoint.msg

Point2D position
float32 velocity
float32 heading
float32 curvature
float32 s
float32 t
float32 a   # 2024.2.6 add
int8 point_type

Point2D

float64 x
float64 y

这种msg套msg在ros里面还是非常常见的。
定位的msg:
PerceptionLocalization.msg

Header header

uint64 frame_unmber
int8 fusion_level
int8 status  #4:固定解，5：浮点解，2：伪距差分，1：单点解
#rotation
float64 roll      # x,R, car body (FLU)
float64 pitch      # y,R, car body (FLU)
float64 yaw      # z,R, car body (FLU) utm (0,360)  0--east, 90--north
float32 roll_std	
float32 pitch_std
float32 yaw_std 

float64 qw          # w
float64 qx          # x
float64 qy          # y
float64 qz          # z

float64 angular_velocity_x	
float64 angular_velocity_y
float64 angular_velocity_z 


#position
float64 latitude	
float64 longitude
float64 altitude
float32 latitude_std	
float32 longitude_std
float32 altitude_std

float64 position_x  # utm_east
float64 position_y  # utm_north
float64 position_z  # utm_up
float32 position_x_std	
float32 position_y_std
float32 position_z_std

float64 velocity_x  # utm_east_speed
float64 velocity_y  # utm_north_speed
float64 velocity_z  # utm_up_speed
float32 velocity_x_std	
float32 velocity_y_std
float32 velocity_z_std

float64 velocity_rx  # ins_x_speed
float64 velocity_ry  # ins_y_speed
float64 velocity_rz  # ins_z_speed
float32 velocity_rx_std	
float32 velocity_ry_std
float32 velocity_rz_std

float64 accel_x     # R, car body (RFU)
float64 accel_y     # F, car body (RFU)
float64 accel_z     # U, car body (RFU)

我们发布的msg是这个：

std_msgs/Header  header
uint32 Gear             #请求档位
float32 ThrottlePedal   #油门踏板开度
float32 BrakePedal      #制动踏板开度
float32 SteeringAngle   #方向盘转角

ok，以上接口都清楚了，那就可以开始c++了：

#include <ros/ros.h>
#include <common_msgs/Control_Test.h>
#include <perception_msgs/PerceptionLocalization.h>
#include <perception_msgs/Trajectory.h>

void CallbackGps(const perception_msgs::PerceptionLocalization::ConstPtr& msg)
{
    // Header information
    std_msgs::Header header = msg->header;
    uint64_t frame_unmber = msg->frame_unmber;
    int8_t fusion_level = msg->fusion_level;
    int8_t status = msg->status;

    // Rotation information
    double roll = msg->roll;
    double pitch = msg->pitch;
    double yaw = msg->yaw;
    float roll_std = msg->roll_std;
    float pitch_std = msg->pitch_std;
    float yaw_std = msg->yaw_std;
    double qw = msg->qw;
    double qx = msg->qx;
    double qy = msg->qy;
    double qz = msg->qz;
    double angular_velocity_x = msg->angular_velocity_x;
    double angular_velocity_y = msg->angular_velocity_y;
    double angular_velocity_z = msg->angular_velocity_z;

    // Position information
    double latitude = msg->latitude;
    double longitude = msg->longitude;
    double altitude = msg->altitude;
    float latitude_std = msg->latitude_std;
    float longitude_std = msg->longitude_std;
    float altitude_std = msg->altitude_std;
    double position_x = msg->position_x;
    double position_y = msg->position_y;
    double position_z = msg->position_z;
    float position_x_std = msg->position_x_std;
    float position_y_std = msg->position_y_std;
    float position_z_std = msg->position_z_std;

    // Velocity information
    double velocity_x = msg->velocity_x;
    double velocity_y = msg->velocity_y;
    double velocity_z = msg->velocity_z;
    float velocity_x_std = msg->velocity_x_std;
    float velocity_y_std = msg->velocity_y_std;
    float velocity_z_std = msg->velocity_z_std;
    double velocity_rx = msg->velocity_rx;
    double velocity_ry = msg->velocity_ry;
    double velocity_rz = msg->velocity_rz;
    float velocity_rx_std = msg->velocity_rx_std;
    float velocity_ry_std = msg->velocity_ry_std;
    float velocity_rz_std = msg->velocity_rz_std;

    // Acceleration information
    double accel_x = msg->accel_x;
    double accel_y = msg->accel_y;
    double accel_z = msg->accel_z;

    // Print the received data
    ROS_INFO("Received GPS data");
    // ROS_INFO("  Frame number: %lu", frame_number);
    // ROS_INFO("  Fusion level: %d", fusion_level);
    // ROS_INFO("  Status: %d", status);
    // ROS_INFO("  Rotation: roll=%.2f, pitch=%.2f, yaw=%.2f", roll, pitch, yaw);
    // ROS_INFO("  Position: x=%.2f, y=%.2f, z=%.2f", position_x, position_y, position_z);
    // ROS_INFO("  Velocity: x=%.2f, y=%.2f, z=%.2f", velocity_x, velocity_y, velocity_z);
    // ROS_INFO("  Acceleration: x=%.2f, y=%.2f, z=%.2f", accel_x, accel_y, accel_z);
}


void CallbackPath(const perception_msgs::Trajectory::ConstPtr& msg)
{
    // Extract header information
    std_msgs::Header header = msg->header;
    uint64_t frame_number = msg->frame_number;
    bool is_valid_frame = msg->is_valid_frame;
    // Extract TrajectoryInfo
    const perception_msgs::TrajectoryInfo& trajectory_info = msg->trajectoryinfo;
    int32_t path_id = trajectory_info.path_id;
    float total_path_length = trajectory_info.total_path_length;
    float total_path_time = trajectory_info.total_path_time;
    int8_t decision_type = trajectory_info.decision_type;
    int8_t light_type = trajectory_info.light_type;
    const std::vector<std::string>& lane_ids = trajectory_info.lane_ids;

    // Extract TrajectoryPoints
    const std::vector<perception_msgs::TrajectoryPoint>& trajectory_points = trajectory_info.trajectorypoints;
    for (const auto& point : trajectory_points)
    {
        const perception_msgs::Point2D& position = point.position;
        float velocity = point.velocity;
        float heading = point.heading;
        float curvature = point.curvature;
        float s = point.s;
        float a = point.a;
        float t = point.t;
        int8_t point_type = point.point_type;
        ROS_INFO("Received Path data");

    //     // Do something with the extracted data
    //     ROS_INFO("Trajectory point: (%.2f, %.2f), velocity: %.2f, heading: %.2f", position.x, position.y, velocity, heading);
    }

    // // Do something with the extracted data
    // ROS_INFO("Frame number: %lu, valid frame: %s, path ID: %d, total path length: %.2f, total path time: %.2f",
    //          frame_number, is_valid_frame ? "true" : "false", path_id, total_path_length, total_path_time);
}


int main(int argc, char *argv[])
{
    ros::init(argc, argv, "control_node");
    ros::NodeHandle nh;

    ros::Publisher pub = nh.advertise<common_msgs::Control_Test>("/control_test", 10);
    ros::Subscriber sub_gps = nh.subscribe("/cicv_location", 10, CallbackGps);
    ros::Subscriber sub_traj = nh.subscribe("/cicv_amr_trajectory", 10, CallbackPath);

    common_msgs::Control_Test ctrl_msg;
    ros::Rate looprate(100);
    while (ros::ok())
    {
        ros::spinOnce();
        ctrl_msg.header.stamp = ros::Time::now();
        ctrl_msg.Gear = 4; // 1-P 2-R 3-N 4-D
        ctrl_msg.ThrottlePedal = 0.2; // 0-1
        ctrl_msg.BrakePedal = 0; // 0-1
        ctrl_msg.SteeringAngle = 0; // right:-540~left:540 
        pub.publish(ctrl_msg);
        looprate.sleep();
    }
    ros::shutdown();
    return 0;
}

我本来还是想用类的，但是在功能还没有开始写的当下，就用现在这个吧。