Go to the documentation of this file.
26 : target_coords(0, 0, 0), target_frame_id(
"map")
36 "target_coords = (%.03f,%.03f,%.03f deg)\n", target_coords.x,
37 target_coords.y, target_coords.phi);
38 s +=
mrpt::format(
"target_frame_id = \"%s\"\n", target_frame_id.c_str());
39 s +=
mrpt::format(
"targetAllowedDistance = %.03f\n", targetAllowedDistance);
41 "targetIsRelative = %s\n", targetIsRelative ?
"YES" :
"NO");
43 "targetIsIntermediaryWaypoint = %s\n",
44 targetIsIntermediaryWaypoint ?
"YES" :
"NO");
45 s +=
mrpt::format(
"targetDesiredRelSpeed = %.02f\n", targetDesiredRelSpeed);
64 s +=
"navparams. Single target:\n";
65 s += target.getAsText();
73 return (rhs !=
nullptr) && (this->target == rhs->target);
158 "CAbstractNavigator",
"enable_time_profiler",
196 "[CAbstractNavigator::navigationStep()] Navigation "
224 "[CAbstractNavigator::navigationStep()] Stopping "
225 "Navigation due to a NAV_ERROR state!");
272 std::string(
"doEmergencyStop called for: ") + msg;
292 params->target.targetDesiredRelSpeed >= .0 &&
293 params->target.targetDesiredRelSpeed <= 1.0);
332 const double robot_time_secs =
336 const double MIN_TIME_BETWEEN_POSE_UPDATES = 20e-3;
339 const double last_call_age =
341 if (last_call_age < MIN_TIME_BETWEEN_POSE_UPDATES)
345 "updateCurrentPoseAndSpeeds: ignoring call, since last call "
346 "was only %f ms ago.",
347 last_call_age * 1e3);
365 "ERROR calling m_robot.getCurrentPoseAndSpeeds, stopping robot "
366 "and finishing navigation");
375 "ERROR calling m_robot.getCurrentPoseAndSpeeds, stopping robot "
376 "and finishing navigation");
377 throw std::runtime_error(
378 "ERROR calling m_robot.getCurrentPoseAndSpeeds, stopping robot "
379 "and finishing navigation");
386 const bool changed_frame_id =
390 if (changed_frame_id)
449 dist_to_target_for_sending_event,
450 "Default value=0, means use the `targetAllowedDistance` passed by the "
451 "user in the navigation request.");
453 alarm_seems_not_approaching_target_timeout,
454 "navigator timeout (seconds) [Default=30 sec]");
456 dist_check_target_is_blocked,
457 "When closer than this distance, check if the target is blocked to "
458 "abort navigation with an error. [Default=0.6 m]");
460 dist_to_target_for_sending_event,
461 "Default value=0, means use the `targetAllowedDistance` passed by the"
462 " user in the navigation request.");
464 alarm_seems_not_approaching_target_timeout,
465 "navigator timeout (seconds) [Default=30 sec]");
467 dist_check_target_is_blocked,
468 "When closer than this distance, check if the target is blocked to "
469 "abort navigation with an error. [Default=0.6 m]");
471 hysteresis_check_target_is_blocked,
472 "How many steps should the condition for dist_check_target_is_blocked "
473 "be fulfilled to raise an event");
480 return typeid(a) ==
typeid(b) && a.
isEqual(b);
486 return (targetDist < m_navigationParams->target.targetAllowedDistance);
500 bool call_virtual_nav_method)
508 "[CAbstractNavigator::navigationStep()] Starting Navigation. "
509 "Watchdog initiated...\n");
512 "[CAbstractNavigator::navigationStep()] Navigation "
545 const double targetDist = seg_robot_mov.
distance(
566 "Navigation target (%.03f,%.03f) was reached\n",
598 .alarm_seems_not_approaching_target_timeout)
601 "Timeout approaching the target. Aborting navigation.");
606 "Timeout approaching the target. Aborting navigation.");
625 const bool send_event =
634 "Target seems to be blocked by obstacles. Invoking"
635 " sendCannotGetCloserToBlockedTargetEvent().");
639 sendCannotGetCloserToBlockedTargetEvent,
653 if (call_virtual_nav_method)
658 catch (
const std::exception& e)
665 std::string(
"Exception: ") + std::string(e.what());
669 "[CAbstractNavigator::navigationStep] Exception:\n %s", e.what());
682 "[CAbstractNavigator::navigationStep] Untyped exception!");
TState m_navigationState
Current internal state of navigator:
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)
Default: forward call to m_robot.changeSpeed().
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)=0
Sends a velocity command to the robot.
double timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds
virtual void onNavigateCommandReceived()
Called after each call to CAbstractNavigator::navigate()
mrpt::system::TTimeStamp timestamp
virtual void sendNavigationEndEvent()
Callback: End of navigation command (reach of single goal, or final waypoint of waypoint list)
virtual void performNavigationStep()=0
To be implemented in derived classes.
double dist_to_target_for_sending_event
Default value=0, means use the "targetAllowedDistance" passed by the user in the navigation request.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes.
#define MRPT_LOG_INFO(_STRING)
void dispatchPendingNavEvents()
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
virtual bool checkHasReachedTarget(const double targetDist) const
Default implementation: check if target_dist is below the accepted distance.
TState
The different states for the navigation system.
void setVerbosityLevel(const VerbosityLevel level)
alias of setMinLoggingLevel()
double m_last_curPoseVelUpdate_robot_time
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
double m_badNavAlarm_minDistTarget
For sending an alarm (error event) when it seems that we are not approaching toward the target in a w...
double phi
Orientation (rads)
std::string pose_frame_id
map frame ID for pose
TErrorReason m_navErrorReason
mrpt::math::TTwist2D velLocal
void rotate(const double ang)
Transform the (vx,vy) components for a counterclockwise rotation of ang radians.
void internal_onStartNewNavigation()
Called before starting a new navigation.
virtual void navigationStep()
This method must be called periodically in order to effectively run the navigation.
void setFrameTF(const std::weak_ptr< mrpt::poses::FrameTransformer< 2 >> &frame_tf)
Sets a user-provided frame transformer object; used only if providing targets in a frame ID different...
bool m_rethrow_exceptions
std::unique_ptr< TNavigationParams > m_navigationParams
Current navigation parameters.
virtual bool startWatchdog(float T_ms)
Start the watchdog timer of the robot platform, if any, for maximum expected delay between consecutiv...
virtual void updateCurrentPoseAndSpeeds()
Call to the robot getCurrentPoseAndSpeeds() and updates members m_curPoseVel accordingly.
virtual void processNavigateCommand(const TNavigationParams *params)
Does the job of navigate(), except the call to onNavigateCommandReceived()
virtual bool changeSpeedsNOP()
Just like changeSpeeds(), but will be called when the last velocity command is still the preferred so...
virtual double getNavigationTime()
Returns the number of seconds ellapsed since the constructor of this class was invoked,...
void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string &s) const override
This method saves the options to a ".ini"-like file or memory-stored string list.
float targetAllowedDistance
(Default=0.5 meters) Allowed distance to target in order to end the navigation.
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
#define MRPT_LOG_THROTTLE_WARN(_PERIOD_SECONDS, _STRING)
iterator erase(iterator element_to_erase)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool m_navigationEndEventSent
Will be false until the navigation end is sent, and it is reset with each new command.
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
bool targetIsRelative
(Default=false) Whether the target coordinates are in global coordinates (false) or are relative to t...
@ ERR_CANNOT_REACH_TARGET
#define ASSERT_(f)
Defines an assertion mechanism.
TAbstractNavigatorParams()
void setInterpolationMethod(TInterpolatorMethod method)
Change the method used to interpolate the robot path.
bool isEqual(const TNavigationParamsBase &o) const override
reverse_iterator rbegin()
TAbstractNavigatorParams params_abstract_navigator
void insert(const mrpt::Clock::time_point &t, const pose_t &p)
Inserts a new pose in the sequence.
virtual void onStartNewNavigation()=0
Called whenever a new navigation has been started.
virtual void loadConfigFile(const mrpt::config::CConfigFileBase &c)
Loads all params from a file.
virtual bool stopWatchdog()
Stop the watchdog timer.
The struct for configuring navigation requests.
void logFmt(const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
Alternative logging method, which mimics the printf behavior.
bool operator==(const CAbstractNavigator::TNavigationParamsBase &, const CAbstractNavigator::TNavigationParamsBase &)
TState m_lastNavigationState
Last internal state of navigator:
virtual void performNavigationStepNavigating(bool call_virtual_nav_method=true)
Factorization of the part inside navigationStep(), for the case of state being NAVIGATING.
double targetDesiredRelSpeed
(Default=.05) Desired relative speed (wrt maximum speed), in range [0,1], of the vehicle at target.
std::string target_frame_id
(Default="map") Frame ID in which target is given.
#define MRPT_LOAD_CONFIG_VAR_CS(variableName, variableType)
Shortcut for MRPT_LOAD_CONFIG_VAR() for config file object named c and section string named s
mrpt::math::TPose2D rawOdometry
raw odometry (frame does not match to "pose", but is expected to be smoother in the short term).
This class allows loading and storing values and vectors of different types from a configuration text...
virtual void doEmergencyStop(const std::string &msg)
Stops the robot and set navigation state to error.
virtual void sendWaySeemsBlockedEvent()
Callback: No progression made towards target for a predefined period of time.
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
#define MRPT_LOG_ERROR(_STRING)
mrpt::math::TPose2D target_coords
Coordinates of desired target location.
mrpt::poses::CPose2DInterpolator m_latestOdomPoses
CRobot2NavInterface & m_robot
The navigator-robot interface.
void clear()
Clears the current sequence of poses.
std::string getAsText() const override
Gets navigation params as a human-readable format.
std::string getAsText() const
Gets navigation params as a human-readable format.
virtual bool stop(bool isEmergencyStop)
Default: forward call to m_robot.stop().
void getContent(std::string &str) const
Return the current contents of the virtual "config file".
2D segment, consisting of two points.
mrpt::vision::TStereoCalibParams params
TRobotPoseVel m_curPoseVel
Current robot pose (updated in CAbstractNavigator::navigationStep() )
virtual bool getCurrentPoseAndSpeeds(mrpt::math::TPose2D &curPose, mrpt::math::TTwist2D &curVelGlobal, mrpt::system::TTimeStamp ×tamp, mrpt::math::TPose2D &curOdometry, std::string &frame_id)=0
Get the current pose and velocity of the robot.
~CAbstractNavigator() override
dtor
#define MRPT_LOG_WARN(_STRING)
virtual void suspend()
Suspend current navegation.
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
mrpt::system::CTimeLogger m_navProfiler
Publicly available time profiling object.
virtual void resetNavError()
Resets a NAV_ERROR state back to IDLE
double distance(const TPoint2D &point) const
Distance to point.
virtual bool changeSpeedsNOP()
Default: forward call to m_robot.changeSpeedsNOP().
virtual bool stop(bool isEmergencyStop=true)=0
Stop the robot right now.
mrpt::system::TTimeStamp getCurrentTime()
Returns the current (UTC) system time.
int hysteresis_check_target_is_blocked
(Default=3) How many steps should the condition for dist_check_target_is_blocked be fulfilled to rais...
virtual bool checkCollisionWithLatestObstacles(const mrpt::math::TPose2D &relative_robot_pose) const
Checks whether the robot shape, when placed at the given pose (relative to the current pose),...
void enable(bool enabled=true)
virtual bool isEqual(const TNavigationParamsBase &o) const =0
LockHelper< T > lockHelper(T &t)
Syntactic sugar to easily create a locker to any kind of std::mutex.
std::weak_ptr< mrpt::poses::FrameTransformer< 2 > > m_frame_tf
Optional, user-provided frame transformer.
#define MRPT_LOG_THROTTLE_DEBUG_FMT(_PERIOD_SECONDS, _FMT_STRING,...)
Usage: MRPT_LOG_THROTTLE_DEBUG_FMT(5.0, "i=%u", i);
std::string m_last_curPoseVelUpdate_pose_frame_id
virtual void cancel()
Cancel current navegation.
virtual void sendNavigationStartEvent()
Callback: Start of navigation command.
CAbstractNavigator(CRobot2NavInterface &robot_interface_impl)
ctor
virtual void navigate(const TNavigationParams *params)
Navigation request to a single target location.
int m_counter_check_target_is_blocked
std::list< std::function< void(void)> > m_pending_events
Events generated during navigationStep(), enqueued to be called at the end of the method execution to...
std::string error_msg
Human friendly description of the error.
bool operator==(const TargetInfo &o) const
double alarm_seems_not_approaching_target_timeout
navigator timeout (seconds) [Default=30 sec]
This class implements a config file-like interface over a memory-stored string list.
mrpt::math::TTwist2D velGlobal
virtual void sendNavigationEndDueToErrorEvent()
Callback: Error asking sensory data from robot or sending motor commands.
Virtual base for velocity commands of different kinematic models of planar mobile robot.
bool targetIsIntermediaryWaypoint
double dist_check_target_is_blocked
(Default value=0.6) When closer than this distance, check if the target is blocked to abort navigatio...
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const
Saves all current options to a config file.
mrpt::poses::CPose2DInterpolator m_latestPoses
Latest robot poses (updated in CAbstractNavigator::navigationStep() )
void loadFromConfigFile(const mrpt::config::CConfigFileBase &c, const std::string &s) override
This method load the options from a ".ini"-like file or memory-stored string list.
std::string std::string format(std::string_view fmt, ARGS &&... args)
COutputLogger()
Default class constructor.
mrpt::system::TTimeStamp m_badNavAlarm_lastMinDistTime
Base for all high-level navigation commands.
std::recursive_mutex m_nav_cs
mutex for all navigation methods
virtual void resume()
Continues with suspended navigation.
const double PREVIOUS_POSES_MAX_AGE
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Thu May 21 21:53:32 UTC 2020 | |