-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
reorganize classes in subdirectories
- Loading branch information
Showing
38 changed files
with
530 additions
and
47 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
File renamed without changes.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
File renamed without changes.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
File renamed without changes.
File renamed without changes.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,222 @@ | ||
| /* +------------------------------------------------------------------------+ | ||
| | Mobile Robot Programming Toolkit (MRPT) | | ||
| | https://www.mrpt.org/ | | ||
| | | | ||
| | Copyright (c) 2005-2021, Individual contributors, see AUTHORS file | | ||
| | See: https://www.mrpt.org/Authors - All rights reserved. | | ||
| | Released under BSD License. See: https://www.mrpt.org/License | | ||
| +------------------------------------------------------------------------+ */ | ||
| #pragma once | ||
|
|
||
| #include <mrpt/config/CConfigFileBase.h> | ||
| #include <mrpt/img/TColor.h> | ||
| #include <mrpt/math/TPoint2D.h> | ||
| #include <mrpt/math/TPose2D.h> | ||
| #include <mrpt/opengl/opengl_frwds.h> | ||
| #include <mrpt/system/datetime.h> | ||
|
|
||
| #include <any> | ||
| #include <optional> | ||
| #include <string> | ||
| #include <vector> | ||
|
|
||
| namespace selfdriving | ||
| { | ||
| /** A single waypoint within WaypointSequence. */ | ||
| struct Waypoint | ||
| { | ||
| /** Ctor with default values */ | ||
| Waypoint() = default; | ||
|
|
||
| Waypoint( | ||
| double target_x, double target_y, double allowed_distance, | ||
| bool allow_skip = true, | ||
| std::optional<double> target_heading_ = std::nullopt, | ||
| double speed_ratio_ = 1.0); | ||
|
|
||
| /** [Must be set by the user] Coordinates of desired target location | ||
| * (world/global coordinates). | ||
| * \sa target_heading */ | ||
| mrpt::math::TPoint2D target{INVALID_NUM, INVALID_NUM}; | ||
|
|
||
| /** [Default=any heading] Optionally, set to the desired orientation | ||
| * [radians] | ||
| * of the robot at this waypoint. Some navigator implementations may ignore | ||
| * this preferred heading anyway, read the docs of each implementation to | ||
| * find it out. */ | ||
| std::optional<double> targetHeading; | ||
|
|
||
| /** (Default="map") Frame ID in which target is given. Optional, use only | ||
| * for submapping applications. */ | ||
| std::string targetFrameId = "map"; | ||
|
|
||
| /** [Must be set by the user] How close should the robot get to this | ||
| * waypoint for it to be considered reached. */ | ||
| double allowedDistance{INVALID_NUM}; | ||
|
|
||
| /** (Default=1.0) Desired robot speed at the target, as a ratio of the full | ||
| * robot speed. That is: speed_ratio=1 means that the user wants the robot | ||
| * to navigate to the target and smoothly continue to the next one when | ||
| * reached. speed_ratio=0 on the other hand means that the robot should | ||
| * approach this waypoint slowing down and end up totally stopped. | ||
| */ | ||
| double speedRatio = 1.0; | ||
|
|
||
| /** [Default=true] Whether it is allowed to the navigator to proceed to a | ||
| * more advanced waypoint | ||
| * in the sequence if it determines that it is easier to skip this one | ||
| * (e.g. it seems blocked by dynamic obstacles). | ||
| * This value is ignored for the last waypoint in a sequence, since it is | ||
| * always considered to be the | ||
| * ultimate goal and hence not subject to be skipped. | ||
| */ | ||
| bool allowSkip = true; | ||
|
|
||
| /** | ||
| * This modifies the behavior of mrpt::nav::TWaypoint::allow_skip according | ||
| * to: | ||
| * | ||
| * \verbatim | ||
| * +------------+-----------------+---------------------------+ | ||
| * | allow_skip | preferNotToSkip | Waypoint obstructed | | ||
| * | | | with obstacles? | | ||
| * | | +------------+--------------+ | ||
| * | | | Yes | No | | ||
| * +------------+-----------------+------------+--------------+ | ||
| * | false | false | Trigger | Pass through | | ||
| * | (default) +-----------------+ rnav error | waypoint | | ||
| * | | true | | | | ||
| * +------------+-----------------+------------+--------------+ | ||
| * | true | false(default) | Skipped | Skipped | | ||
| * +------------+-----------------+ +--------------+ | ||
| * | true | true | | Not skipped | | ||
| * +------------+-----------------+------------+--------------+ | ||
| * \endverbatim | ||
| * | ||
| */ | ||
| bool preferNotToSkip = false; | ||
|
|
||
| /** Check whether all the minimum mandatory fields have been filled by the | ||
| * user. */ | ||
| bool isValid() const; | ||
|
|
||
| /** get in human-readable format */ | ||
| std::string getAsText() const; | ||
|
|
||
| /** The default value of fields (used to detect non-set values) */ | ||
| static constexpr int INVALID_NUM{-100000}; | ||
| }; | ||
|
|
||
| /** used in getAsOpenglVisualization() */ | ||
| struct WaypointsRenderingParams | ||
| { | ||
| WaypointsRenderingParams(); | ||
|
|
||
| double outter_radius{.3}, inner_radius{.2}; | ||
| double outter_radius_non_skippable{.3}, inner_radius_non_skippable{.0}; | ||
| double outter_radius_reached{.2}, inner_radius_reached{.1}; | ||
| double heading_arrow_len{1.0}; | ||
| mrpt::img::TColor color_regular, color_current_goal, color_reached; | ||
| bool show_labels{true}; | ||
| }; | ||
|
|
||
| /** The struct for requesting navigation requests for a sequence of waypoints. | ||
| * Used in CWaypointsNavigator::navigateWaypoints(). | ||
| * Users can directly fill in the list of waypoints manipulating the public | ||
| * field `waypoints`. | ||
| */ | ||
| struct WaypointSequence | ||
| { | ||
| std::vector<Waypoint> waypoints; | ||
|
|
||
| /** Ctor with default values */ | ||
| WaypointSequence(); | ||
|
|
||
| void clear() { waypoints.clear(); } | ||
|
|
||
| /** Gets navigation params as a human-readable format */ | ||
| std::string getAsText() const; | ||
|
|
||
| /** Renders the sequence of waypoints (previous contents of `obj` are | ||
| * cleared) */ | ||
| void getAsOpenglVisualization( | ||
| mrpt::opengl::CSetOfObjects& obj, | ||
| const WaypointsRenderingParams& params = {}) const; | ||
|
|
||
| /** Saves waypoints to a config file section */ | ||
| void save(mrpt::config::CConfigFileBase& c, const std::string& s) const; | ||
|
|
||
| /** Loads waypoints to a config file section */ | ||
| void load(const mrpt::config::CConfigFileBase& c, const std::string& s); | ||
| }; | ||
|
|
||
| /** A waypoint with an execution status. \ingroup nav_reactive */ | ||
| struct WaypointStatus : public Waypoint | ||
| { | ||
| WaypointStatus() = default; | ||
|
|
||
| /** Whether this waypoint has been reached already (to within the allowed | ||
| * distance as per user specifications) or skipped. */ | ||
| bool reached{false}; | ||
|
|
||
| /** If `reached==true` this boolean tells whether the waypoint was | ||
| * physically reached (false) or marked as reached because it was skipped | ||
| * (true). */ | ||
| bool skipped{false}; | ||
|
|
||
| /** Timestamp of when this waypoint was reached. (Default=INVALID_TIMESTAMP | ||
| * means not reached so far) */ | ||
| mrpt::system::TTimeStamp timestamp_reach = INVALID_TIMESTAMP; | ||
|
|
||
| /** (Initialized to 0 automatically) How many times this waypoint has been | ||
| * seen as "reachable" before it being the current active waypoint. */ | ||
| int counter_seen_reachable{0}; | ||
|
|
||
| /** Any user-stored custom status data */ | ||
| std::any user_status_data; | ||
|
|
||
| /** Only copies the base class Waypoint data fields */ | ||
| WaypointStatus& operator=(const Waypoint& wp); | ||
|
|
||
| /** Gets navigation params as a human-readable format */ | ||
| std::string getAsText() const; | ||
| }; | ||
|
|
||
| /** The struct for querying the status of waypoints navigation. Used in | ||
| * CWaypointsNavigator::getWaypointNavStatus(). | ||
| * \ingroup nav_reactive */ | ||
| struct WaypointStatusSequence | ||
| { | ||
| WaypointStatusSequence() = default; | ||
|
|
||
| /** Waypoints parameters and status (reached, skipped, etc.) */ | ||
| std::vector<WaypointStatus> waypoints; | ||
|
|
||
| /** Timestamp of user navigation command. */ | ||
| mrpt::system::TTimeStamp timestamp_nav_started = INVALID_TIMESTAMP; | ||
|
|
||
| /** Whether the final waypoint has been reached successfuly. */ | ||
| bool final_goal_reached{false}; | ||
|
|
||
| /** Index in `waypoints` of the waypoint the navigator is currently trying | ||
| * to reach. | ||
| * This will point to the last waypoint after navigation ends successfully. | ||
| * Its value is `-1` if navigation has not started yet */ | ||
| int waypoint_index_current_goal{-1}; | ||
|
|
||
| /** Robot pose at last time step (has INVALID_NUM fields upon | ||
| * initialization) */ | ||
| mrpt::math::TPose2D last_robot_pose{ | ||
| Waypoint::INVALID_NUM, Waypoint::INVALID_NUM, Waypoint::INVALID_NUM}; | ||
|
|
||
| /** Ctor with default values */ | ||
| /** Gets navigation params as a human-readable format */ | ||
| std::string getAsText() const; | ||
|
|
||
| /** Renders the sequence of waypoints (previous contents of `obj` are | ||
| * cleared) */ | ||
| void getAsOpenglVisualization( | ||
| mrpt::opengl::CSetOfObjects& obj, | ||
| const WaypointsRenderingParams& params = {}) const; | ||
| }; | ||
| } // namespace selfdriving |
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.