Localizer Class Reference

#include <Localizer.hpp>

Collaboration diagram for Localizer:

Public Member Functions
	Localizer ()
void	init (rclcpp::Time &timestamp, double orientationRad)
bool	is_empty ()
void	add_position_timepoint (rclcpp::Time &timestamp, galaxis::lina::Vector2f positionMillimeter, double speedMillimeterPerSec, double orientationRad)
void	add_state (rclcpp::Time &timestamp, double speedMillimeterPerSec, double orientationRad)
std::shared_ptr< PositionTimepoint >	get_latest_position_timepoint ()
std::shared_ptr< PositionTimepoint >	get_position_timepoint_by_time (rclcpp::Time &timestamp)

Private Attributes
PositionTimepointBuffer	m_positionHistory

Detailed Description

Definition at line 93 of file Localizer.hpp.

Constructor & Destructor Documentation

Localizer()

Localizer::Localizer

(

)

Definition at line 8 of file Localizer.cpp.

                     :
    m_positionHistory(200) {}

Member Function Documentation

add_position_timepoint()

void Localizer::add_position_timepoint	(	rclcpp::Time &	timestamp,
		galaxis::lina::Vector2f	positionMillimeter,
		double	speedMillimeterPerSec,
		double	orientationRad
	)

Definition at line 19 of file Localizer.cpp.

                                                                                                                                                         {
  PositionTimepoint newPositionPoint;
  newPositionPoint.timestamp = timestamp;
  newPositionPoint.speed = speedMillimeterPerSec;
  newPositionPoint.position = positionMillimeter;
  newPositionPoint.orientation = orientationRad;
 
  m_positionHistory.add(newPositionPoint);
}

References PositionTimepointBuffer::add(), m_positionHistory, PositionTimepoint::orientation, PositionTimepoint::position, PositionTimepoint::speed, and PositionTimepoint::timestamp.

Referenced by add_state(), AutoPilotNode::AutoPilotNode(), and AutoPilotNode::on_simulation_pose_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

add_state()

void Localizer::add_state	(	rclcpp::Time &	timestamp,
		double	speedMillimeterPerSec,
		double	orientationRad
	)

Definition at line 30 of file Localizer.cpp.

                                                                                                    {
  if(m_positionHistory.is_empty()) {
    log_warning("[LOCALIZER] Position History Empty!");
    this->add_position_timepoint(timestamp, galaxis::lina::Vector2f(0.f,0.f), speedMillimeterPerSec, orientationRad);
    return;
  }
 
  orientationRad = std::fmod(orientationRad, 2*MATH_PI_DOUBLE);
  // assert(orientationRad <= 2 * MATH_PI_DOUBLE + 0.1);
 
  PositionTimepoint newPositionPoint;
  newPositionPoint.timestamp = timestamp;
  newPositionPoint.orientation = orientationRad;
  newPositionPoint.speed = speedMillimeterPerSec;
 
  PositionTimepoint lastPositionPoint = m_positionHistory.get_last();
 
  // calc driven distance within time interval
  rclcpp::Duration timeIntervalNanoseconds = timestamp - lastPositionPoint.timestamp;
  double speed = (lastPositionPoint.speed + speedMillimeterPerSec) / 2.f;
  double drivenDistanceMillimeter = timeIntervalNanoseconds.nanoseconds() * speed / 1000000000.f; // based on speed at last point // TODO: CHECK
 
  // based on single-track model
  std::shared_ptr<CarPose> lastPose = std::make_shared<CarPose>();
  lastPose->orientation             = lastPositionPoint.orientation;
 
  double orientationDiff = orientationRad - lastPositionPoint.orientation;
 
  if(orientationDiff > MATH_PI_DOUBLE) { // if currentOrientation is lower than 0 deg (something around 2*PI) and lastOrientation greater than 0 deg
    orientationDiff -= 2 * MATH_PI_DOUBLE;
  } else if(orientationDiff < -MATH_PI_DOUBLE) { // analog the other way round
    orientationDiff += 2 * MATH_PI_DOUBLE;
  }
 
  // orientationDiff treated as turn angle
  // lastPose->position                = galaxis::lina::Vector2f(m_positionHistory.get_last().position[1], m_positionHistory.get_last().position[0]); // in mm
  // std::shared_ptr<CarPose> newPoseSPtr = galaxis::control::get_new_pose(lastPose, galaxis::controllerSettings::carWheelbase * 1000, drivenDistanceMillimeter, orientationDiff);
  // newPositionPoint.position    = galaxis::lina::Vector2f(newPoseSPtr->position[1], newPoseSPtr->position[0]);
  // newPositionPoint.orientation = orientationRad; // should be approx. equal to newPoseSPtr->orientation // TODO: check (look above) ***
 
  // new calculation
  Eigen::Rotation2Df lastOrientationRotMat(-lastPositionPoint.orientation); // counter-clock wise rotation
  galaxis::lina::Vector2f drivenDistanceVector = lastOrientationRotMat * galaxis::lina::Vector2f(0.f, drivenDistanceMillimeter);
  Eigen::Rotation2Df orientationDiffRotMat(-orientationDiff);
  drivenDistanceVector = orientationDiffRotMat * drivenDistanceVector;
 
  newPositionPoint.position = lastPositionPoint.position + drivenDistanceVector;
 
 
  log_debug("[LOCALIZER] Position: ", newPositionPoint.position[0], ",", newPositionPoint.position[1], "; Orientation: ", orientationRad);
 
  // append to buffer
  m_positionHistory.add(newPositionPoint);
}

