This class is a multi-threaded mobile robot navigator, implementing an (anytime) PTG-based Rapidly-exploring Random Tree (PRRT) search algorithm. More...
#include <mrpt/reactivenav/CPRRTNavigator.h>
Classes | |
class | CRobotStateEstimator |
The filter that estimates and extrapolates the robot pose from odometry and localization data The implemented model is a state vector:
| |
class | TOptions |
struct | TPathData |
Each data point in m_planned_path. More... | |
Public Member Functions | |
CPRRTNavigator () | |
Constructor. | |
virtual | ~CPRRTNavigator () |
Destructor. | |
Navigation control methods | |
bool | initialize () |
Initialize the navigator using the parameters in "params"; read the usage instructions in CPRRTNavigator. | |
void | navigate (const mrpt::math::TPose2D &target_pose) |
Starts a navigation to a 2D pose (including a desired heading at the target). | |
void | navigate (const mrpt::math::TPoint2D &target_point) |
Starts a navigation to a given 2D point (any heading at the target). | |
void | cancel () |
Cancel current navegacion. | |
void | resume () |
Continues with suspended navigation. | |
void | suspend () |
Suspend current navegation. | |
Debug and logging | |
void | setPathToFollow (const TPlannedPath &path) |
Manually sets the short-time path to be followed by the robot (use 'navigate' instead, this method is for debugging mainly). | |
void | getCurrentPlannedPath (TPlannedPath &path) const |
Returns the current planned path the robot is following. | |
Sensory data methods: call them to update the navigator knowledge on the outside world. | |
void | processNewLocalization (const TPose2D &new_robot_pose, const CMatrixDouble33 &new_robot_cov, TTimeStamp timestamp) |
Updates the navigator with a low or high-rate estimate from a localization (or SLAM) algorithm running somewhere else. | |
void | processNewOdometryInfo (const TPose2D &newOdoPose, TTimeStamp timestamp, bool hasVelocities=false, float v=0, float w=0) |
Updates the navigator with high-rate odometry from the mobile base. | |
void | processNewObstaclesData (const mrpt::slam::CPointsMap *obstacles, TTimeStamp timestamp) |
Must be called in a timely fashion to let the navigator know about the obstacles in the environment. | |
Virtual methods to be implemented by the user. | |
virtual bool | onMotionCommand (float v, float w)=0 |
This is the most important method the user must provide: to send an instantaneous velocity command to the robot. | |
virtual void | onNavigationStart () |
Re-implement this method if you want to know when a new navigation has started. | |
virtual void | onNavigationEnd (bool targetReachedOK) |
Re-implement this method if you want to know when and why a navigation has ended. | |
virtual void | onApproachingTarget () |
Re-implement this method if you want to know when the robot is approaching the target: this event is raised before onNavigationEnd, when the robot is closer than a certain distance to the target. | |
Public Attributes | |
CRobotStateEstimator | m_robotStateFilter |
Object maintained by the robot-tracking thread (All methods are thread-safe). | |
Static Public Attributes | |
static const double | INVALID_HEADING |
Used to refer to undefined or "never mind" headings values. | |
Static Private Member Functions | |
static void | thread_planner (void *obj) |
Thread function. | |
static void | thread_test_collision (void *obj) |
Thread function. | |
static void | thread_path_tracking (void *obj) |
Thread function. | |
Private Attributes | |
TThreadHandle | m_thr_planner |
Thread handle. | |
TThreadHandle | m_thr_testcol |
Thread handle. | |
TThreadHandle | m_thr_pathtrack |
Thread handle. | |
bool | m_initialized |
The object is ready to navigate. Users must call "initialize" first. | |
bool | m_closingThreads |
set to true at destructor. | |
TPose2D | m_target_pose |
Heading may be INVALID_HEADING to indicate "don't mind". | |
TTimeStamp | m_target_pose_time |
CCriticalSection | m_target_pose_cs |
vector_float | m_last_obstacles_x |
vector_float | m_last_obstacles_y |
TTimeStamp | m_last_obstacles_time |
CCriticalSection | m_last_obstacles_cs |
CCriticalSection | m_planned_path_cs |
TTimeStamp | m_planned_path_time |
The last modification time. INVALID_TIMESTAMP means there is no path. | |
TPlannedPath | m_planned_path |
TListPTGs | m_PTGs |
The list of PTGs used by the anytime planner to explore the free-space. | |
CCriticalSection | m_PTGs_cs |
Navigator data structures | |
| |
typedef std::list< TPathData > | TPlannedPath |
An ordered list of path poses. | |
TOptions | params |
Change here all the parameters of the navigator. |
This class is a multi-threaded mobile robot navigator, implementing an (anytime) PTG-based Rapidly-exploring Random Tree (PRRT) search algorithm.
Usage:
Note that all the public methods are thread-safe.
About the algorithm:
Changes history
Definition at line 70 of file CPRRTNavigator.h.
typedef std::list<TPathData> mrpt::reactivenav::CPRRTNavigator::TPlannedPath |
An ordered list of path poses.
Definition at line 129 of file CPRRTNavigator.h.
mrpt::reactivenav::CPRRTNavigator::CPRRTNavigator | ( | ) |
Constructor.
virtual mrpt::reactivenav::CPRRTNavigator::~CPRRTNavigator | ( | ) | [virtual] |
Destructor.
void mrpt::reactivenav::CPRRTNavigator::cancel | ( | ) |
Cancel current navegacion.
void mrpt::reactivenav::CPRRTNavigator::getCurrentPlannedPath | ( | TPlannedPath & | path | ) | const |
Returns the current planned path the robot is following.
bool mrpt::reactivenav::CPRRTNavigator::initialize | ( | ) |
Initialize the navigator using the parameters in "params"; read the usage instructions in CPRRTNavigator.
This method should be called only once at the beginning of the robot operation.
void mrpt::reactivenav::CPRRTNavigator::navigate | ( | const mrpt::math::TPoint2D & | target_point | ) |
Starts a navigation to a given 2D point (any heading at the target).
void mrpt::reactivenav::CPRRTNavigator::navigate | ( | const mrpt::math::TPose2D & | target_pose | ) |
Starts a navigation to a 2D pose (including a desired heading at the target).
virtual void mrpt::reactivenav::CPRRTNavigator::onApproachingTarget | ( | ) | [inline, virtual] |
Re-implement this method if you want to know when the robot is approaching the target: this event is raised before onNavigationEnd, when the robot is closer than a certain distance to the target.
Definition at line 241 of file CPRRTNavigator.h.
virtual bool mrpt::reactivenav::CPRRTNavigator::onMotionCommand | ( | float | v, | |
float | w | |||
) | [pure virtual] |
This is the most important method the user must provide: to send an instantaneous velocity command to the robot.
v | Desired linear speed, in meters/second. | |
w | Desired angular speed, in rads/second. |
virtual void mrpt::reactivenav::CPRRTNavigator::onNavigationEnd | ( | bool | targetReachedOK | ) | [inline, virtual] |
Re-implement this method if you want to know when and why a navigation has ended.
targetReachedOK | Will be false if the navigation failed. |
Definition at line 236 of file CPRRTNavigator.h.
virtual void mrpt::reactivenav::CPRRTNavigator::onNavigationStart | ( | ) | [inline, virtual] |
Re-implement this method if you want to know when a new navigation has started.
Definition at line 231 of file CPRRTNavigator.h.
void mrpt::reactivenav::CPRRTNavigator::processNewLocalization | ( | const TPose2D & | new_robot_pose, | |
const CMatrixDouble33 & | new_robot_cov, | |||
TTimeStamp | timestamp | |||
) |
Updates the navigator with a low or high-rate estimate from a localization (or SLAM) algorithm running somewhere else.
new_robot_pose | The (global) coordinates of the mean robot pose as estimated by some external module. | |
new_robot_cov | The 3x3 covariance matrix of that estimate. | |
timestamp | The associated timestamp of the data. |
void mrpt::reactivenav::CPRRTNavigator::processNewObstaclesData | ( | const mrpt::slam::CPointsMap * | obstacles, | |
TTimeStamp | timestamp | |||
) |
Must be called in a timely fashion to let the navigator know about the obstacles in the environment.
obstacles | ||
timestamp | The associated timestamp of the sensed points. |
void mrpt::reactivenav::CPRRTNavigator::processNewOdometryInfo | ( | const TPose2D & | newOdoPose, | |
TTimeStamp | timestamp, | |||
bool | hasVelocities = false , |
|||
float | v = 0 , |
|||
float | w = 0 | |||
) |
Updates the navigator with high-rate odometry from the mobile base.
The odometry poses are dealed internally as increments only, so there is no problem is arbitrary mismatches between localization (from a particle filter or SLAM) and the odometry.
newOdoPose | The global, cummulative odometry as computed by the robot. | |
timestamp | The associated timestamp of the measurement. | |
hasVelocities | If false, the next arguments are ignored. | |
v | Measured linear speed, in m/s. | |
w | Measured angular speed, in rad/s. |
void mrpt::reactivenav::CPRRTNavigator::resume | ( | ) |
Continues with suspended navigation.
void mrpt::reactivenav::CPRRTNavigator::setPathToFollow | ( | const TPlannedPath & | path | ) |
Manually sets the short-time path to be followed by the robot (use 'navigate' instead, this method is for debugging mainly).
void mrpt::reactivenav::CPRRTNavigator::suspend | ( | ) |
static void mrpt::reactivenav::CPRRTNavigator::thread_path_tracking | ( | void * | obj | ) | [static, private] |
Thread function.
static void mrpt::reactivenav::CPRRTNavigator::thread_planner | ( | void * | obj | ) | [static, private] |
Thread function.
static void mrpt::reactivenav::CPRRTNavigator::thread_test_collision | ( | void * | obj | ) | [static, private] |
Thread function.
const double mrpt::reactivenav::CPRRTNavigator::INVALID_HEADING [static] |
Used to refer to undefined or "never mind" headings values.
Definition at line 245 of file CPRRTNavigator.h.
bool mrpt::reactivenav::CPRRTNavigator::m_closingThreads [private] |
set to true at destructor.
Definition at line 262 of file CPRRTNavigator.h.
bool mrpt::reactivenav::CPRRTNavigator::m_initialized [private] |
The object is ready to navigate. Users must call "initialize" first.
Definition at line 261 of file CPRRTNavigator.h.
Definition at line 274 of file CPRRTNavigator.h.
TTimeStamp mrpt::reactivenav::CPRRTNavigator::m_last_obstacles_time [private] |
Definition at line 273 of file CPRRTNavigator.h.
Definition at line 272 of file CPRRTNavigator.h.
Definition at line 272 of file CPRRTNavigator.h.
Definition at line 279 of file CPRRTNavigator.h.
Definition at line 277 of file CPRRTNavigator.h.
TTimeStamp mrpt::reactivenav::CPRRTNavigator::m_planned_path_time [private] |
The last modification time. INVALID_TIMESTAMP means there is no path.
Definition at line 278 of file CPRRTNavigator.h.
The list of PTGs used by the anytime planner to explore the free-space.
Definition at line 282 of file CPRRTNavigator.h.
Definition at line 283 of file CPRRTNavigator.h.
Object maintained by the robot-tracking thread (All methods are thread-safe).
Definition at line 361 of file CPRRTNavigator.h.
Heading may be INVALID_HEADING to indicate "don't mind".
Definition at line 264 of file CPRRTNavigator.h.
Definition at line 266 of file CPRRTNavigator.h.
TTimeStamp mrpt::reactivenav::CPRRTNavigator::m_target_pose_time [private] |
Definition at line 265 of file CPRRTNavigator.h.
Thread handle.
Definition at line 252 of file CPRRTNavigator.h.
Thread handle.
Definition at line 250 of file CPRRTNavigator.h.
Thread handle.
Definition at line 251 of file CPRRTNavigator.h.
Change here all the parameters of the navigator.
Definition at line 175 of file CPRRTNavigator.h.
Page generated by Doxygen 1.6.1 for MRPT 0.7.1 SVN: at Tue Dec 22 08:29:35 CET 2009 |