NODE navigation_manager

See also

see also the user documentation concerning the navigation manager node. In particular, check HOW TO update the navigation manager node.

interface with the navigation system.

the project could have may navigation algorihms implemented, in order to make the robot behaviour more adaptable to the current situation. hence, the task of the navigation mannager is to provide an abstraction, hiding the detals of many algorithms and allowing the program to choose one of the algorithms at runtine.

The node currently implements two controllers so far: ID=0 nav_bug_m, and ID=1 nav_move_base

Authors: Francesco Ganci
Version: v2.0

Note

this implementation supports the developer mode: waitkey, print develop.

Defines

NODE_NAME

LOGSQUARE(str)

OUTLABEL

TLOG(msg)

TWARN(msg)

TERR(msg)

DEVELOP_MODE

DEVELOP_PRINT

WTLOG(msg)

WTWARN(msg)

WTERR(msg)

DEVELOP_WAIKEY

WAITKEY

SS(chr)

SSS(el)

SERVICE_SET_ALGORITHM

SERVICE_NAVIGATION

SERVICE_BUGM_SWITCH

SERVICE_BUGM_SIGNAL

SERVICE_MOVE_BASE_SWITCH

SERVICE_MOVE_BASE_SIGNAL

Functions

void shut_msg(int sig): shudown message

int main(int argc, char *argv[])

ROS node main - Navigation Manager.

Here are instanciated and registered the controllers used for the navigation.

class navigation_controller

virtual interface for a navigation implementation

Note

this class is also a template for a motion planning algorithm

Subclassed by nav_bug_m, nav_move_base

Public Functions

inline navigation_controller(std::string name = "", int code = -1): class constructor

inline ~navigation_controller(): class destructor

inline virtual bool enable(): (virtual) turn on the algorithm

inline virtual bool perform_navigation(robocluedo_movement_controller_msgs::NavigationService::Request &req, robocluedo_movement_controller_msgs::NavigationService::Response &res): (virtual) use the algorithm for the 2D navigation

inline virtual bool disable(): (virtual) turn off the algorithm

inline bool is_enabled(): check wether the algorithm is enabled or not

Public Members

int nav_code = -1: navigation identfier

std::string nav_name = "": navigation algorithm name

Protected Attributes

ros::NodeHandle nh: ROS Node handle.

bool enabled = false: activity state of the algorithm

class nav_bug_m : public navigation_controller 

nav service for the low level bug_m algorithm

This controller uses the bug_m implementation to navigate to the target and achieve whatever final orientation pose.

This low level is a bit limited in many aspects:

it cannot check if the target is reachable
the algorithm doesn’t work in complex environments with obstacles

The implementation turns on the channels at the first enable call, and leaves them active for the entire lifecycle of the navigation manager.

Public Functions

inline nav_bug_m(std::string name = "", int code = -1): class constructor

inline ~nav_bug_m(): class destructor

inline virtual bool enable(): turn on the algorithm

inline virtual bool perform_navigation(robocluedo_movement_controller_msgs::NavigationService::Request &req, robocluedo_movement_controller_msgs::NavigationService::Response &res)

use the algorithm for the 2D navigation

The controller first sends the target to the low level using the parameter server as required by bug_m; then , it waits for the ending signal from the low level.

Attention: The waiting is endless if a way to reach the final target doesn’t exist. There’s not a timeout.

Parameters

request – the objective
response – the result of the navigation

Returns

false if the navigation fails or the node is not enabled

inline virtual bool disable(): turn off the algorithm

inline bool cbk_bug_signal(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res): service for the signal from the bug_m node

Private Functions

inline void set_parameter_server(robocluedo_movement_controller_msgs::NavigationService::Request &req): set the target into the parameter server

inline void wait_for_signal(): wait for the signal

Private Members

bool channels_enabled = false: channels opened?

ros::ServiceClient cl_bug_switch: client /bug_switch : std_srvs/SetBool

ros::ServiceServer cl_bug_signal: service /bug_m_signal : std_srvs/Empty

bool signal = false: received a signal from the node

class nav_move_base : public navigation_controller 

move_base navigation

this class implements the workflow with the navigation algorithm implemented in the node ‘move_base_nav.py’. The way it works is quite similar to the one of ‘bug_m’ except for the different navigation system.

