apollo VehicleStateProvider代码分析
作者:互联网
因为很多模块都会用到本车的车辆状态,包括底盘信息和定位信息
成员函数:
Status Update(const localization::LocalizationEstimate& localization,
const canbus::Chassis& chassis);
double timestamp() const;
const localization::Pose& pose() const;
const localization::Pose& original_pose() const;
double x() const;
double y() const;
double z() const;
double kappa() const; //不太明白kappa代表什么?
double roll() const; //欧拉滚转角
double pitch() const; //欧拉俯仰角
double yaw() const; //欧拉偏航角,到目前位置,使用航向代替偏航角,航向角和东向重合,航向角和北向重合???
double heading() const; //得到车辆位置的航向,即车辆航向和x轴的夹角。
double linear_velocity() const; //车辆线速度
double angular_velocity() const; //车辆角速度,也就是横摆角速度
double linear_acceleration() const; //车辆线性加速度
double gear() const; //档位信息
double steering_percentage() const; //方向盘转角
void set_linear_velocity(const double linear_velocity); //设置车辆线速度,为什么还需要设置啊?
math::Vec2d EstimateFuturePosition(const double t) const; //从当前位置和航向,沿一段时间,通过恒定线速度,线加速度,角速度来估计未来的位置。
math::Vec2d ComputeCOMPosition(const double rear_to_com_distance) const; //给定后轮到质心的距离,计算车辆质心的位置(COM,center of mass)
const VehicleState& vehicle_state() const; //
bool ConstructExceptLinearVelocity(
const localization::LocalizationEstimate& localization);
common::VehicleState vehicle_state_;
localization::LocalizationEstimate original_localization_;
下表分别是Update()和ConstructExceptLinearVelocity()函数中分别更新的vehicle_state_状态
Update | ConstructExceptLinearVelocity |
---|---|
set_timestamp | |
set_gear | |
set_linear_velocity | |
set_steering_percentage | |
set_kappa | |
set_driving_mode | |
– | mutable_pose |
– | set_x |
– | set_y |
– | set_z |
– | set_heading |
– | set_angular_velocity |
– | set_linear_acceleration |
– | set_roll |
– | set_pitch |
– | set_yaw |
//判断是否正确收到定位信息,为什么函数的名字是Construct Except Linear Velocity(除线速度外的构造?)这个函数主要是对vehicle_state_进行赋值,
bool VehicleStateProvider::ConstructExceptLinearVelocity(
const localization::LocalizationEstimate &localization) {
if (!localization.has_pose()) {
AERROR << "Invalid localization input.";
return false;
}
// skip localization update when it is in use_navigation_mode.
if (FLAGS_use_navigation_mode) {
ADEBUG << "Skip localization update when it is in use_navigation_mode.";
return true;
}
vehicle_state_.mutable_pose()->CopyFrom(localization.pose());
if (localization.pose().has_position()) {
vehicle_state_.set_x(localization.pose().position().x());
vehicle_state_.set_y(localization.pose().position().y());
vehicle_state_.set_z(localization.pose().position().z());
}
const auto &orientation = localization.pose().orientation();
if (localization.pose().has_heading()) {
vehicle_state_.set_heading(localization.pose().heading());
} else {
vehicle_state_.set_heading(
math::QuaternionToHeading(orientation.qw(), orientation.qx(),
orientation.qy(), orientation.qz()));
}
if (FLAGS_enable_map_reference_unify) {
if (!localization.pose().has_angular_velocity_vrf()) {
AERROR << "localization.pose().has_angular_velocity_vrf() must be true "
"when FLAGS_enable_map_reference_unify is true.";
return false;
}
vehicle_state_.set_angular_velocity(
localization.pose().angular_velocity_vrf().z());
if (!localization.pose().has_linear_acceleration_vrf()) {
AERROR << "localization.pose().has_linear_acceleration_vrf() must be "
"true when FLAGS_enable_map_reference_unify is true.";
return false;
}
vehicle_state_.set_linear_acceleration(
localization.pose().linear_acceleration_vrf().y());
} else {
if (!localization.pose().has_angular_velocity()) {
AERROR << "localization.pose() has no angular velocity.";
return false;
}
vehicle_state_.set_angular_velocity(
localization.pose().angular_velocity().z());
if (!localization.pose().has_linear_acceleration()) {
AERROR << "localization.pose() has no linear acceleration.";
return false;
}
vehicle_state_.set_linear_acceleration(
localization.pose().linear_acceleration().y());
}
if (localization.pose().has_euler_angles()) {
vehicle_state_.set_roll(localization.pose().euler_angles().y());
vehicle_state_.set_pitch(localization.pose().euler_angles().x());
vehicle_state_.set_yaw(localization.pose().euler_angles().z());
} else {
math::EulerAnglesZXYd euler_angle(orientation.qw(), orientation.qx(),
orientation.qy(), orientation.qz());
vehicle_state_.set_roll(euler_angle.roll());
vehicle_state_.set_pitch(euler_angle.pitch());
vehicle_state_.set_yaw(euler_angle.yaw());
}
return true;
}
//接收底盘和定位信息,进行一些定位和底盘是否异常的判断,把定位和底盘的信息加载到vehicle_state_变量里进行存储。
Status VehicleStateProvider::Update(
const localization::LocalizationEstimate &localization,
const canbus::Chassis &chassis) {
original_localization_ = localization;
if (!ConstructExceptLinearVelocity(localization)) {
std::string msg = absl::StrCat(
"Fail to update because ConstructExceptLinearVelocity error.",
"localization:\n", localization.DebugString());
return Status(ErrorCode::LOCALIZATION_ERROR, msg);
}
if (localization.has_measurement_time()) {
vehicle_state_.set_timestamp(localization.measurement_time());
} else if (localization.header().has_timestamp_sec()) {
vehicle_state_.set_timestamp(localization.header().timestamp_sec());
} else if (chassis.has_header() && chassis.header().has_timestamp_sec()) {
AERROR << "Unable to use location timestamp for vehicle state. Use chassis "
"time instead.";
vehicle_state_.set_timestamp(chassis.header().timestamp_sec());
}
if (chassis.has_gear_location()) {
vehicle_state_.set_gear(chassis.gear_location());
} else {
vehicle_state_.set_gear(canbus::Chassis::GEAR_NONE);
}
if (chassis.has_speed_mps()) {
vehicle_state_.set_linear_velocity(chassis.speed_mps());
if (!FLAGS_reverse_heading_vehicle_state &&
vehicle_state_.gear() == canbus::Chassis::GEAR_REVERSE) {
vehicle_state_.set_linear_velocity(-vehicle_state_.linear_velocity());
}
}
if (chassis.has_steering_percentage()) {
vehicle_state_.set_steering_percentage(chassis.steering_percentage());
}
static constexpr double kEpsilon = 1e-6;
if (std::abs(vehicle_state_.linear_velocity()) < kEpsilon) {
vehicle_state_.set_kappa(0.0);
} else {
vehicle_state_.set_kappa(vehicle_state_.angular_velocity() /
vehicle_state_.linear_velocity());
}
vehicle_state_.set_driving_mode(chassis.driving_mode());
return Status::OK();
}
//利用横摆角速度计算t sec时间后的车辆横向和纵向位置,假设t=0时,当前x,y为[0,0]
//vec_distance[0]为横向t时间的运动距离,vec_distance[1]为纵向t时间的运动距离,为局部笛卡尔坐标系下的运动。x_next = v/yawrate*sin(yawrate*t),y_next = v/yawrate*(1-cos(yawrate*t)),通过几何关系很容易推导出来,最后在叠加当前车辆的x和y输出预测t时间后的x和y。
math::Vec2d VehicleStateProvider::EstimateFuturePosition(const double t) const {
Eigen::Vector3d vec_distance(0.0, 0.0, 0.0);
double v = vehicle_state_.linear_velocity();
// Predict distance travel vector
if (std::fabs(vehicle_state_.angular_velocity()) < 0.0001) {
vec_distance[0] = 0.0;
vec_distance[1] = v * t;
} else {
vec_distance[0] = -v / vehicle_state_.angular_velocity() *
(1.0 - std::cos(vehicle_state_.angular_velocity() * t));
vec_distance[1] = std::sin(vehicle_state_.angular_velocity() * t) * v /
vehicle_state_.angular_velocity();
}
// If we have rotation information, take it into consideration.
//如果我们有旋转信息?,请考虑它,,看不懂。。。
if (vehicle_state_.pose().has_orientation()) {
const auto &orientation = vehicle_state_.pose().orientation();
Eigen::Quaternion<double> quaternion(orientation.qw(), orientation.qx(),
orientation.qy(), orientation.qz());
Eigen::Vector3d pos_vec(vehicle_state_.x(), vehicle_state_.y(),
vehicle_state_.z());
const Eigen::Vector3d future_pos_3d =
quaternion.toRotationMatrix() * vec_distance + pos_vec;
return math::Vec2d(future_pos_3d[0], future_pos_3d[1]);
}
// If no valid rotation information provided from localization,
// return the estimated future position without rotation.
return math::Vec2d(vec_distance[0] + vehicle_state_.x(),
vec_distance[1] + vehicle_state_.y());
}
vehicle_state_provider.h
/******************************************************************************
* Copyright 2017 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.
*****************************************************************************/
/**
* @file vehicle_state.h
*
* @brief Declaration of the class VehicleStateProvider.
*/
#pragma once
#include <memory>
#include <string>
#include "modules/canbus/proto/chassis.pb.h"
#include "modules/common/math/box2d.h"
#include "modules/common/math/vec2d.h"
#include "modules/common/status/status.h"
#include "modules/common/vehicle_state/proto/vehicle_state.pb.h"
#include "modules/localization/proto/localization.pb.h"
/**
* @namespace apollo::common
* @brief apollo::common
*/
namespace apollo {
namespace common {
/**
* @class VehicleStateProvider
* @brief The class of vehicle state.
* It includes basic information and computation
* about the state of the vehicle.
*/
class VehicleStateProvider {
public:
/**
* @brief Constructor by information of localization and chassis.
* @param localization Localization information of the vehicle.
* @param chassis Chassis information of the vehicle.
*/
Status Update(const localization::LocalizationEstimate& localization,
const canbus::Chassis& chassis);
/**
* @brief Update VehicleStateProvider instance by protobuf files.
* @param localization_file the localization protobuf file.
* @param chassis_file The chassis protobuf file
*/
void Update(const std::string& localization_file,
const std::string& chassis_file);
double timestamp() const;
const localization::Pose& pose() const;
const localization::Pose& original_pose() const;
/**
* @brief Default destructor.
*/
virtual ~VehicleStateProvider() = default;
/**
* @brief Get the x-coordinate of vehicle position.
* @return The x-coordinate of vehicle position.
*/
double x() const;
/**
* @brief Get the y-coordinate of vehicle position.
* @return The y-coordinate of vehicle position.
*/
double y() const;
/**
* @brief Get the z coordinate of vehicle position.
* @return The z coordinate of vehicle position.
*/
double z() const;
/**
* @brief Get the kappa of vehicle position.
* the positive or negative sign is decided by the vehicle heading vector
* along the path
* @return The kappa of vehicle position.
*/
double kappa() const;
/**
* @brief Get the vehicle roll angle.
* @return The euler roll angle.
*/
double roll() const;
/**
* @brief Get the vehicle pitch angle.
* @return The euler pitch angle.
*/
double pitch() const;
/**
* @brief Get the vehicle yaw angle.
* As of now, use the heading instead of yaw angle.
* Heading angle with East as zero, yaw angle has North as zero
* @return The euler yaw angle.
*/
double yaw() const;
/**
* @brief Get the heading of vehicle position, which is the angle
* between the vehicle's heading direction and the x-axis.
* @return The angle between the vehicle's heading direction
* and the x-axis.
*/
double heading() const;
/**
* @brief Get the vehicle's linear velocity.
* @return The vehicle's linear velocity.
*/
double linear_velocity() const;
/**
* @brief Get the vehicle's angular velocity.
* @return The vehicle's angular velocity.
*/
double angular_velocity() const;
/**
* @brief Get the vehicle's linear acceleration.
* @return The vehicle's linear acceleration.
*/
double linear_acceleration() const;
/**
* @brief Get the vehicle's gear position.
* @return The vehicle's gear position.
*/
double gear() const;
/**
* @brief Get the vehicle's steering angle.
* @return double
*/
double steering_percentage() const;
/**
* @brief Set the vehicle's linear velocity.
* @param linear_velocity The value to set the vehicle's linear velocity.
*/
void set_linear_velocity(const double linear_velocity);
/**
* @brief Estimate future position from current position and heading,
* along a period of time, by constant linear velocity,
* linear acceleration, angular velocity.
* @param t The length of time period.
* @return The estimated future position in time t.
*/
math::Vec2d EstimateFuturePosition(const double t) const;
/**
* @brief Compute the position of center of mass(COM) of the vehicle,
* given the distance from rear wheels to the center of mass.
* @param rear_to_com_distance Distance from rear wheels to
* the vehicle's center of mass.
* @return The position of the vehicle's center of mass.
*/
math::Vec2d ComputeCOMPosition(const double rear_to_com_distance) const;
const VehicleState& vehicle_state() const;
private:
bool ConstructExceptLinearVelocity(
const localization::LocalizationEstimate& localization);
common::VehicleState vehicle_state_;
localization::LocalizationEstimate original_localization_;
};
} // namespace common
} // namespace apollo
标签:const,localization,double,代码,VehicleStateProvider,_.,state,vehicle,apollo 来源: https://blog.csdn.net/jch924583667/article/details/118755982