simple_planner Namespace Reference

Namespace for simple_planner package. More...

Classes
struct	ConflictSample
struct	is_vector
struct	is_vector< std::vector< T, A > >
struct	ObjectTrajectory
struct	OrientedBox2D
struct	SimplePath
struct	SimplePathPoint
class	SimplePlannerNode
struct	TimedBox2D
struct	TopicDiagnosticConfig
	Configuration parameters for topic diagnostics. More...

Functions
double	wrap_angle_rad (double angle_rad)
	Wraps an angle to the range [-pi, pi].
Eigen::Vector2d	rotate (const Eigen::Vector2d &vec, double yaw)
	Rotates a 2D vector by the given yaw angle.
double	getStateRelativeTime (const perception_msgs::msg::ObjectState &state, const std_msgs::msg::Header &fallback_header, const rclcpp::Time &planning_stamp)
	Computes an object state's time relative to the planning stamp.
OrientedBox2D	buildOrientedBox (const Eigen::Vector2d &center, double yaw, double length, double width)
	Builds an oriented 2D bounding box from center pose and dimensions.
OrientedBox2D	expandBoxWithSafetyMargins (const OrientedBox2D &box, double longitudinal_safety_distance, double lateral_safety_distance)
	Expands a box by the given safety margins.
std::array< Eigen::Vector2d, 4 >	getBoxCorners (const OrientedBox2D &box)
	Returns the corners of an oriented box.
geometry_msgs::msg::Pose	toPose (const OrientedBox2D &box)
	Converts an oriented box center and heading to a ROS pose.
TimedBox2D	interpolateTimedBox (const TimedBox2D &lhs, const TimedBox2D &rhs, double t)
	Interpolates a timed box sample at the requested relative time.
bool	overlaps (const OrientedBox2D &first_box, const OrientedBox2D &second_box)
	Checks whether two oriented boxes overlap.
TimedBox2D	buildObjectSample (const perception_msgs::msg::ObjectState &state, const std_msgs::msg::Header &fallback_header, const rclcpp::Time &stamp, double length, double width)
	Builds a timed bounding-box sample for a single object state.

Variables
template<typename C >
constexpr bool	is_vector_v = is_vector<C>::value

Detailed Description

Namespace for simple_planner package.

Function Documentation

buildObjectSample()

TimedBox2D simple_planner::buildObjectSample ( const perception_msgs::msg::ObjectState & state, const std_msgs::msg::Header & fallback_header, const rclcpp::Time & stamp, double length, double width )

inline

Builds a timed bounding-box sample for a single object state.

Parameters

[in]	state	Object state in the vehicle frame.
[in]	fallback_header	Header used if the state has no own stamp.
[in]	stamp	Planning stamp used as relative time reference.
[in]	length	Object length.
[in]	width	Object width.

Returns

Timed box sample for the object state.

Definition at line 224 of file object_geometry.hpp.

                                                  {
  const auto center_msg = perception_msgs::object_access::getCenterPosition(state);
  TimedBox2D sample;
  sample.yaw = perception_msgs::object_access::getYaw(state);
  sample.length = length;
  sample.width = width;
  sample.box = buildOrientedBox(Eigen::Vector2d(center_msg.x, center_msg.y), sample.yaw, length, width);
  sample.t = getStateRelativeTime(state, fallback_header, stamp);
  return sample;
}

buildOrientedBox()

OrientedBox2D simple_planner::buildOrientedBox ( const Eigen::Vector2d & center, double yaw, double length, double width )

inline

Builds an oriented 2D bounding box from center pose and dimensions.

Parameters

[in]	center	Box center in the target frame.
[in]	yaw	Box heading in radians.
[in]	length	Box length.
[in]	width	Box width.

Returns

Oriented bounding box.

Definition at line 110 of file object_geometry.hpp.

                                                                                                            {
  OrientedBox2D box;
  box.center = center;
  box.axis_x = rotate(Eigen::Vector2d::UnitX(), yaw);
  box.axis_y = rotate(Eigen::Vector2d::UnitY(), yaw);
  box.half_length = std::max(length, 0.0) / 2.0;
  box.half_width = std::max(width, 0.0) / 2.0;
  return box;
}

expandBoxWithSafetyMargins()

OrientedBox2D simple_planner::expandBoxWithSafetyMargins ( const OrientedBox2D & box, double longitudinal_safety_distance, double lateral_safety_distance )

inline

Expands a box by the given safety margins.

Parameters

[in]	box	Box to expand.
[in]	longitudinal_safety_distance	Longitudinal safety margin.
[in]	lateral_safety_distance	Lateral safety margin.

Returns

Expanded box.

Definition at line 128 of file object_geometry.hpp.

                                                                                {
  OrientedBox2D expanded_box = box;
  const double longitudinal_margin = std::max(longitudinal_safety_distance, -2.0 * box.half_length);
  const double lateral_margin = std::max(lateral_safety_distance, -box.half_width);
  expanded_box.center += 0.5 * longitudinal_margin * box.axis_x;
  expanded_box.half_length += 0.5 * longitudinal_margin;
  expanded_box.half_width += lateral_margin;
  return expanded_box;
}

getBoxCorners()

std::array< Eigen::Vector2d, 4 > simple_planner::getBoxCorners ( const OrientedBox2D & box )

inline

Returns the corners of an oriented box.

Parameters

[in]

box

Box whose corners should be returned.

Returns

Box corners.

