【车路协同】数据通讯格式

1 思路

实际还是ros消息，或者ros消息的tcp、udp版本。
参考2021世界智能驾驶挑战赛

2 感知信息

obstacle

std_msgs/Header header
string ns # Namespace to place this obstacle in
int32 id # Unique number for every obstacle in the same namespace

string label # Class name of obstacle

geometry_msgs/Pose pose
geometry_msgs/Vector3 scale

bool v_validity # If v_validity is false, vx & vy & vz are not credible.
float64 vx
float64 vy
float64 vz

bool a_validity # If a_validity is false, ax & ay & az are not credible.
float64 ax
float64 ay
float64 az

obstacles

Obstacle[] obstacles

关键数据

perception msgs

产生一个空的obs

perception_msgs::Obstacle make_one_empty_Obstacle()
{
  perception_msgs::Obstacle t;
  t.header.frame_id = "fake";
  t.header.stamp = ros::Time::now();
  t.ns="obstacle";
  t.id=0;

  t.label="obstacle";

  // 设置标记位姿
  t.pose.position.x = 0;
  t.pose.position.y = 0;
  t.pose.position.z = 0;
  t.pose.orientation.x = 0;
  t.pose.orientation.y = 0;
  t.pose.orientation.z = 0;
  t.pose.orientation.w = 0;

  // 设置标记尺寸
  t.scale.x = 0;
  t.scale.y = 0;
  t.scale.z = 0;

  t.v_validity=0;
  t.vx=0;
  t.vy=0;
  t.vz=0;

  t.a_validity=0;
  t.ax=0;
  t.ay=0;
  t.az=0;
  return t;
}

2 产生一个空obs array
perception_msgs::ObstacleArray make_one_empty_ObstacleArray()
{
perception_msgs::ObstacleArray t;
}

3 赋值

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++obs
      
      perception_msgs::Obstacle obs;
      obs=make_one_empty_Obstacle();
      // 设置命名空间和ID，ID应该是独一无二的
      obs.ns = "obstacle";
      obs.id = 10002;

      // 设置位姿
      obs.pose.position.x = event_x-obu_x;
      obs.pose.position.y = event_y-obu_y;
      double tmp_x=obs.pose.position.x;
      double tmp_y=obs.pose.position.y;
      obs.pose.position.x = tmp_x * cos(-obu_hd) - tmp_y * sin(-obu_hd);
      obs.pose.position.y = tmp_x * sin(-obu_hd) + tmp_y * cos(-obu_hd);
      obs.pose.position.z = 0;
      obs.pose.orientation.x = 0;
      obs.pose.orientation.y = 0;

      double obs_angle_hd=o_angle*3.1415926/180;
      obs.pose.orientation.z = sin(0.5*(obs_angle_hd - obu_hd));
      obs.pose.orientation.w = cos(0.5*(obs_angle_hd - obu_hd));

      // 设置尺寸
      obs.scale.x = o_scale_x;
      obs.scale.y = o_scale_y;
      obs.scale.z = o_scale_z;
      //
      o=obs;
      o_add_flag=1;
      o_time=ros::Time::now().toSec();
      //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++obs

3 可视化

产生一个空的marker
visualization_msgs::MarkerArray obu_to_xy::make_one_empty_markerarray()
{
  visualization_msgs::MarkerArray t;
  return t;
}
产生一个空的marker array
visualization_msgs::Marker obu_to_xy::make_one_empty_marker()
{
  visualization_msgs::Marker t;
  t.header.frame_id = "fake";
  t.header.stamp = ros::Time::now();
  // Set the namespace and id for this marker.  This serves to create a unique ID
  // Any marker sent with the same namespace and id will overwrite the old one
  t.ns = "obu_marker";
  t.id = 0;
  // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
  t.type = visualization_msgs::Marker::CUBE;
  // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
  t.action = visualization_msgs::Marker::ADD;
  // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
  t.pose.position.x = 0;
  t.pose.position.y = 0;
  t.pose.position.z = 0;
  t.pose.orientation.x = 0.0;
  t.pose.orientation.y = 0.0;
  t.pose.orientation.z = 0.0;
  t.pose.orientation.w = 0.0;
  // Set the scale of the marker -- 1x1x1 here means 1m on a side
  t.scale.x = 0.0;
  t.scale.y = 0.0;
  t.scale.z = 0.0;
  // Set the color -- be sure to set alpha to something non-zero!
  t.color.r = 0.0f;
  t.color.g = 1.0f;
  t.color.b = 0.0f;
  t.color.a = 1.0;
  t.lifetime = ros::Duration(1);
  return t;
}