Apollo 10.0
自动驾驶开放平台
apollo::common::VehicleStateProvider类 参考

The class of vehicle state. 更多...

#include <vehicle_state_provider.h>

apollo::common::VehicleStateProvider 的协作图:

Public 成员函数

Status Update (const localization::LocalizationEstimate &localization, const canbus::Chassis &chassis)
 Constructor by information of localization and chassis.
 
void Update (const std::string &localization_file, const std::string &chassis_file)
 Update VehicleStateProvider instance by protobuf files.
 
double timestamp () const
 
const localization::Posepose () const
 
const localization::Poseoriginal_pose () const
 
virtual ~VehicleStateProvider ()=default
 Default destructor.
 
double x () const
 Get the x-coordinate of vehicle position.
 
double y () const
 Get the y-coordinate of vehicle position.
 
double z () const
 Get the z coordinate of vehicle position.
 
double kappa () const
 Get the kappa of vehicle position.
 
double roll () const
 Get the vehicle roll angle.
 
double pitch () const
 Get the vehicle pitch angle.
 
double yaw () const
 Get the vehicle yaw angle.
 
double heading () const
 Get the heading of vehicle position, which is the angle between the vehicle's heading direction and the x-axis.
 
double linear_velocity () const
 Get the vehicle's linear velocity.
 
double angular_velocity () const
 Get the vehicle's angular velocity.
 
double linear_acceleration () const
 Get the vehicle's linear acceleration.
 
double gear () const
 Get the vehicle's gear position.
 
double steering_percentage () const
 Get the vehicle's steering angle.
 
void set_linear_velocity (const double linear_velocity)
 Set the vehicle's linear velocity.
 
math::Vec2d EstimateFuturePosition (const double t) const
 Estimate future position from current position and heading, along a period of time, by constant linear velocity, linear acceleration, angular velocity.
 
math::Vec2d ComputeCOMPosition (const double rear_to_com_distance) const
 Compute the position of center of mass(COM) of the vehicle, given the distance from rear wheels to the center of mass.
 
const VehicleStatevehicle_state () const
 

详细描述

The class of vehicle state.

It includes basic information and computation about the state of the vehicle.

在文件 vehicle_state_provider.h46 行定义.

构造及析构函数说明

◆ ~VehicleStateProvider()

virtual apollo::common::VehicleStateProvider::~VehicleStateProvider ( )
virtualdefault

Default destructor.

成员函数说明

◆ angular_velocity()

double apollo::common::VehicleStateProvider::angular_velocity ( ) const

Get the vehicle's angular velocity.

返回
The vehicle's angular velocity.

在文件 vehicle_state_provider.cc181 行定义.

181 {
182 return vehicle_state_.angular_velocity();
183}

◆ ComputeCOMPosition()

math::Vec2d apollo::common::VehicleStateProvider::ComputeCOMPosition ( const double  rear_to_com_distance) const

Compute the position of center of mass(COM) of the vehicle, given the distance from rear wheels to the center of mass.

参数
rear_to_com_distanceDistance from rear wheels to the vehicle's center of mass.
返回
The position of the vehicle's center of mass.

在文件 vehicle_state_provider.cc247 行定义.

248 {
249 // set length as distance between rear wheel and center of mass.
250 Eigen::Vector3d v;
251 if ((FLAGS_state_transform_to_com_reverse &&
252 vehicle_state_.gear() == canbus::Chassis::GEAR_REVERSE) ||
253 (FLAGS_state_transform_to_com_drive &&
254 vehicle_state_.gear() == canbus::Chassis::GEAR_DRIVE)) {
255 v << 0.0, rear_to_com_distance, 0.0;
256 } else {
257 v << 0.0, 0.0, 0.0;
258 }
259 Eigen::Vector3d pos_vec(vehicle_state_.x(), vehicle_state_.y(),
260 vehicle_state_.z());
261 // Initialize the COM position without rotation
262 Eigen::Vector3d com_pos_3d = v + pos_vec;
263
264 // If we have rotation information, take it into consideration.
265 if (vehicle_state_.pose().has_orientation()) {
266 const auto &orientation = vehicle_state_.pose().orientation();
267 Eigen::Quaternion<double> quaternion(orientation.qw(), orientation.qx(),
268 orientation.qy(), orientation.qz());
269 // Update the COM position with rotation
270 com_pos_3d = quaternion.toRotationMatrix() * v + pos_vec;
271 }
272 return math::Vec2d(com_pos_3d[0], com_pos_3d[1]);
273}
optional apollo::localization::Pose pose
optional apollo::canbus::Chassis::GearPosition gear
optional apollo::common::Quaternion orientation
Definition pose.proto:31

◆ EstimateFuturePosition()