Definition at line 146 of file object_geometry.hpp.

                                                                          {
  return {box.center + box.half_length * box.axis_x + box.half_width * box.axis_y,
          box.center + box.half_length * box.axis_x - box.half_width * box.axis_y,
          box.center - box.half_length * box.axis_x - box.half_width * box.axis_y,
          box.center - box.half_length * box.axis_x + box.half_width * box.axis_y};
}

getStateRelativeTime()

double simple_planner::getStateRelativeTime ( const perception_msgs::msg::ObjectState & state, const std_msgs::msg::Header & fallback_header, const rclcpp::Time & planning_stamp )

inline

Computes an object state's time relative to the planning stamp.

Parameters

[in]	state	Object state whose stamp should be used if available.
[in]	fallback_header	Header used when the state has no own stamp.
[in]	planning_stamp	Reference time of the planning cycle.

Returns

Relative time in seconds.

Definition at line 93 of file object_geometry.hpp.

                                                                     {
  const bool has_state_stamp = state.header.stamp.sec != 0 || state.header.stamp.nanosec != 0;
  const auto& source_header = has_state_stamp ? state.header : fallback_header;
  return (rclcpp::Time(source_header.stamp) - planning_stamp).seconds();
}

interpolateTimedBox()

TimedBox2D simple_planner::interpolateTimedBox ( const TimedBox2D & lhs, const TimedBox2D & rhs, double t )

inline

Interpolates a timed box sample at the requested relative time.

Parameters

[in]	lhs	Earlier or first sample.
[in]	rhs	Later or second sample.
[in]	t	Relative time to sample.

Returns

Interpolated timed box.

Definition at line 178 of file object_geometry.hpp.

                                                                                              {
  const double duration = rhs.t - lhs.t;
  const double alpha = std::abs(duration) > 1e-6 ? std::clamp((t - lhs.t) / duration, 0.0, 1.0) : 0.0;
  TimedBox2D sample;
  sample.t = t;
  sample.yaw = wrap_angle_rad(lhs.yaw + alpha * wrap_angle_rad(rhs.yaw - lhs.yaw));
  sample.length = lhs.length + alpha * (rhs.length - lhs.length);
  sample.width = lhs.width + alpha * (rhs.width - lhs.width);
  const Eigen::Vector2d center = lhs.box.center + alpha * (rhs.box.center - lhs.box.center);
  sample.box = buildOrientedBox(center, sample.yaw, sample.length, sample.width);
  return sample;
}

overlaps()

bool simple_planner::overlaps ( const OrientedBox2D & first_box, const OrientedBox2D & second_box )

inline

Checks whether two oriented boxes overlap.

Parameters

[in]	first_box	First bounding box.
[in]	second_box	Second bounding box.

Returns

true if the boxes overlap.

false if a separating axis exists.

Definition at line 199 of file object_geometry.hpp.

                                                                                      {
  const Eigen::Vector2d center_delta = second_box.center - first_box.center;
  const std::array<Eigen::Vector2d, 4> axes = {first_box.axis_x, first_box.axis_y, second_box.axis_x, second_box.axis_y};
  for (const auto& axis : axes) {
    const double first_extent = first_box.half_length * std::abs(axis.dot(first_box.axis_x)) +
                                first_box.half_width * std::abs(axis.dot(first_box.axis_y));
    const double second_extent = second_box.half_length * std::abs(axis.dot(second_box.axis_x)) +
                                 second_box.half_width * std::abs(axis.dot(second_box.axis_y));
    if (std::abs(axis.dot(center_delta)) > first_extent + second_extent + 1e-6) {
      return false;
    }
  }
  return true;
}

rotate()

Eigen::Vector2d simple_planner::rotate ( const Eigen::Vector2d & vec, double yaw )

inline

Rotates a 2D vector by the given yaw angle.

Parameters

[in]	vec	Vector to rotate.
[in]	yaw	Rotation angle in radians.

Returns

Rotated vector.

Definition at line 79 of file object_geometry.hpp.

                                                                  {
  const double c = std::cos(yaw);
  const double s = std::sin(yaw);
  return Eigen::Vector2d(c * vec.x() - s * vec.y(), s * vec.x() + c * vec.y());
}

toPose()

geometry_msgs::msg::Pose simple_planner::toPose ( const OrientedBox2D & box )

inline

Converts an oriented box center and heading to a ROS pose.

Parameters

[in]

box

Box to convert.

Returns

Pose at the box center with the box heading.

Definition at line 159 of file object_geometry.hpp.

                                                             {
  geometry_msgs::msg::Pose pose;
  pose.position.x = box.center.x();
  pose.position.y = box.center.y();
  pose.position.z = 0.0;
  const double yaw = std::atan2(box.axis_x.y(), box.axis_x.x());
  pose.orientation.z = std::sin(yaw / 2.0);
  pose.orientation.w = std::cos(yaw / 2.0);
  return pose;
}

wrap_angle_rad()

double simple_planner::wrap_angle_rad ( double angle_rad )

inline

Wraps an angle to the range [-pi, pi].

Parameters

[in]

angle_rad

Angle in radians.

Returns

Wrapped angle in radians.

Definition at line 65 of file object_geometry.hpp.

                                               {
  double capped_angle_rad = angle_rad;
  while (capped_angle_rad > M_PI) capped_angle_rad -= 2 * M_PI;
  while (capped_angle_rad < -M_PI) capped_angle_rad += 2 * M_PI;
  return capped_angle_rad;
}

Variable Documentation

is_vector_v

template<typename C >

bool simple_planner::is_vector_v = is_vector<C>::value

inlineconstexpr

Definition at line 49 of file simple_planner.hpp.