References PositionTimepointBuffer::add(), add_position_timepoint(), PositionTimepointBuffer::get_last(), PositionTimepointBuffer::is_empty(), galaxis::utils::log_debug(), galaxis::utils::log_warning(), m_positionHistory, MATH_PI_DOUBLE, PositionTimepoint::orientation, PositionTimepoint::position, PositionTimepoint::speed, and PositionTimepoint::timestamp.

Referenced by AutoPilotNode::on_ecu_measurement_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

get_latest_position_timepoint()

std::shared_ptr< PositionTimepoint > Localizer::get_latest_position_timepoint

(

)

Definition at line 85 of file Localizer.cpp.

                                                                          {
  return std::make_shared<PositionTimepoint>(m_positionHistory.get_last());
}

References PositionTimepointBuffer::get_last(), and m_positionHistory.

Referenced by AutoPilotNode::car_pose_publish_loop(), AutoPilot::get_drive_information(), AutoPilot::get_lane_blocked(), AutoPilotNode::on_stop_line_callback(), and AutoPilotNode::send_map_debug_image().

Here is the call graph for this function:

Here is the caller graph for this function:

get_position_timepoint_by_time()

std::shared_ptr< PositionTimepoint > Localizer::get_position_timepoint_by_time

(

rclcpp::Time &

timestamp

)

Definition at line 89 of file Localizer.cpp.

                                                                                                {
  return std::make_shared<PositionTimepoint>(m_positionHistory.get_last_by_time(timestamp));
}

References PositionTimepointBuffer::get_last_by_time(), and m_positionHistory.

Referenced by AutoPilotNode::on_obstacle_callback(), and AutoPilotNode::on_vci_lane_detection_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

init()

void Localizer::init	(	rclcpp::Time &	timestamp,
		double	orientationRad
	)

Definition at line 11 of file Localizer.cpp.

                                                                 {
  m_positionHistory.add({ timestamp, galaxis::lina::Vector2f(0.f, 0.f), 0, orientationRad });
}

References PositionTimepointBuffer::add(), and m_positionHistory.

Here is the call graph for this function:

is_empty()

bool Localizer::is_empty

(

)

Definition at line 15 of file Localizer.cpp.

                         {
  return m_positionHistory.is_empty();
}

References PositionTimepointBuffer::is_empty(), and m_positionHistory.

Referenced by AutoPilotNode::car_pose_publish_loop(), AutoPilotNode::on_obstacle_callback(), and AutoPilotNode::on_vci_lane_detection_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

m_positionHistory

PositionTimepointBuffer Localizer::m_positionHistory

private

Definition at line 106 of file Localizer.hpp.

Referenced by add_position_timepoint(), add_state(), get_latest_position_timepoint(), get_position_timepoint_by_time(), init(), and is_empty().

---

The documentation for this class was generated from the following files:

• nodes/Autopilot/include/Localization/Localizer.hpp
• nodes/Autopilot/src/Localization/Localizer.cpp