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::Pose &	pose () const

const localization::Pose &	original_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 VehicleState &	vehicle_state () const

详细描述

The class of vehicle state.

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

在文件 vehicle_state_provider.h 第 46 行定义.

构造及析构函数说明

◆ ~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.cc 第 181 行定义.

                                                    {
  return vehicle_state_.angular_velocity();
}

◆ 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_distance Distance from rear wheels to the vehicle's center of mass.

返回: The position of the vehicle's center of mass.

在文件 vehicle_state_provider.cc 第 247 行定义.

                                             {
  // set length as distance between rear wheel and center of mass.
  Eigen::Vector3d v;
  if ((FLAGS_state_transform_to_com_reverse &&
       vehicle_state_.gear() == canbus::Chassis::GEAR_REVERSE) ||
      (FLAGS_state_transform_to_com_drive &&
       vehicle_state_.gear() == canbus::Chassis::GEAR_DRIVE)) {
    v << 0.0, rear_to_com_distance, 0.0;
  } else {
    v << 0.0, 0.0, 0.0;
  }
  Eigen::Vector3d pos_vec(vehicle_state_.x(), vehicle_state_.y(),
                          vehicle_state_.z());
  // Initialize the COM position without rotation
  Eigen::Vector3d com_pos_3d = v + pos_vec;
 
  // 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());
    // Update the COM position with rotation
    com_pos_3d = quaternion.toRotationMatrix() * v + pos_vec;
  }
  return math::Vec2d(com_pos_3d[0], com_pos_3d[1]);
}

◆ 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.

参数

t	The length of time period.

返回: The estimated future position in time t.

在文件 vehicle_state_provider.cc 第 215 行定义.

                                                                           {
  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());
}

◆ gear()

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

Get the vehicle's gear position.

返回: The vehicle's gear position.

在文件 vehicle_state_provider.cc 第 189 行定义.

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.cc 第 171 行定义.

                                           {
  return vehicle_state_.heading();
}

◆ 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.cc 第 175 行定义.

175{ return vehicle_state_.kappa(); }

apollo::common::VehicleState::kappa

optional double kappa

Definition vehicle_state.proto:17

◆ linear_acceleration()

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

Get the vehicle's linear acceleration.

返回: The vehicle's linear acceleration.

在文件 vehicle_state_provider.cc 第 185 行定义.

                                                       {
  return vehicle_state_.linear_acceleration();
}

◆ linear_velocity()

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

Get the vehicle's linear velocity.

返回: The vehicle's linear velocity.

在文件 vehicle_state_provider.cc 第 177 行定义.

                                                   {
  return vehicle_state_.linear_velocity();
}

◆ original_pose()

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

在文件 vehicle_state_provider.cc 第 203 行定义.

                                                                  {
  return original_localization_.pose();
}

◆ pitch()

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

Get the vehicle pitch angle.

返回: The euler pitch angle.

在文件 vehicle_state_provider.cc 第 167 行定义.

167{ return vehicle_state_.pitch(); }

apollo::common::VehicleState::pitch

optional double pitch

Definition vehicle_state.proto:14

◆ pose()

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

在文件 vehicle_state_provider.cc 第 199 行定义.

                                                         {
  return vehicle_state_.pose();
}

◆ roll()

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

Get the vehicle roll angle.

返回: The euler roll angle.

在文件 vehicle_state_provider.cc 第 165 行定义.

165{ return vehicle_state_.roll(); }

apollo::common::VehicleState::roll

optional double roll

Definition vehicle_state.proto:13

◆ set_linear_velocity()

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

Set the vehicle's linear velocity.

参数

linear_velocity The value to set the vehicle's linear velocity.

在文件 vehicle_state_provider.cc 第 207 行定义.

                                                                           {
  vehicle_state_.set_linear_velocity(linear_velocity);
}

◆ steering_percentage()

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

Get the vehicle's steering angle.

返回: double

在文件 vehicle_state_provider.cc 第 191 行定义.

                                                       {
  return vehicle_state_.steering_percentage();
}

◆ timestamp()

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

在文件 vehicle_state_provider.cc 第 195 行定义.

                                             {
  return vehicle_state_.timestamp();
}

◆ Update() [1/2]

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

Constructor by information of localization and chassis.

参数

localization	Localization information of the vehicle.
chassis	Chassis information of the vehicle.

在文件 vehicle_state_provider.cc 第 31 行定义.

                                  {
  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 = 0.1;
  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();
}

◆ 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_file	the localization protobuf file.
chassis_file	The chassis protobuf file

◆ vehicle_state()

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

在文件 vehicle_state_provider.cc 第 211 行定义.

                                                              {
  return vehicle_state_;
}

◆ x()

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

Get the x-coordinate of vehicle position.

返回: The x-coordinate of vehicle position.

在文件 vehicle_state_provider.cc 第 159 行定义.

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.cc 第 161 行定义.

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.cc 第 169 行定义.

169{ return vehicle_state_.yaw(); }

apollo::common::VehicleState::yaw

optional double yaw

Definition vehicle_state.proto:15

◆ z()

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

Get the z coordinate of vehicle position.

返回: The z coordinate of vehicle position.

在文件 vehicle_state_provider.cc 第 163 行定义.

163{ return vehicle_state_.z(); }

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

modules/common/vehicle_state/vehicle_state_provider.h
modules/common/vehicle_state/vehicle_state_provider.cc

Public 成员函数

详细描述

构造及析构函数说明

◆ ~VehicleStateProvider()

成员函数说明

◆ angular_velocity()

◆ ComputeCOMPosition()

◆ EstimateFuturePosition()

◆ gear()

◆ heading()

◆ kappa()

◆ linear_acceleration()

◆ linear_velocity()

◆ original_pose()

◆ pitch()

◆ pose()

◆ roll()

◆ set_linear_velocity()

◆ steering_percentage()

◆ timestamp()

◆ Update() [1/2]

◆ Update() [2/2]

◆ vehicle_state()

◆ x()

◆ y()

◆ yaw()

◆ z()