math::Vec2d apollo::common::VehicleStateProvider::EstimateFuturePosition ( const double  t) const

Estimate future position from current position and heading, along a period of time, by constant linear velocity, linear acceleration, angular velocity.

参数
tThe length of time period.
返回
The estimated future position in time t.

在文件 vehicle_state_provider.cc215 行定义.

215 {
216 Eigen::Vector3d vec_distance(0.0, 0.0, 0.0);
217 double v = vehicle_state_.linear_velocity();
218 // Predict distance travel vector
219 if (std::fabs(vehicle_state_.angular_velocity()) < 0.0001) {
220 vec_distance[0] = 0.0;
221 vec_distance[1] = v * t;
222 } else {
223 vec_distance[0] = -v / vehicle_state_.angular_velocity() *
224 (1.0 - std::cos(vehicle_state_.angular_velocity() * t));
225 vec_distance[1] = std::sin(vehicle_state_.angular_velocity() * t) * v /
226 vehicle_state_.angular_velocity();
227 }
228
229 // If we have rotation information, take it into consideration.
230 if (vehicle_state_.pose().has_orientation()) {
231 const auto &orientation = vehicle_state_.pose().orientation();
232 Eigen::Quaternion<double> quaternion(orientation.qw(), orientation.qx(),
233 orientation.qy(), orientation.qz());
234 Eigen::Vector3d pos_vec(vehicle_state_.x(), vehicle_state_.y(),
235 vehicle_state_.z());
236 const Eigen::Vector3d future_pos_3d =
237 quaternion.toRotationMatrix() * vec_distance + pos_vec;
238 return math::Vec2d(future_pos_3d[0], future_pos_3d[1]);
239 }
240
241 // If no valid rotation information provided from localization,
242 // return the estimated future position without rotation.
243 return math::Vec2d(vec_distance[0] + vehicle_state_.x(),
244 vec_distance[1] + vehicle_state_.y());
245}

◆ gear()

double apollo::common::VehicleStateProvider::gear ( ) const

Get the vehicle's gear position.

返回
The vehicle's gear position.

在文件 vehicle_state_provider.cc189 行定义.

189{ return vehicle_state_.gear(); }

◆ heading()

double apollo::common::VehicleStateProvider::heading ( ) const

Get the heading of vehicle position, which is the angle between the vehicle's heading direction and the x-axis.

返回
The angle between the vehicle's heading direction and the x-axis.

在文件 vehicle_state_provider.cc171 行定义.

171 {
172 return vehicle_state_.heading();
173}

◆ kappa()

double apollo::common::VehicleStateProvider::kappa ( ) const

Get the kappa of vehicle position.

the positive or negative sign is decided by the vehicle heading vector along the path

返回
The kappa of vehicle position.

在文件 vehicle_state_provider.cc175 行定义.

175{ return vehicle_state_.kappa(); }

◆ linear_acceleration()

double apollo::common::VehicleStateProvider::linear_acceleration ( ) const

Get the vehicle's linear acceleration.

返回
The vehicle's linear acceleration.

在文件 vehicle_state_provider.cc185 行定义.

185 {
186 return vehicle_state_.linear_acceleration();
187}

◆ linear_velocity()

double apollo::common::VehicleStateProvider::linear_velocity ( ) const

Get the vehicle's linear velocity.

返回
The vehicle's linear velocity.

在文件 vehicle_state_provider.cc177 行定义.

177 {
178 return vehicle_state_.linear_velocity();
179}

◆ original_pose()

const localization::Pose & apollo::common::VehicleStateProvider::original_pose ( ) const

在文件 vehicle_state_provider.cc203 行定义.

203 {
204 return original_localization_.pose();
205}
optional apollo::localization::Pose pose

◆ pitch()

double apollo::common::VehicleStateProvider::pitch ( ) const

Get the vehicle pitch angle.

返回
The euler pitch angle.

在文件 vehicle_state_provider.cc167 行定义.

167{ return vehicle_state_.pitch(); }

◆ pose()

const localization::Pose & apollo::common::VehicleStateProvider::pose ( ) const

在文件 vehicle_state_provider.cc199 行定义.

199 {
200 return vehicle_state_.pose();
201}

◆ roll()

double apollo::common::VehicleStateProvider::roll ( ) const

Get the vehicle roll angle.

返回
The euler roll angle.

在文件 vehicle_state_provider.cc165 行定义.

165{ return vehicle_state_.roll(); }

◆ set_linear_velocity()

void apollo::common::VehicleStateProvider::set_linear_velocity ( const double  linear_velocity)

Set the vehicle's linear velocity.

参数
linear_velocityThe value to set the vehicle's linear velocity.

在文件 vehicle_state_provider.cc207 行定义.