The implementation turns on the channels at the first enable call, and leaves them active for the entire lifecycle of the navigation manager.

Note

this kind of navigation is particularly indicated for indoor environments.

Public Functions

inline nav_move_base(std::string name = "", int code = -1): class constructor

inline ~nav_move_base(): class destructor

inline virtual bool enable(): turn on the algorithm

inline virtual bool perform_navigation(robocluedo_movement_controller_msgs::NavigationService::Request &req, robocluedo_movement_controller_msgs::NavigationService::Response &res)

use the algorithm for the 2D navigation

The controller first sends the target to the low level using the parameter server as required by move_base_nav; then , it waits for the ending signal from the low level.

Parameters

request – the objective
response – the result of the navigation

Returns

false if the navigation fails or the node is not enabled

inline virtual bool disable(): turn off the algorithm

inline bool cbk_bug_signal(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res): service for the signal from the move_base_nav node

Private Functions

inline void set_parameter_server(robocluedo_movement_controller_msgs::NavigationService::Request &req): set the target into the parameter server

inline void wait_for_signal(): wait for the signal

Private Members

bool channels_enabled = false: channels opened?

ros::ServiceClient cl_move_base_switch: client /nav_stack_switch : std_srvs/SetBool

ros::ServiceServer srv_move_base_signal: service /nav_stack_signal : std_srvs/Empty

bool signal = false: received a signal from the node

class class_navigation_manager

implementation of the node as class

The navigation manager uses the controllers, i.e. modules (objects) using the low-level implementation of the navigation in order to reach the objective provided. In particular, here are some characteristics of this particular system:

the navigation manager is a list of controllers with one controller active set apart from the array
each controller can be enabled or disabled
all the services and topics needed to make the low level work are hidden in the controller, whereas the navigation manager sees alway the same interface
a controller can be replaced at runtime depending on the situation, if needed, which means that the robot can use many combinations of the low level behaviours in order to carry out the objective

see the documentationof the package for further informations.

Public Functions

inline class_navigation_manager()

class constructor

The cnode class constructor opens only two services: the first one used for selecting the algorithm among the ones registered; and the second one, for performing the navigation in a synchronous way.

inline ~class_navigation_manager(): class destructor

inline int register_algorithm(navigation_controller *algo)

register one navigation algorithm

The function first of all disables the algorithm provided, then it allocates its address inside an array of controllers.

Todo:: check for the null value as argument! Otherwise the function will try to call disable() on a null, causing a segmentation fault error.

Note

the function will disable the algorithm before registering it

Parameters: algo – (navigation controller &) the implementation of one algorithm
Returns: the index associated with the algorithm

inline bool switch_controller(int controller_code)

activate one controller

the function disables the currently enabled controller, and replaces that with the selected one, only if feasible.

Parameters: controller_code – the index of the controller
Returns: if the operation succeeded or not

inline bool set_status(bool new_status): switch the status of the current controller

inline bool is_enabled(): check wether the currently acivated algorithm is enabled or not

inline bool cbk_set_algorithm(robocluedo_movement_controller_msgs::Algorithm::Request &req, robocluedo_movement_controller_msgs::Algorithm::Response &res)

set one algorithm among the available ones and activate it

Attention: you cannot call the navigation until you haven’t selected an algorithm with this service!

Parameters

request – the algorithm to activate (see the details of the message)
response – the result of the operation

inline bool cbk_navigation(robocluedo_movement_controller_msgs::NavigationService::Request &req, robocluedo_movement_controller_msgs::NavigationService::Response &res)

use the currently running algorithm for the 2D navigation (blocking)

the method just tries to pass the calling to the currently active controller, if any.

Attention: there must be a controller selected before calling this function! See the algorithm selection service.

Parameters

request – the navigation objective
response – the result of the operation

Private Members

ros::NodeHandle nh: ROS Node handle.

std::vector<navigation_controller*> controllers: list of navigation algorithms

navigation_controller *running_controller = nullptr: the currently active controller, if any

std::string last_error_explain = "": explaination of the last error

ros::ServiceServer srv_set_algorithm: algorithm selection service handle

ros::ServiceServer srv_navigation: navigation service handle