Motion planning geometry utility functions

Overview

(#include <mrpt/nav/nav_plan_geometry_utils.h>)

// global functions

std::optional<double> mrpt::nav::collision_free_dist_segment_circ_robot(
    const mrpt::math::TPoint2D& p_start,
    const mrpt::math::TPoint2D& p_end,
    const double robot_radius,
    const mrpt::math::TPoint2D& obstacle
    );

std::optional<double> mrpt::nav::collision_free_dist_arc_circ_robot(
    const double arc_radius,
    const double robot_radius,
    const mrpt::math::TPoint2D& obstacle
    );

bool mrpt::nav::collision_free_dist_segment_circ_robot(
    const mrpt::math::TPoint2D& p_start,
    const mrpt::math::TPoint2D& p_end,
    const double robot_radius,
    const mrpt::math::TPoint2D& obstacle,
    double& out_col_dist
    );

bool mrpt::nav::collision_free_dist_arc_circ_robot(
    const double arc_radius,
    const double robot_radius,
    const mrpt::math::TPoint2D& obstacle,
    double& out_col_dist
    );

Global Functions

std::optional<double> mrpt::nav::collision_free_dist_segment_circ_robot(
    const mrpt::math::TPoint2D& p_start,
    const mrpt::math::TPoint2D& p_end,
    const double robot_radius,
    const mrpt::math::TPoint2D& obstacle
    )

Computes the collision-free distance for a linear segment path between two points, for a circular robot, and a point obstacle.

Parameters:

std::runtime_error

If the two points are closer than an epsilon (1e-10)

Returns:

The distance along the segment at which the robot first touches the obstacle, or std::nullopt if the segment is collision-free.

std::optional<double> mrpt::nav::collision_free_dist_arc_circ_robot(
    const double arc_radius,
    const double robot_radius,
    const mrpt::math::TPoint2D& obstacle
    )

Computes the collision-free distance for a forward path (+X) circular arc path segment from pose (0,0,0) and radius of curvature arc_radius (>0 -> turn towards +Y, <0 -> towards -Y), a circular robot and a point obstacle.

Returns:

The arc length at which the robot first touches the obstacle (0 if it is already in collision at the starting pose), or std::nullopt if the arc is collision-free (which includes the degenerate case of the robot enclosing the obstacle at every pose along the arc).

bool mrpt::nav::collision_free_dist_segment_circ_robot(
    const mrpt::math::TPoint2D& p_start,
    const mrpt::math::TPoint2D& p_end,
    const double robot_radius,
    const mrpt::math::TPoint2D& obstacle,
    double& out_col_dist
    )

Deprecated Use the std::optional-returning overload.

bool mrpt::nav::collision_free_dist_arc_circ_robot(
    const double arc_radius,
    const double robot_radius,
    const mrpt::math::TPoint2D& obstacle,
    double& out_col_dist
    )

Deprecated Use the std::optional-returning overload.