29 yCDebug(FAKENAVIGATION) <<
"config configuration: \n" << config.
toString().c_str();
31 std::string context_name;
32 std::string file_name;
34 if (config.
check(
"context")) {
35 context_name = config.
find(
"context").asString();
37 if (config.
check(
"from")) {
38 file_name = config.
find(
"from").asString();
46 std::string configFile = rf.
findFile(
"from");
47 if (configFile !=
"") {
50 yCDebug(FAKENAVIGATION) <<
"device configuration: \n" << p.
toString().c_str();
57 if (rf.
check(
"navigation_time"))
59 m_navig_duration_param = rf.
find (
"navigation_time").
asInt32();
61 if (rf.
check(
"reached_time"))
63 m_reached_duration_param = rf.
find(
"reached_time").
asInt32();
82 if (m_status == NavigationStatusEnum::navigation_status_idle)
84 m_status = NavigationStatusEnum::navigation_status_moving;
86 m_time_counter=m_navig_duration_param;
93 yCInfo(FAKENAVIGATION) <<
"gotoTargetByRelativeLocation not yet implemented";
99 yCInfo(FAKENAVIGATION) <<
"gotoTargetByRelativeLocation not yet implemented";
105 m_control_out.linear_xvel = x_vel;
106 m_control_out.linear_yvel = y_vel;
107 m_control_out.angular_vel = theta_vel;
108 m_control_out.timeout = timeout;
116 x_vel = m_control_out.linear_xvel;
117 y_vel = m_control_out.linear_yvel;
118 theta_vel = m_control_out.angular_vel;
124 m_status=NavigationStatusEnum::navigation_status_idle;
131 if (m_status == NavigationStatusEnum::navigation_status_moving)
133 m_status=NavigationStatusEnum::navigation_status_paused;
140 if (m_status == NavigationStatusEnum::navigation_status_paused)
142 m_status = NavigationStatusEnum::navigation_status_moving;
149 yCInfo(FAKENAVIGATION) <<
"getAllNavigationWaypoints not yet implemented";
155 yCInfo(FAKENAVIGATION) <<
"getCurrentNavigationWaypoint not yet implemented";
161 yCInfo(FAKENAVIGATION) <<
"getCurrentNavigationMap not yet implemented";
173 target=m_absgoal_loc;
179 if (m_status == NavigationStatusEnum::navigation_status_moving)
188 yCInfo(FAKENAVIGATION) <<
"getRelativeLocationOfCurrentTarget not yet implemented";
204 if (m_status == NavigationStatusEnum::navigation_status_moving)
206 if (m_time_counter>0)
212 m_status = NavigationStatusEnum::navigation_status_goal_reached;
213 m_time_counter = m_reached_duration_param;
216 if (m_status == NavigationStatusEnum::navigation_status_goal_reached)
218 if (m_time_counter > 0)
224 m_status = NavigationStatusEnum::navigation_status_idle;
define control board standard interfaces
bool threadInit() override
Initialization method.
bool resumeNavigation() override
Resume a previously suspended navigation task.
virtual bool close() override
Close the DeviceDriver.
bool getNavigationStatus(yarp::dev::Nav2D::NavigationStatusEnum &status) override
Gets the current status of the navigation task.
bool getCurrentNavigationWaypoint(yarp::dev::Nav2D::Map2DLocation &curr_waypoint) override
Returns the current waypoint pursued by the navigation algorithm.
bool getLastVelocityCommand(double &x_vel, double &y_vel, double &theta_vel) override
Returns the last applied velocity command.
bool getRelativeLocationOfCurrentTarget(double &x, double &y, double &theta) override
Gets the last navigation target in the robot reference frame.
bool stopNavigation() override
Terminates the current navigation task.
bool suspendNavigation(double time) override
Ask to the robot to suspend the current navigation task for a defined amount of time.
bool applyVelocityCommand(double x_vel, double y_vel, double theta_vel, double timeout=0.1) override
Apply a velocity command.
void threadRelease() override
Release method.
bool gotoTargetByRelativeLocation(double x, double y, double theta) override
Ask the robot to reach a position defined in the robot reference frame.
bool recomputeCurrentNavigationPath() override
Forces the navigation system to recompute the path from the current robot position to the current goa...
bool getAbsoluteLocationOfCurrentTarget(yarp::dev::Nav2D::Map2DLocation &target) override
Gets the last navigation target in the world reference frame.
void run() override
Loop function.
bool gotoTargetByAbsoluteLocation(yarp::dev::Nav2D::Map2DLocation loc) override
Ask the robot to reach a position defined in the world reference frame.
bool getAllNavigationWaypoints(yarp::dev::Nav2D::TrajectoryTypeEnum trajectory_type, yarp::dev::Nav2D::Map2DPath &waypoints) override
Returns the list of waypoints generated by the navigation algorithm.
bool getCurrentNavigationMap(yarp::dev::Nav2D::NavigationMapTypeEnum map_type, yarp::dev::Nav2D::MapGrid2D &map) override
Returns the current navigation map processed by the navigation algorithm.
A class for storing options and configuration information.
std::string toString() const override
Return a standard text representation of the content of the object.
void fromString(const std::string &txt, bool wipe=true)
Interprets a string as a list of properties.
bool fromConfigFile(const std::string &fname, bool wipe=true)
Interprets a file as a list of properties.
Helper class for finding config files and other external resources.
bool check(const std::string &key) const override
Check if there exists a property of the given name.
bool setDefaultContext(const std::string &contextName)
Sets the context for the current ResourceFinder object.
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
std::string findFile(const std::string &name)
Find the full path to a file.
bool setDefaultConfigFile(const std::string &fname)
Provide a default value for the configuration file (can be overridden from command line with the –fro...
A base class for nested structures that can be searched.
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
virtual std::int32_t asInt32() const
Get 32-bit integer value.
#define yCInfo(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
The main, catch-all namespace for YARP.