Go to the documentation of this file.
22 std::string s = TNavigationParams::getAsText();
23 if (!multiple_targets.empty())
25 s +=
"multiple_targets:\n";
27 for (
const auto& e : multiple_targets)
42 return o !=
nullptr &&
44 multiple_targets == o->multiple_targets;
75 const size_t N = nav_request.
waypoints.size();
76 ASSERTMSG_(N > 0,
"List of waypoints is empty!");
80 for (
size_t i = 0; i < N; i++)
163 const double dist2target = robot_move_seg.
distance(wp.target);
164 if (dist2target < wp.allowed_distance ||
167 bool consider_wp_reached =
false;
170 consider_wp_reached =
true;
178 const double tim_since_last_align =
181 const double ALIGN_WAIT_TIME = 1.5;
183 if (std::abs(ang_err) <=
185 .waypoint_angle_tolerance &&
188 tim_since_last_align > ALIGN_WAIT_TIME)
190 consider_wp_reached =
true;
204 "[CWaypointsNavigator::navigationStep] "
205 "Trying to align to heading: %.02f deg. "
206 "Relative heading: %.02f deg. "
207 "With motion cmd: %s",
210 align_cmd ? align_cmd->asString().c_str()
211 :
"nullptr (operation not "
212 "supported by this robot)");
224 consider_wp_reached =
233 "[CWaypointsNavigator::navigationStep] "
234 "Waiting for the robot to get aligned: "
235 "current_heading=%.02f deg "
236 "target_heading=%.02f deg",
243 if (consider_wp_reached)
246 "[CWaypointsNavigator::navigationStep] Waypoint "
250 " segment-to-target dist: "
252 <<
", allowed_dist: " << wp.allowed_distance);
258 new_events.emplace_back(std::bind(
290 const int most_advanced_wp_at_begin = most_advanced_wp;
295 if (idx < 0)
continue;
296 if (wps.
waypoints[idx].reached)
continue;
301 wps.
waypoints[idx].target, wp_local_wrt_robot);
304 .max_distance_to_allow_skip_waypoint > 0 &&
305 wp_local_wrt_robot.
norm() >
310 const bool is_reachable =
318 if (++wps.
waypoints[idx].counter_seen_reachable >
322 most_advanced_wp = idx;
332 if (most_advanced_wp >= 0)
335 for (
int k = most_advanced_wp_at_begin;
336 k < most_advanced_wp; k++)
343 new_events.emplace_back(std::bind(
345 std::ref(
m_robot), k,
false ));
384 wp_last_idx < int(wps.
waypoints.size()) - 1 &&
395 wp_idx <= wp_last_idx; wp_idx++)
398 const bool is_final_wp =
399 ((wp_idx + 1) ==
int(wps.
waypoints.size()));
427 "[CWaypointsNavigator::navigationStep] Active waypoint "
428 "changed. Current status:\n"
513 max_distance_to_allow_skip_waypoint,
514 "Max distance to `foresee` waypoints [meters]. (<0: unlimited)");
516 min_timesteps_confirm_skip_waypoints,
517 "Min timesteps a `future` waypoint must be seen as reachable to become "
520 "waypoint_angle_tolerance", waypoint_angle_tolerance,
521 "Angular error tolerance for waypoints with an assigned heading [deg] "
524 multitarget_look_ahead,
525 ">=0 number of waypoints to forward to the underlying navigation "
526 "engine, to ease obstacles avoidance when a waypoint is blocked "
527 "(Default=0 : none)");
548 wp = (!wps.waypoints.empty() && wps.waypoint_index_current_goal >= 0 &&
549 wps.waypoint_index_current_goal < (int)wps.waypoints.size())
550 ? &wps.waypoints[wps.waypoint_index_current_goal]
554 targetDist <= m_navigationParams->target.targetAllowedDistance) ||
562 "CWaypointsNavigator::checkHasReachedTarget() called "
564 << targetDist <<
" return=" << ret
565 <<
" waypoint: " << (wp ==
nullptr ? std::string(
"") : wp->
getAsText())
566 <<
"wps.timestamp_nav_started="
568 ?
"INVALID_TIMESTAMP"
570 wps.timestamp_nav_started)));
TState m_navigationState
Current internal state of navigator:
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)
Default: forward call to m_robot.changeSpeed().
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
bool isRelativePointReachable(const mrpt::math::TPoint2D &wp_local_wrt_robot) const
Returns true if, according to the information gathered at the last navigation step,...
static const int INVALID_NUM
The default value of fields (used to detect non-set values)
virtual void onNavigateCommandReceived()
Called after each call to CAbstractNavigator::navigate()
double target_heading
[Default=any heading] Optionally, set to the desired orientation [radians] of the robot at this waypo...
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
std::string dateTimeLocalToString(const mrpt::system::TTimeStamp t)
Convert a timestamp into this textual form (in local time): YEAR/MONTH/DAY,HH:MM:SS....
#define MRPT_LOG_THROTTLE_INFO_FMT(_PERIOD_SECONDS, _FMT_STRING,...)
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Individual target info in CAbstractNavigator::TNavigationParamsBase and derived classes.
void dispatchPendingNavEvents()
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
virtual bool impl_waypoint_is_reachable(const mrpt::math::TPoint2D &wp_local_wrt_robot) const =0
Implements the way to waypoint is free function in children classes: true must be returned if,...
double phi
Orientation (rads)
virtual void navigationStep()
This method must be called periodically in order to effectively run the navigation.
bool m_was_aligning
Whether the last timestep was "is_aligning" in a waypoint with heading.
bool reached
Whether this waypoint has been reached already (to within the allowed distance as per user specificat...
std::unique_ptr< TNavigationParams > m_navigationParams
Current navigation parameters.
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations,...
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()
TargetInfo target
Navigation target.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
mrpt::system::TTimeStamp m_last_alignment_cmd
float targetAllowedDistance
(Default=0.5 meters) Allowed distance to target in order to end the navigation.
void navigationStep() override
This method must be called periodically in order to effectively run the navigation.
bool checkHasReachedTarget(const double targetDist) const override
Default implementation: check if target_dist is below the accepted distance.
~CWaypointsNavigator() override
dtor
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
This is the base class for any reactive/planned navigation system.
The struct for configuring navigation requests to CWaypointsNavigator and derived classes.
TWaypointStatusSequence getWaypointNavStatus() const
Get a copy of the control structure which describes the progress status of the waypoint navigation.
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...
#define ASSERT_(f)
Defines an assertion mechanism.
int waypoint_index_current_goal
Index in waypoints of the waypoint the navigator is currently trying to reach.
std::vector< TWaypoint > waypoints
bool isEqual(const TNavigationParamsBase &o) const override
virtual mrpt::kinematics::CVehicleVelCmd::Ptr getAlignCmd(const double relative_heading_radians)
Gets a motion command to make the robot to align with a given relative heading, without translating.
T norm() const
Point norm: |v| = sqrt(x^2+y^2)
virtual void waypoints_navigationStep()
The waypoints-specific part of navigationStep()
void onNavigateCommandReceived() override
Called after each call to CAbstractNavigator::navigate()
virtual void loadConfigFile(const mrpt::config::CConfigFileBase &c)
Loads all params from a file.
void cancel() override
Cancel current navegation.
TWaypointsNavigatorParams params_waypoints_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.
double max_distance_to_allow_skip_waypoint
In meters.
This class allows loading and storing values and vectors of different types from a configuration text...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
constexpr double RAD2DEG(const double x)
Radians to degrees.
mrpt::system::TTimeStamp timestamp_nav_started
Timestamp of user navigation command.
mrpt::math::TPose2D target_coords
Coordinates of desired target location.
CRobot2NavInterface & m_robot
The navigator-robot interface.
bool final_goal_reached
Whether the final waypoint has been reached successfuly.
#define MRPT_SAVE_CONFIG_VAR_DEGREES_COMMENT( __entryName, __variable, __comment)
virtual bool stop(bool isEmergencyStop)
Default: forward call to m_robot.stop().
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.
return_t square(const num_t x)
Inline function for the square of a number.
std::vector< mrpt::nav::CAbstractNavigator::TargetInfo > multiple_targets
If not empty, this will prevail over the base class single goal target.
2D segment, consisting of two points.
std::recursive_mutex m_nav_waypoints_cs
constexpr double DEG2RAD(const double x)
Degrees to radians
TRobotPoseVel m_curPoseVel
Current robot pose (updated in CAbstractNavigator::navigationStep() )
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
void inverseComposePoint(const double gx, const double gy, double &lx, double &ly) const
Computes the 2D point L such as .
std::string getAsText() const
Gets navigation params as a human-readable format.
TWaypointsNavigatorParams()
std::shared_ptr< CVehicleVelCmd > Ptr
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.
double distance(const TPoint2D &point) const
Distance to point.
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
A waypoint with an execution status.
bool isEqual(const CAbstractNavigator::TNavigationParamsBase &o) const override
virtual void sendWaypointReachedEvent(int waypoint_index, bool reached_nSkipped)
Callback: Reached an intermediary waypoint in waypoint list navigation.
The struct for requesting navigation requests for a sequence of waypoints.
void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.
CWaypointsNavigator(CRobot2NavInterface &robot_interface_impl)
ctor
double allowed_distance
[Must be set by the user] How close should the robot get to this waypoint for it to be considered rea...
virtual void cancel()
Cancel current navegation.
mrpt::math::TPose2D last_robot_pose
Robot pose at last time step (has INVALID_NUM fields upon initialization)
std::string getAsText() const override
Gets navigation params as a human-readable format.
std::string target_frame_id
(Default="map") Frame ID in which target is given.
double speed_ratio
(Default=1.0) Desired robot speed at the target, as a ratio of the full robot speed.
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...
int min_timesteps_confirm_skip_waypoints
How many times shall a future waypoint be seen as reachable to skip to it (Default: 1)
std::vector< TWaypointStatus > waypoints
Waypoints parameters and status (reached, skipped, etc.)
mrpt::system::CTimeLogger m_timlog_delays
Time logger to collect delay-related stats.
virtual void navigateWaypoints(const TWaypointSequence &nav_request)
Waypoint navigation request.
int multitarget_look_ahead
>=0 number of waypoints to forward to the underlying navigation engine, to ease obstacles avoidance w...
TPoint2D point1
Origin point.
mrpt::math::TPoint2D target
[Must be set by the user] Coordinates of desired target location (world/global coordinates).
TPoint2D point2
Destiny point.
virtual void sendNewWaypointTargetEvent(int waypoint_index)
Callback: Heading towards a new intermediary/final waypoint in waypoint list navigation.
bool targetIsIntermediaryWaypoint
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const
Saves all current options to a config file.
void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
The struct for querying the status of waypoints navigation.
TWaypointStatusSequence m_waypoint_nav_status
The latest waypoints navigation command and the up-to-date control status.
std::string std::string format(std::string_view fmt, ARGS &&... args)
#define MRPT_LOAD_CONFIG_VAR_DEGREES( variableName, configFileObject, sectionNameStr)
Loads a double variable, stored as radians but entered in the INI-file as degrees.
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.
Base for all high-level navigation commands.
void onStartNewNavigation() override
Called whenever a new navigation has been started.
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020 | |