common.h

浏览该文件的文档.

/******************************************************************************
 * Copyright 2023 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
 
#pragma once
 
#include <limits>
 
#include "modules/common_msgs/perception_msgs/perception_obstacle.pb.h"
 
#include "modules/perception/common/base/object.h"
 
namespace apollo {
namespace perception {
 
bool ConvertObjectToPb(const base::ObjectPtr &object_ptr,
                       PerceptionObstacle *pb_msg) {
  if (object_ptr == nullptr || pb_msg == nullptr) {
    return false;
  }
 
  pb_msg->set_id(object_ptr->track_id);
  pb_msg->set_theta(object_ptr->theta);
 
  apollo::common::Point3D *obj_center = pb_msg->mutable_position();
  obj_center->set_x(object_ptr->center(0));
  obj_center->set_y(object_ptr->center(1));
  obj_center->set_z(object_ptr->center(2));
 
  apollo::common::Point3D *obj_velocity = pb_msg->mutable_velocity();
  obj_velocity->set_x(object_ptr->velocity(0));
  obj_velocity->set_y(object_ptr->velocity(1));
  obj_velocity->set_z(object_ptr->velocity(2));
 
  apollo::common::Point3D *obj_acceleration = pb_msg->mutable_acceleration();
  obj_acceleration->set_x(object_ptr->acceleration(0));
  obj_acceleration->set_y(object_ptr->acceleration(1));
  obj_acceleration->set_z(object_ptr->acceleration(2));
 
  pb_msg->set_length(object_ptr->size(0));
  pb_msg->set_width(object_ptr->size(1));
  pb_msg->set_height(object_ptr->size(2));
 
  for (size_t i = 0; i < object_ptr->polygon.size(); ++i) {
    auto &pt = object_ptr->polygon.at(i);
    apollo::common::Point3D *p = pb_msg->add_polygon_point();
    p->set_x(pt.x);
    p->set_y(pt.y);
    p->set_z(pt.z);
  }
 
  for (auto &point : object_ptr->lidar_supplement.cloud.points()) {
    pb_msg->add_point_cloud(point.x);
    pb_msg->add_point_cloud(point.y);
    pb_msg->add_point_cloud(point.z);
  }
 
  apollo::common::Point3D *obj_anchor_point = pb_msg->mutable_anchor_point();
  obj_anchor_point->set_x(object_ptr->anchor_point(0));
  obj_anchor_point->set_y(object_ptr->anchor_point(1));
  obj_anchor_point->set_z(object_ptr->anchor_point(2));
 
  BBox2D *obj_bbox2d = pb_msg->mutable_bbox2d();
  const base::BBox2DF &box = object_ptr->camera_supplement.box;
  obj_bbox2d->set_xmin(box.xmin);
  obj_bbox2d->set_ymin(box.ymin);
  obj_bbox2d->set_xmax(box.xmax);
  obj_bbox2d->set_ymax(box.ymax);
 
  for (size_t i = 0; i < 3; i++) {
    for (size_t j = 0; j < 3; j++) {
      pb_msg->add_position_covariance(object_ptr->center_uncertainty(i, j));
      pb_msg->add_velocity_covariance(object_ptr->velocity_uncertainty(i, j));
      pb_msg->add_acceleration_covariance(
          object_ptr->acceleration_uncertainty(i, j));
    }
  }
 
  pb_msg->set_tracking_time(object_ptr->tracking_time);
  pb_msg->set_type(static_cast<PerceptionObstacle::Type>(object_ptr->type));
  pb_msg->set_sub_type(
      static_cast<PerceptionObstacle::SubType>(object_ptr->sub_type));
  pb_msg->set_timestamp(object_ptr->latest_tracked_time);  // in seconds.
  pb_msg->set_semantic_type(static_cast<PerceptionObstacle::SemanticType>(
      object_ptr->lidar_supplement.semantic_type));
 
  constexpr float kFloatMax = std::numeric_limits<float>::max();
  if (object_ptr->lidar_supplement.height_above_ground != kFloatMax) {
    pb_msg->set_height_above_ground(
        object_ptr->lidar_supplement.height_above_ground);
  } else {
    pb_msg->set_height_above_ground(std::numeric_limits<double>::quiet_NaN());
  }
 
  if (object_ptr->type == base::ObjectType::VEHICLE) {
    LightStatus *light_status = pb_msg->mutable_light_status();
    const base::CarLight &car_light = object_ptr->car_light;
    light_status->set_brake_visible(car_light.brake_visible);
    light_status->set_brake_switch_on(car_light.brake_switch_on);
 
    light_status->set_left_turn_visible(car_light.left_turn_visible);
    light_status->set_left_turn_switch_on(car_light.left_turn_switch_on);
 
    light_status->set_right_turn_visible(car_light.right_turn_visible);
    light_status->set_right_turn_switch_on(car_light.right_turn_switch_on);
  }
 
  if (object_ptr->fusion_supplement.on_use) {
    for (const auto &measurement : object_ptr->fusion_supplement.measurements) {
      SensorMeasurement *pb_measurement = pb_msg->add_measurements();
      pb_measurement->set_sensor_id(measurement.sensor_id);
      pb_measurement->set_id(measurement.track_id);
 
      apollo::common::Point3D *pb_position = pb_measurement->mutable_position();
      pb_position->set_x(measurement.center(0));
      pb_position->set_y(measurement.center(1));
      pb_position->set_z(measurement.center(2));
 
      pb_measurement->set_theta(measurement.theta);
      pb_measurement->set_length(measurement.size(0));
      pb_measurement->set_width(measurement.size(1));
      pb_measurement->set_height(measurement.size(2));
 
      apollo::common::Point3D *pb_velocity = pb_measurement->mutable_velocity();
      pb_velocity->set_x(measurement.velocity(0));
      pb_velocity->set_y(measurement.velocity(1));
      pb_velocity->set_z(measurement.velocity(2));
 
      pb_measurement->set_type(
          static_cast<PerceptionObstacle::Type>(measurement.type));
      // pb_measurement->set_sub_type();
      pb_measurement->set_timestamp(measurement.timestamp);
 
      BBox2D *pb_box = pb_measurement->mutable_box();
      pb_box->set_xmin(measurement.box.xmin);
      pb_box->set_ymin(measurement.box.ymin);
      pb_box->set_xmax(measurement.box.xmax);
      pb_box->set_ymax(measurement.box.ymax);
    }
  }
 
  return true;
}
 
}  // namespace perception
}  // namespace apollo