NODE navigation_unit

mission manager bridge between the robocluedo ROSPlan framework and the navigation controller

This node is a bridge between the ROSPlan framework, which assumes some other node implemented the service for the navigation from rosplan, and the navigation system: a service call into a service call.

The task of this node is quite simple: when received a request from the ROSPlan module, the node associates the cartesian coordinates of the pointed waypoint, then calls the navigation service; when the navigation has finished, the node adapt and returns the response to ROSPlan.

There are also other sub-tasks performed by this node: listening for the markers in order to find the coordinates of the waypoints, for instance, using a one-shot subscriber. Or also to initialise the navigation system.

Authors

Francesco Ganci

Version

v1.1

Note

This implementation supports the develop mode: waitey, debug print

Note

the node requires the move_base controller

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_ROSPLAN_NAVIGATION

SERVICE_SET_ALGORITHM

SERVICE_NAVIGATION

TOPIC_MARMERS

NAV_ALGO_BUG_M

NAV_ALGO_MOVE_BASE

SCALING_FACTOR

Functions

void shut_msg(int sig)

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

class node_navigation_unit

implementation of the node

The navigation unit is implemented as a class containing all the services and the topics needed to perform the navigation.

Public Functions

inline node_navigation_unit()

node class constructor

here the node opens all the channels with the other components.

inline void spin()

spin function: just wait for shutdown

inline bool cbk_navigation(robocluedo_rosplan_msgs::NavigationCommand::Request &req, robocluedo_rosplan_msgs::NavigationCommand::Response &res)

the service required by the rosplan package

A service call inside the service call: the node, after received the request, translates it into a request for the navigation manager, then calls that service and waits. When the navigation ends, the service of this node returns a proper response to the ROSPlan framework.

The service does also something else. First of all, it tries to set the algorithm the first time it is called. And second, it waits for the first reading of the markers from the markers publisher. These operations are performed only during the first service call.

Parameters

• request – the waypoint to reach

• response – success or not?

inline void cbk_markers(const visualization_msgs::MarkerArray::ConstPtr &data)

“one-shot” listener for the markers from the Oracle

The callback stores the markers, then turns off the subscription to not waste computational resources in a continuous updating of the markers.

Note

we’re assuming here that the markers don’t change during the simulation. If the markers are able to move though, the subscription should keep opened.

Note

currently, the function assumes that the index of a marker inside the array is its ID as well. for instance, data->markers[2] is called “wp3” by the function.

Private Members

ros::NodeHandle nh

ROS node handle.

ros::ServiceServer srv_rosplan_nav

service handle rosplan to unit

ros::ServiceClient cl_nav

client handle unit to navigation

ros::Subscriber sub_markers

subscription handle to markers from the Oracle

ros::ServiceClient cl_nav_algorithm

to change the algorithm (in case…)

std::map<std::string, robocluedo_movement_controller_msgs::Pose2D> waypoints

positions of the waypoints

bool found_markers = false

signal fro the markers listener

bool mpl_enabled = false

is the motion planning algorithm active?