navigation2DClient
: A device which allows a client application to perform navigation tasks, such as commanding the robot to reach a specific location in a map.
More...
#include <navigation2DClient/Navigation2DClient.h>
Public Member Functions | |
bool | open (yarp::os::Searchable &config) override |
Open the DeviceDriver. More... | |
bool | close () override |
Close the DeviceDriver. More... | |
bool | parse_respond_string (const yarp::os::Bottle &command, yarp::os::Bottle &reply) |
virtual bool | read (yarp::os::ConnectionReader &connection) override |
Read this object from a network connection. More... | |
virtual bool | checkInsideArea (yarp::dev::Nav2D::Map2DArea area) override |
Check if the robot is currently inside the specified area. More... | |
virtual bool | checkInsideArea (std::string area_name) override |
Check if the robot is currently inside the specified area. More... | |
virtual bool | checkNearToLocation (yarp::dev::Nav2D::Map2DLocation loc, double linear_tolerance, double angular_tolerance=std::numeric_limits< double >::infinity()) override |
Check if the robot is currently near to the specified area. More... | |
virtual bool | checkNearToLocation (std::string location_name, double linear_tolerance, double angular_tolerance=std::numeric_limits< double >::infinity()) override |
Check if the robot is currently near to the specified area. More... | |
bool | gotoTargetByAbsoluteLocation (yarp::dev::Nav2D::Map2DLocation loc) override |
Ask the robot to reach a position defined in the world reference frame. More... | |
bool | gotoTargetByLocationName (std::string location_name) override |
Ask the robot to reach a previously stored location/area. More... | |
bool | gotoTargetByRelativeLocation (double x, double y, double theta) override |
Ask the robot to reach a position defined in the robot reference frame. More... | |
bool | gotoTargetByRelativeLocation (double x, double y) override |
Ask the robot to reach a position defined in the robot reference frame. More... | |
bool | applyVelocityCommand (double x_vel, double y_vel, double theta_vel, double timeout=0.1) override |
Apply a velocity command. More... | |
bool | recomputeCurrentNavigationPath () override |
Forces the navigation system to recompute the path from the current robot position to the current goal. More... | |
bool | getAbsoluteLocationOfCurrentTarget (yarp::dev::Nav2D::Map2DLocation &loc) override |
Gets the last navigation target in the world reference frame. More... | |
bool | getNameOfCurrentTarget (std::string &location_name) override |
Gets the name of the current target, if available (set by gotoTargetByLocationName) More... | |
bool | getRelativeLocationOfCurrentTarget (double &x, double &y, double &theta) override |
Gets the last navigation target in the robot reference frame. More... | |
bool | getCurrentPosition (yarp::dev::Nav2D::Map2DLocation &loc) override |
Gets the current position of the robot w.r.t world reference frame. More... | |
bool | getEstimatedOdometry (yarp::dev::OdometryData &odom) override |
Gets the estimated odometry the robot, including its velocity expressed in the world and in the local reference frame. More... | |
bool | setInitialPose (const yarp::dev::Nav2D::Map2DLocation &loc) override |
Sets the initial pose for the localization algorithm which estimates the current position of the robot w.r.t world reference frame. More... | |
bool | getLocalizationStatus (yarp::dev::Nav2D::LocalizationStatusEnum &status) override |
Gets the current status of the localization task. More... | |
bool | getEstimatedPoses (std::vector< yarp::dev::Nav2D::Map2DLocation > &poses) override |
Gets a set of pose estimates computed by the localization algorithm. More... | |
bool | setInitialPose (const yarp::dev::Nav2D::Map2DLocation &loc, const yarp::sig::Matrix &cov) override |
Sets the initial pose for the localization algorithm which estimates the current position of the robot w.r.t world reference frame. More... | |
bool | getCurrentPosition (yarp::dev::Nav2D::Map2DLocation &loc, yarp::sig::Matrix &cov) override |
Gets the current position of the robot w.r.t world reference frame, plus the covariance. More... | |
bool | startLocalizationService () override |
Starts the localization service. More... | |
bool | stopLocalizationService () override |
Stops the localization service. More... | |
bool | storeCurrentPosition (std::string location_name) override |
Store the current location of the robot. More... | |
bool | storeLocation (std::string location_name, yarp::dev::Nav2D::Map2DLocation loc) override |
Store a location specified by the user in the world reference frame. More... | |
bool | getLocation (std::string location_name, yarp::dev::Nav2D::Map2DLocation &loc) override |
Retrieves a location previously stored by the user. More... | |
bool | getArea (std::string area_name, yarp::dev::Nav2D::Map2DArea &area) override |
Retrieves an area previously stored by the user. More... | |
bool | deleteLocation (std::string location_name) override |
Delete a location. More... | |
bool | getLocationsList (std::vector< std::string > &locations) override |
Get a list of all stored locations. More... | |
bool | getNavigationStatus (yarp::dev::Nav2D::NavigationStatusEnum &status) override |
Gets the current status of the navigation task. More... | |
bool | clearAllLocations () override |
Delete all stored locations. More... | |
bool | stopNavigation () override |
Terminates the current navigation task. More... | |
bool | suspendNavigation (const double time_s) override |
Ask to the robot to suspend the current navigation task for a defined amount of time. More... | |
bool | resumeNavigation () override |
Resume a previously suspended navigation task. More... | |
bool | getAllNavigationWaypoints (yarp::dev::Nav2D::TrajectoryTypeEnum trajectory_type, yarp::dev::Nav2D::Map2DPath &waypoints) override |
Returns the list of waypoints generated by the navigation algorithm. More... | |
bool | getCurrentNavigationWaypoint (yarp::dev::Nav2D::Map2DLocation &curr_waypoint) override |
Returns the current waypoint pursued by the navigation algorithm. More... | |
bool | getCurrentNavigationMap (yarp::dev::Nav2D::NavigationMapTypeEnum map_type, yarp::dev::Nav2D::MapGrid2D &map) override |
Returns the current navigation map processed by the navigation algorithm. More... | |
Public Member Functions inherited from yarp::dev::DeviceDriver | |
~DeviceDriver () override=default | |
Destructor. More... | |
template<class T > | |
bool | view (T *&x) |
Get an interface to the device driver. More... | |
virtual DeviceDriver * | getImplementation () |
Some drivers are bureaucrats, pointing at others. More... | |
Public Member Functions inherited from yarp::os::IConfig | |
virtual | ~IConfig () |
Destructor. More... | |
virtual bool | configure (Searchable &config) |
Change online parameters. More... | |
Public Member Functions inherited from yarp::dev::Nav2D::INavigation2D | |
virtual | ~INavigation2D () |
Destructor. More... | |
Public Member Functions inherited from yarp::dev::Nav2D::INavigation2DTargetActions | |
virtual | ~INavigation2DTargetActions () |
Destructor. More... | |
Public Member Functions inherited from yarp::dev::Nav2D::INavigation2DControlActions | |
virtual | ~INavigation2DControlActions () |
Destructor. More... | |
Public Member Functions inherited from yarp::dev::Nav2D::ILocalization2D | |
virtual | ~ILocalization2D () |
Destructor. More... | |
Public Member Functions inherited from yarp::os::PortReader | |
virtual | ~PortReader () |
Destructor. More... | |
virtual Type | getReadType () const |
Protected Attributes | |
std::mutex | m_mutex |
yarp::os::Port | m_rpc_port_navigation_server |
yarp::os::Port | m_rpc_port_map_locations_server |
yarp::os::Port | m_rpc_port_localization_server |
yarp::os::Port | m_rpc_port_user_commands |
std::string | m_local_name |
std::string | m_navigation_server_name |
std::string | m_map_locations_server_name |
std::string | m_localization_server_name |
int | m_period |
navigation2DClient
: A device which allows a client application to perform navigation tasks, such as commanding the robot to reach a specific location in a map.
Parameters required by this device are:
Parameter name | SubParameter | Type | Units | Default Value | Required | Description | Notes |
---|---|---|---|---|---|---|---|
local | - | string | - | - | Yes | Full port name opened by the Navigation2DClient device. | |
navigation_server | - | string | - | - | Yes | Full port name of the port remotely opened by the Navigation server, to which the Navigation2DClient connects to. | |
map_locations_server | - | string | - | - | Yes | Full port name of the port remotely opened by the Map2DServer, to which the Navigation2DClient connects to. | |
localization_server | - | string | - | - | Yes | Full port name of the port remotely opened by the Localization server, to which the Navigation2DClient connects to. |
Definition at line 52 of file Navigation2DClient.h.
|
overridevirtual |
Apply a velocity command.
velocities are expressed in the robot reference frame
x | [m/s] |
y | [m/s] |
theta | [deg/s] |
timeout | The velocity command expires after the specified amount of time (by default 0.1 seconds) |
Implements yarp::dev::Nav2D::INavigation2DTargetActions.
Definition at line 1496 of file Navigation2DClient.cpp.
|
overridevirtual |
Check if the robot is currently inside the specified area.
area_name | the name of an area previously saved |
Implements yarp::dev::Nav2D::INavigation2D.
Definition at line 635 of file Navigation2DClient.cpp.
|
overridevirtual |
Check if the robot is currently inside the specified area.
area | the area to be checked |
Implements yarp::dev::Nav2D::INavigation2D.
Definition at line 617 of file Navigation2DClient.cpp.
|
overridevirtual |
Check if the robot is currently near to the specified area.
location_name | the name of the location: it will be searched in the server |
linear_tolerance | linear tolerance [m] |
angular_tolerance | [deg] |
Implements yarp::dev::Nav2D::INavigation2D.
Definition at line 598 of file Navigation2DClient.cpp.
|
overridevirtual |
Check if the robot is currently near to the specified area.
loc | the location to be checked |
linear_tolerance | linear tolerance [m] |
angular_tolerance | [deg 0-360] |
Implements yarp::dev::Nav2D::INavigation2D.
Definition at line 586 of file Navigation2DClient.cpp.
|
overridevirtual |
Delete all stored locations.
Implements yarp::dev::Nav2D::INavigation2D.
Definition at line 1259 of file Navigation2DClient.cpp.
|
overridevirtual |
Close the DeviceDriver.
Reimplemented from yarp::dev::DeviceDriver.
Definition at line 180 of file Navigation2DClient.cpp.
|
overridevirtual |
Delete a location.
location_name | the name of the location |
Implements yarp::dev::Nav2D::INavigation2D.
Definition at line 1232 of file Navigation2DClient.cpp.
|
overridevirtual |
Gets the last navigation target in the world reference frame.
loc | the location of the robot |
Implements yarp::dev::Nav2D::INavigation2DTargetActions.
Definition at line 947 of file Navigation2DClient.cpp.
|
overridevirtual |
Returns the list of waypoints generated by the navigation algorithm.
trajectory_type | specifies if we are requesting the waypoints of the global trajectory or the waypoints of the local trajectory |
waypoints | the list of waypoints generated by the navigation algorithm |
Implements yarp::dev::Nav2D::INavigation2DControlActions.
Definition at line 1335 of file Navigation2DClient.cpp.
|
overridevirtual |
Retrieves an area previously stored by the user.
area_name | the name of the area |
area | the area on the map |
Implements yarp::dev::Nav2D::INavigation2D.
Definition at line 1193 of file Navigation2DClient.cpp.
|
overridevirtual |
Returns the current navigation map processed by the navigation algorithm.
map_type | the map to be requested (e.g. global, local, etc.) |
map | the map, currently used by the navigation algorithm |
Implements yarp::dev::Nav2D::INavigation2DControlActions.
Definition at line 1427 of file Navigation2DClient.cpp.
|
overridevirtual |
Returns the current waypoint pursued by the navigation algorithm.
curr_waypoint | the current waypoint pursued by the navigation algorithm |
Implements yarp::dev::Nav2D::INavigation2DControlActions.
Definition at line 1385 of file Navigation2DClient.cpp.
|
overridevirtual |
Gets the current position of the robot w.r.t world reference frame.
loc | the location of the robot |
Implements yarp::dev::Nav2D::ILocalization2D.
Definition at line 888 of file Navigation2DClient.cpp.
|
overridevirtual |
Gets the current position of the robot w.r.t world reference frame, plus the covariance.
loc | the location of the robot |
cov | the 3x3 covariance matrix |
Implements yarp::dev::Nav2D::ILocalization2D.
Definition at line 841 of file Navigation2DClient.cpp.
|
overridevirtual |
Gets the estimated odometry the robot, including its velocity expressed in the world and in the local reference frame.
loc | the estimated odometry. |
Implements yarp::dev::Nav2D::ILocalization2D.
Definition at line 882 of file Navigation2DClient.cpp.
|
overridevirtual |
Gets a set of pose estimates computed by the localization algorithm.
Implements yarp::dev::Nav2D::ILocalization2D.
Definition at line 1527 of file Navigation2DClient.cpp.
|
overridevirtual |
Gets the current status of the localization task.
Implements yarp::dev::Nav2D::ILocalization2D.
Definition at line 1466 of file Navigation2DClient.cpp.
|
overridevirtual |
Retrieves a location previously stored by the user.
location_name | the name of the location |
loc | the location on the map |
Implements yarp::dev::Nav2D::INavigation2D.
Definition at line 1159 of file Navigation2DClient.cpp.
|
overridevirtual |
Get a list of all stored locations.
the | returned list of locations |
Implements yarp::dev::Nav2D::INavigation2D.
Definition at line 1115 of file Navigation2DClient.cpp.
|
overridevirtual |
Gets the name of the current target, if available (set by gotoTargetByLocationName)
location_name | the name of the current target |
Implements yarp::dev::Nav2D::INavigation2D.
Definition at line 980 of file Navigation2DClient.cpp.
|
overridevirtual |
Gets the current status of the navigation task.
Implements yarp::dev::Nav2D::INavigation2DControlActions.
Definition at line 526 of file Navigation2DClient.cpp.
|
overridevirtual |
Gets the last navigation target in the robot reference frame.
x | |
y | |
theta |
Implements yarp::dev::Nav2D::INavigation2DTargetActions.
Definition at line 994 of file Navigation2DClient.cpp.
|
overridevirtual |
Ask the robot to reach a position defined in the world reference frame.
loc | the location to be reached |
Implements yarp::dev::Nav2D::INavigation2DTargetActions.
Definition at line 556 of file Navigation2DClient.cpp.
|
overridevirtual |
Ask the robot to reach a previously stored location/area.
location_name | the name of a location/area previously saved |
Implements yarp::dev::Nav2D::INavigation2D.
Definition at line 660 of file Navigation2DClient.cpp.
|
overridevirtual |
Ask the robot to reach a position defined in the robot reference frame.
The final orientation of the goal is unspecified.
x | |
y |
Implements yarp::dev::Nav2D::INavigation2DTargetActions.
Definition at line 692 of file Navigation2DClient.cpp.
|
overridevirtual |
Ask the robot to reach a position defined in the robot reference frame.
x | |
y | |
theta |
Implements yarp::dev::Nav2D::INavigation2DTargetActions.
Definition at line 721 of file Navigation2DClient.cpp.
|
overridevirtual |
Open the DeviceDriver.
config | is a list of parameters for the device. Which parameters are effective for your device can vary. See device invocation examples. If there is no example for your device, you can run the "yarpdev" program with the verbose flag set to probe what parameters the device is checking. If that fails too, you'll need to read the source code (please nag one of the yarp developers to add documentation for your device). |
Reimplemented from yarp::dev::DeviceDriver.
Definition at line 62 of file Navigation2DClient.cpp.
bool Navigation2DClient::parse_respond_string | ( | const yarp::os::Bottle & | command, |
yarp::os::Bottle & | reply | ||
) |
Definition at line 189 of file Navigation2DClient.cpp.
|
overridevirtual |
Read this object from a network connection.
Override this for your particular class.
reader | an interface to the network connection for reading |
Implements yarp::os::PortReader.
Definition at line 500 of file Navigation2DClient.cpp.
|
overridevirtual |
Forces the navigation system to recompute the path from the current robot position to the current goal.
If no goal has been set, the command has no effect.
Implements yarp::dev::Nav2D::INavigation2DControlActions.
Definition at line 751 of file Navigation2DClient.cpp.
|
overridevirtual |
Resume a previously suspended navigation task.
Implements yarp::dev::Nav2D::INavigation2DControlActions.
Definition at line 1310 of file Navigation2DClient.cpp.
|
overridevirtual |
Sets the initial pose for the localization algorithm which estimates the current position of the robot w.r.t world reference frame.
loc | the location of the robot |
Implements yarp::dev::Nav2D::ILocalization2D.
Definition at line 776 of file Navigation2DClient.cpp.
|
overridevirtual |
Sets the initial pose for the localization algorithm which estimates the current position of the robot w.r.t world reference frame.
loc | the location of the robot |
cov | the 3x3 covariance matrix |
Implements yarp::dev::Nav2D::ILocalization2D.
Definition at line 805 of file Navigation2DClient.cpp.
|
overridevirtual |
Starts the localization service.
Implements yarp::dev::Nav2D::ILocalization2D.
Definition at line 1594 of file Navigation2DClient.cpp.
|
overridevirtual |
Stops the localization service.
Implements yarp::dev::Nav2D::ILocalization2D.
Definition at line 1619 of file Navigation2DClient.cpp.
|
overridevirtual |
Terminates the current navigation task.
Cannot be resumed.
Implements yarp::dev::Nav2D::INavigation2DControlActions.
Definition at line 1285 of file Navigation2DClient.cpp.
|
overridevirtual |
Store the current location of the robot.
location_name | the name of the location |
Implements yarp::dev::Nav2D::INavigation2D.
Definition at line 1026 of file Navigation2DClient.cpp.
|
overridevirtual |
Store a location specified by the user in the world reference frame.
location_name | the name of the location |
loc | the location of the robot |
Implements yarp::dev::Nav2D::INavigation2D.
Definition at line 1084 of file Navigation2DClient.cpp.
|
overridevirtual |
Ask to the robot to suspend the current navigation task for a defined amount of time.
Can be resumed by resume().
time_s |
Implements yarp::dev::Nav2D::INavigation2DControlActions.
Definition at line 921 of file Navigation2DClient.cpp.
|
protected |
Definition at line 63 of file Navigation2DClient.h.
|
protected |
Definition at line 66 of file Navigation2DClient.h.
|
protected |
Definition at line 65 of file Navigation2DClient.h.
|
protected |
Definition at line 58 of file Navigation2DClient.h.
|
protected |
Definition at line 64 of file Navigation2DClient.h.
|
protected |
Definition at line 67 of file Navigation2DClient.h.
|
protected |
Definition at line 61 of file Navigation2DClient.h.
|
protected |
Definition at line 60 of file Navigation2DClient.h.
|
protected |
Definition at line 59 of file Navigation2DClient.h.
|
protected |
Definition at line 62 of file Navigation2DClient.h.