207 {
208 vehicle_state_.set_linear_velocity(linear_velocity);
209}
double linear_velocity() const
Get the vehicle's linear velocity.

◆ steering_percentage()

double apollo::common::VehicleStateProvider::steering_percentage ( ) const

Get the vehicle's steering angle.

返回
double

在文件 vehicle_state_provider.cc191 行定义.

191 {
192 return vehicle_state_.steering_percentage();
193}

◆ timestamp()

double apollo::common::VehicleStateProvider::timestamp ( ) const

在文件 vehicle_state_provider.cc195 行定义.

195 {
196 return vehicle_state_.timestamp();
197}

◆ Update() [1/2]

Status apollo::common::VehicleStateProvider::Update ( const localization::LocalizationEstimate localization,
const canbus::Chassis chassis 
)

Constructor by information of localization and chassis.

参数
localizationLocalization information of the vehicle.
chassisChassis information of the vehicle.

在文件 vehicle_state_provider.cc31 行定义.

33 {
34 original_localization_ = localization;
35 if (!ConstructExceptLinearVelocity(localization)) {
36 std::string msg = absl::StrCat(
37 "Fail to update because ConstructExceptLinearVelocity error.",
38 "localization:\n", localization.DebugString());
40 }
41 if (localization.has_measurement_time()) {
42 vehicle_state_.set_timestamp(localization.measurement_time());
43 } else if (localization.header().has_timestamp_sec()) {
44 vehicle_state_.set_timestamp(localization.header().timestamp_sec());
45 } else if (chassis.has_header() && chassis.header().has_timestamp_sec()) {
46 AERROR << "Unable to use location timestamp for vehicle state. Use chassis "
47 "time instead.";
48 vehicle_state_.set_timestamp(chassis.header().timestamp_sec());
49 }
50
51 if (chassis.has_gear_location()) {
52 vehicle_state_.set_gear(chassis.gear_location());
53 } else {
54 vehicle_state_.set_gear(canbus::Chassis::GEAR_NONE);
55 }
56
57 if (chassis.has_speed_mps()) {
58 vehicle_state_.set_linear_velocity(chassis.speed_mps());
59 if (!FLAGS_reverse_heading_vehicle_state &&
60 vehicle_state_.gear() == canbus::Chassis::GEAR_REVERSE) {
61 vehicle_state_.set_linear_velocity(-vehicle_state_.linear_velocity());
62 }
63 }
64
65 if (chassis.has_steering_percentage()) {
66 vehicle_state_.set_steering_percentage(chassis.steering_percentage());
67 }
68
69 static constexpr double kEpsilon = 0.1;
70 if (std::abs(vehicle_state_.linear_velocity()) < kEpsilon) {
71 vehicle_state_.set_kappa(0.0);
72 } else {
73 vehicle_state_.set_kappa(vehicle_state_.angular_velocity() /
74 vehicle_state_.linear_velocity());
75 }
76
77 vehicle_state_.set_driving_mode(chassis.driving_mode());
78
79 return Status::OK();
80}
static Status OK()
generate a success status.
Definition status.h:60
#define AERROR
Definition log.h:44

◆ Update() [2/2]

void apollo::common::VehicleStateProvider::Update ( const std::string &  localization_file,
const std::string &  chassis_file 
)

Update VehicleStateProvider instance by protobuf files.

参数
localization_filethe localization protobuf file.
chassis_fileThe chassis protobuf file

◆ vehicle_state()

const VehicleState & apollo::common::VehicleStateProvider::vehicle_state ( ) const

在文件 vehicle_state_provider.cc211 行定义.

211 {
212 return vehicle_state_;
213}

◆ x()

double apollo::common::VehicleStateProvider::x ( ) const

Get the x-coordinate of vehicle position.

返回
The x-coordinate of vehicle position.

在文件 vehicle_state_provider.cc159 行定义.

159{ return vehicle_state_.x(); }

◆ y()

double apollo::common::VehicleStateProvider::y ( ) const

Get the y-coordinate of vehicle position.

返回
The y-coordinate of vehicle position.

在文件 vehicle_state_provider.cc161 行定义.

161{ return vehicle_state_.y(); }

◆ yaw()

double apollo::common::VehicleStateProvider::yaw ( ) const

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

返回
The euler yaw angle.

在文件 vehicle_state_provider.cc169 行定义.

169{ return vehicle_state_.yaw(); }

◆ z()

double apollo::common::VehicleStateProvider::z ( ) const

Get the z coordinate of vehicle position.

返回
The z coordinate of vehicle position.

在文件 vehicle_state_provider.cc163 行定义.

163{ return vehicle_state_.z(); }

该类的文档由以下文件生成: