#include <mpp/algos/NavEngine.h>

Inheritance diagram for mpp::NavEngine:

Classes
struct	AboutToReachWpInfo

struct	AlignStatus

struct	Configuration

struct	CurrentNavInternalState

struct	PathPlannerInput

struct	PathPlannerOutput

Public Member Functions
	NavEngine ()

virtual	~NavEngine ()

void	send_planner_output_to_viz (const PathPlannerOutput &ppo)

void	send_path_to_viz_and_navlog (const MotionPrimitivesTreeSE2 &tree, const std::optional< TNodeID > &finalNode, const PlannerInput &originalPlanInput, const std::vector< CostEvaluator::Ptr > &costEvaluators)

void	send_current_state_to_viz_and_navlog ()

Protected Member Functions
void	dispatch_pending_nav_events ()

virtual void	update_robot_kinematic_state ()

virtual void	impl_navigation_step ()

void	internal_on_start_new_navigation ()

void	internal_start_navlog_file ()

void	internal_write_to_navlog_file ()

PathPlannerOutput	path_planner_function (PathPlannerInput ppi)

void	check_immediate_collision ()

void	check_have_to_replan ()

void	check_new_planner_output ()

void	send_next_motion_cmd_or_nop ()

waypoint_idx_t	find_next_waypoint_for_planner ()

void	enqueue_path_planner_towards (const waypoint_idx_t target, const mpp::SE2_KinState &startingFrom, const std::optional< TNodeID > &startingFromNodeID=std::nullopt)

bool	approach_target_controller ()

void	merge_new_plan_if_better (const PathPlannerOutput &result)

void	internal_mark_current_wp_as_reached ()

bool	check_all_waypoints_are_done ()

AboutToReachWpInfo	internal_check_about_to_reach_stop_wp ()
	Checks whether the robot is within `maxDistanceForTargetApproach` meters of the next non-skippable waypoint. This does not check for potential obstacles, just the physical nearness. More...

Protected Attributes
NavStatus	navigationStatus_ = NavStatus::IDLE

NavStatus	lastNavigationState_ = NavStatus::IDLE

NavErrorReason	navErrorReason_

mrpt::system::output_logger_callback_t	loggerToNavlog_

bool	initialized_ = false

std::recursive_mutex	navMtx_

VehicleLocalizationState	lastVehicleLocalization_

VehicleOdometryState	lastVehicleOdometry_

double	lastVehiclePosRobotTime_ = 0

std::list< std::function< void(void)> >	pendingEvents_

std::optional< mrpt::io::CFileGZOutputStream >	navlog_output_file_
	Created in internal_start_navlog_file() More...

bool	navlogOutputFirstEntry_ = true

mrpt::WorkerThreadsPool	pathPlannerPool_

CurrentNavInternalState	innerState_

mrpt::kinematics::CVehicleVelCmd::TVelCmdParams	absoluteSpeedLimits_

Initialization and parameters
Configuration	config_

virtual void	initialize ()

const mrpt::kinematics::CVehicleVelCmd::TVelCmdParams &	absoluteSpeedLimits () const

void	absoluteSpeedLimits (const mrpt::kinematics::CVehicleVelCmd::TVelCmdParams &newLimits)

Waypoint navigation control API
mrpt::system::CTimeLogger	navProfiler_ {true , "NavEngine"}

virtual void	request_navigation (const WaypointSequence &navRequest)

virtual void	navigation_step ()

virtual void	cancel ()

virtual void	resume ()

virtual void	suspend ()

virtual void	reset_nav_error ()

NavStatus	current_status () const

const NavErrorReason &	error_reason () const

WaypointStatusSequence	waypoint_nav_status () const

WaypointStatusSequence &	beginWaypointsAccess ()

void	end_waypoints_access ()

Detailed Description

The top-level interface for users to control the vehicle navigation.

This class is based and extends mrpt::nav::CAbstractNavigator with the capability of following a list of waypoints. By default, waypoints are followed one by one, but, refer to Waypoint for a discussion on allow_skip.

How to use:

initialize() must be called before running any actual navigation.
Callbacks must be provided for interfacing the real (or simulated) robot/vehicle and its sensors. They must be provided as std::function<> instances within config_. Note they may be actual functions, or lambdas.
navigation_step() must be called periodically in order to effectively run the navigation. This method will internally call the callbacks to gather sensor data and robot positioning data.

This class implements the following state machine (see current_status()):

All methods are thread-safe, in the sense that mutexes are internally used to ensure no undefined navigation state is possible if invoking an object of this class from more than one thread.

Constructor & Destructor Documentation

◆ NavEngine()

mpp::NavEngine::NavEngine ( )

inline

ctor

◆ ~NavEngine()

virtual mpp::NavEngine::~NavEngine ( )

virtual

dtor

Member Function Documentation

◆ absoluteSpeedLimits() [1/2]

const mrpt::kinematics::CVehicleVelCmd::TVelCmdParams& mpp::NavEngine::absoluteSpeedLimits ( ) const

inline

Read access to current absolute speed limits

◆ absoluteSpeedLimits() [2/2]

void mpp::NavEngine::absoluteSpeedLimits ( const mrpt::kinematics::CVehicleVelCmd::TVelCmdParams & newLimits )

Changes the current speed limits

◆ approach_target_controller()

bool mpp::NavEngine::approach_target_controller ( )

protected

Special behavior: if we are about to reach a WP with a stop condition, handle it specially if there's an obvious free path towards it.

Returns: true if the special motion has been generated (or it's under execution). false if the regular path plan should go on.

Note: Call from within send_next_motion_cmd_or_nop()

◆ beginWaypointsAccess()

WaypointStatusSequence& mpp::NavEngine::beginWaypointsAccess ( )

inline

Gets a write-enabled reference to the list of waypoints, simultaneously acquiring the critical section mutex. Caller must call endWaypointsAccess() when done editing the waypoints.

◆ cancel()

virtual void mpp::NavEngine::cancel ( )

virtual

Cancel current navegation.

◆ check_all_waypoints_are_done()

bool mpp::NavEngine::check_all_waypoints_are_done ( )

protected

Returns true if all waypoints has been reached successfully.

◆ check_have_to_replan()

void mpp::NavEngine::check_have_to_replan ( )

protected

Checks whether we need to launch a new RRT* path planner

◆ check_immediate_collision()

void mpp::NavEngine::check_immediate_collision ( )

protected

Checks whether the current motion leads us into an obstacle

◆ check_new_planner_output()

void mpp::NavEngine::check_new_planner_output ( )

protected

Checks whether the A* planner finished, then send a new active trajectory to the path tracker

◆ current_status()

NavStatus mpp::NavEngine::current_status ( ) const

inline

Returns the current navigator status.

◆ dispatch_pending_nav_events()

void mpp::NavEngine::dispatch_pending_nav_events ( )

protected

◆ end_waypoints_access()

void mpp::NavEngine::end_waypoints_access ( )

inline

Must be called after beginWaypointsAccess()

◆ enqueue_path_planner_towards()

void mpp::NavEngine::enqueue_path_planner_towards	(	const waypoint_idx_t	target,
		const mpp::SE2_KinState &	startingFrom,
		const std::optional< TNodeID > &	startingFromNodeID = `std::nullopt`
	)

protected

Enqueues a task in pathPlannerPool_ running path_planner_function() and saving future results into pathPlannerFuture.

If this is a path refining, startingFrom and startingFromNodeID must be supplied, with the latter being the nodeId of the the plan starting state in activePlanOutput, activePlanPath, activePlanPathEdges.

◆ error_reason()

const NavErrorReason& mpp::NavEngine::error_reason ( ) const

inline

In case of status=NAV_ERROR, this returns the reason for the error. Error status is reseted every time a new navigation starts with a call to navigate(), or when reset_nav_error() is called.

◆ find_next_waypoint_for_planner()

waypoint_idx_t mpp::NavEngine::find_next_waypoint_for_planner ( )

protected

Finds the next waypt index up to which we should find a new RRT* plan

◆ impl_navigation_step()

virtual void mpp::NavEngine::impl_navigation_step ( )

protectedvirtual

The actual action that happens inside navigation_step() for the case of state being NAVIGATING.

◆ initialize()

virtual void mpp::NavEngine::initialize ( )

virtual

Must be called before any other navigation command, and after filling in all the required data into config_

◆ internal_check_about_to_reach_stop_wp()

AboutToReachWpInfo mpp::NavEngine::internal_check_about_to_reach_stop_wp ( )

protected

Checks whether the robot is within maxDistanceForTargetApproach meters of the next non-skippable waypoint. This does not check for potential obstacles, just the physical nearness.

Returns: See AboutToReachWpInfo.

◆ internal_mark_current_wp_as_reached()

void mpp::NavEngine::internal_mark_current_wp_as_reached ( )

protected

◆ internal_on_start_new_navigation()

void mpp::NavEngine::internal_on_start_new_navigation ( )

protected

Implements the way to waypoint is free function in children classes: true must be returned if, according to the information gathered at the last navigation step, there is a free path to the given point; false otherwise: if way is blocked or there is missing information, the point is out of range, etc.

◆ internal_start_navlog_file()

void mpp::NavEngine::internal_start_navlog_file ( )

protected

◆ internal_write_to_navlog_file()

void mpp::NavEngine::internal_write_to_navlog_file ( )

protected

◆ merge_new_plan_if_better()

void mpp::NavEngine::merge_new_plan_if_better ( const PathPlannerOutput & result )

protected

◆ navigation_step()

virtual void mpp::NavEngine::navigation_step ( )

virtual

This method must be called periodically in order to effectively run the navigation

◆ path_planner_function()

PathPlannerOutput mpp::NavEngine::path_planner_function ( PathPlannerInput ppi )

protected

◆ request_navigation()

virtual void mpp::NavEngine::request_navigation ( const WaypointSequence & navRequest )

virtual

Waypoint navigation request. This immediately cancels any other previous on-going navigation.

◆ reset_nav_error()

virtual void mpp::NavEngine::reset_nav_error ( )

virtual

Resets a NAV_ERROR state back to IDLE

◆ resume()

virtual void mpp::NavEngine::resume ( )

virtual

Continues with suspended navigation.

See also: suspend

◆ send_current_state_to_viz_and_navlog()

void mpp::NavEngine::send_current_state_to_viz_and_navlog ( )

Update current path plan visualization details in the GUI, or in the opengl object buffered to be writen to navlog files.

◆ send_next_motion_cmd_or_nop()

void mpp::NavEngine::send_next_motion_cmd_or_nop ( )

protected

Checks and send next motion command, or NOP, if we are on track

◆ send_path_to_viz_and_navlog()

void mpp::NavEngine::send_path_to_viz_and_navlog	(	const MotionPrimitivesTreeSE2 &	tree,
		const std::optional< TNodeID > &	finalNode,
		const PlannerInput &	originalPlanInput,
		const std::vector< CostEvaluator::Ptr > &	costEvaluators
	)

Update the GUI with a partial or final path only (no whole tree)

◆ send_planner_output_to_viz()

void mpp::NavEngine::send_planner_output_to_viz ( const PathPlannerOutput & ppo )

Use the callbacks above and render_tree() to update a visualization with a given plan output

◆ suspend()

virtual void mpp::NavEngine::suspend ( )

virtual

Suspend current navegation.

See also: resume

◆ update_robot_kinematic_state()

virtual void mpp::NavEngine::update_robot_kinematic_state ( )

protectedvirtual

Call to the robot getCurrentPoseAndSpeeds() and updates members m_curPoseVel accordingly. If an error is returned by the user callback, first, it calls robot.stop() ,then throws an std::runtime_error exception.

◆ waypoint_nav_status()

WaypointStatusSequence mpp::NavEngine::waypoint_nav_status ( ) const

Get a copy of the control structure which describes the progress status of the waypoint navigation.

Member Data Documentation

◆ absoluteSpeedLimits_

mrpt::kinematics::CVehicleVelCmd::TVelCmdParams mpp::NavEngine::absoluteSpeedLimits_

protected

Speed limits: If not defined, the values from the first PTG will be copied here upon initialize()

◆ config_

Configuration mpp::NavEngine::config_

◆ initialized_

bool mpp::NavEngine::initialized_ = false

protected

◆ innerState_

CurrentNavInternalState mpp::NavEngine::innerState_

protected

Navigation state variables, protected by navMtx_

◆ lastNavigationState_

NavStatus mpp::NavEngine::lastNavigationState_ = NavStatus::IDLE

protected

◆ lastVehicleLocalization_

VehicleLocalizationState mpp::NavEngine::lastVehicleLocalization_

protected

Current robot kinematic state; Updated in navigation_step() with a minimum period of MIN_TIME_BETWEEN_POSE_UPDATES.

◆ lastVehicleOdometry_

VehicleOdometryState mpp::NavEngine::lastVehicleOdometry_

protected

◆ lastVehiclePosRobotTime_

double mpp::NavEngine::lastVehiclePosRobotTime_ = 0

protected

◆ loggerToNavlog_

mrpt::system::output_logger_callback_t mpp::NavEngine::loggerToNavlog_

protected

◆ navErrorReason_

NavErrorReason mpp::NavEngine::navErrorReason_

protected

◆ navigationStatus_

NavStatus mpp::NavEngine::navigationStatus_ = NavStatus::IDLE

protected

Current and last internal state of navigator:

◆ navlog_output_file_

std::optional<mrpt::io::CFileGZOutputStream> mpp::NavEngine::navlog_output_file_

protected

Created in internal_start_navlog_file()

◆ navlogOutputFirstEntry_

bool mpp::NavEngine::navlogOutputFirstEntry_ = true

protected

◆ navMtx_

std::recursive_mutex mpp::NavEngine::navMtx_

protected

mutex for all navigation methods

◆ navProfiler_

mrpt::system::CTimeLogger mpp::NavEngine::navProfiler_ {true , "NavEngine"}

Publicly available time profiling object. Default: disabled

◆ pathPlannerPool_

mrpt::WorkerThreadsPool mpp::NavEngine::pathPlannerPool_

protected

Initial value:

{
        1 , mrpt::WorkerThreadsPool::POLICY_DROP_OLD,
        "path_planner"}

◆ pendingEvents_

std::list<std::function<void(void)> > mpp::NavEngine::pendingEvents_

protected

Events generated during navigation_step(), enqueued to be called at the end of the method execution to avoid user code to change the navigator state.

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

/home/docs/checkouts/readthedocs.org/user_builds/selfdriving/checkouts/latest/mrpt_path_planning/include/mpp/algos/NavEngine.h

Classes

Public Member Functions

Protected Member Functions

Protected Attributes

Initialization and parameters

Waypoint navigation control API

Detailed Description

Constructor & Destructor Documentation

◆ NavEngine()

◆ ~NavEngine()

Member Function Documentation

◆ absoluteSpeedLimits() [1/2]

◆ absoluteSpeedLimits() [2/2]

◆ approach_target_controller()

◆ beginWaypointsAccess()

◆ cancel()

◆ check_all_waypoints_are_done()

◆ check_have_to_replan()

◆ check_immediate_collision()

◆ check_new_planner_output()

◆ current_status()

◆ dispatch_pending_nav_events()

◆ end_waypoints_access()

◆ enqueue_path_planner_towards()

◆ error_reason()

◆ find_next_waypoint_for_planner()

◆ impl_navigation_step()

◆ initialize()

◆ internal_check_about_to_reach_stop_wp()

◆ internal_mark_current_wp_as_reached()

◆ internal_on_start_new_navigation()

◆ internal_start_navlog_file()

◆ internal_write_to_navlog_file()

◆ merge_new_plan_if_better()

◆ navigation_step()

◆ path_planner_function()

◆ request_navigation()

◆ reset_nav_error()

◆ resume()

◆ send_current_state_to_viz_and_navlog()

◆ send_next_motion_cmd_or_nop()

◆ send_path_to_viz_and_navlog()

◆ send_planner_output_to_viz()

◆ suspend()

◆ update_robot_kinematic_state()

◆ waypoint_nav_status()

Member Data Documentation

◆ absoluteSpeedLimits_

◆ config_

◆ initialized_

◆ innerState_

◆ lastNavigationState_

◆ lastVehicleLocalization_

◆ lastVehicleOdometry_

◆ lastVehiclePosRobotTime_

◆ loggerToNavlog_

◆ navErrorReason_

◆ navigationStatus_

◆ navlog_output_file_

◆ navlogOutputFirstEntry_

◆ navMtx_

◆ navProfiler_

◆ pathPlannerPool_

◆ pendingEvents_