YARP-ROS Inter-Operation in a 2D Navigation Task

This paper presents some recent developments in YARP middleware, aimed to improve its integration with ROS. They include a new mechanism to read/write ROS transform frames and a new set of standard interfaces to intercommunicate with the ROS navigation stack. A novel set of YARP companion modules, which provide basic navigation functionalities for robots unable to run ROS, is also presented. These modules are optional, independent from each other, and they provide compatible functionalities to well-known packages available inside ROS framework. This paper also discusses how developers can customize their own hybrid YARP-ROS environment in the way it best suits their needs (e.g., the system can be configured to have a YARP application sending navigation commands to a ROS path planner, or vice versa). A number of available possibilities is presented through a set of chosen test cases applied to both real and simulated robots. Finally, example applications discussed in this paper are also made available to the community by providing snippets of code and links to source files hosted on github repository https://github.com/robotology.1


III TRANSFORMCLIENT EXAMPLE
The following code snippet, extracted from YARP FrameTransformClient regression test, 105 shows an example in which a yarp::dev::FrameTransformClient registers on the 106 yarp::dev::FrameTransformServer two transforms: frame1→frame2 and frame2→frame3. The 107 client is then asked to compute the chained transform frame1→frame3. The server needs to be externally 108 launched before the execution of this example.    The cell does not contain any detectable obstacle, but is marked as keep-out, so computed path will avoid this area. map cell temporary obstacle The cell is marked as free on the map, but robot sensors detected that it is currently occupied by an obstacle. Temporary obstacles are not used by localization modules to determine robot position. map cell enlarged obstacle The cell is marked as free on the map, but path planning algorithm is currently considering it as a keep-out area because a nearby obstacle is potentially able intersect robot footprint. Temporary obstacles are not used by localization modules to determine robot position. map cell wall The cell is occupied by a fixed wall and it can be used for robot localization. map cell unknown The cell occupancy status is unknown. Mapping is required. map cell unsafe area The robot will stop when an obstacle is detected in this area, instead of trying to avoid it. map cell caution area The robot will move with user-definable reduced speed when crossing this particular area.

VI RETRIEVE MAP EXAMPLE
The following code snippet, taken from the source code of robotPathPlanner module, shows how to retrieve 169 a map from the yarp::dev::Map2DServer storage using the yarp::dev::IMap2D interface.

170
The server needs to be externally launched before the execution of this example.

VIII NAVIGATION STATUS ENUM
A complete list of yarp::dev::INavigation2D::NavigationStatusEnum is shown in the 244 following table.

245
NavigationStatusEnum Notes navigation status idle The robot is idle, waiting for a navigation command. navigation status preparing before move The robot is executing custom actions defined by the user before starting the navigation task. navigation status moving The robot is currently navigating towards the goal. navigation status waiting obstacle The robot is currently avoiding an obstacle or it is waiting for external help. A watchdog is typically used to limit the maximum waiting time. If the timeout expires, the navigation status is changed to navigation status failing or navigation status aborted. navigation status goal reached The robot has reached its current goal and it is about to change its navigation status to navigation status idle. navigation status aborted The path planner detected that there are no valid paths to reach the commanded goal. The user is requested to stop the current navigation task to clear this error. navigation status failing The robot has tried unsuccessfully to avoid an obstacle. A path replanning is required. If no replanning is performed, navigation status switches to navigation status aborted. navigation status paused Navigation task has been paused on user request. navigation status thinking The path planner is currently computing the robot path. The system automatically switches to navigation status moving when the operation is complete. navigation status error An unforeseen event occurred during navigation or not all required modules are currently available (e.g. missing localization or sensor data) Fig.S1 shows an example of transition diagram between the states defined by NavigationStatusEnum.

246
In particular, the presented diagram refers to the finite-state machine implemented in robotPathPlanner.

247
It must be noticed that different navigation modules may implement just a subset of these states (e.g. modules. This will maximize the integration with already existing YARP applications. Figure S1: Finite-state machine for robotPathPlanner navigation module.

IX NAVIGATION2DCLIENT EXAMPLE
The following code snippet shows an example of usage of the two interfaces: yarp::dev::IMap2D 254 and yarp::dev::INavigation2D. In order to execute successfully, this example assumes that:

255
• A mobile robot, either real or simulated, is available. baseControl module must be started and 256 connected to the low-level motor/sensor interfaces provided by YarpRobotInterface. Examples included 257 in robotology/navigation repository show how to simulate a simple wheeled robot able to move in a 258 2D world without using an external physics simulator such as Gazebo.

259
• A navigation module, such as RobotPathPlanner, is running. The module has to be connected to 260 the robot control port (e.g. /baseControl/control:i). This port may belong either to a real or 261 to a simulated robot.

265
In order to better clarify the execution flow of the example, a sequence diagram is provided in Section 266 x, Fig.S2. Please note that for sake of clarity, the diagram omits all the operations performed by the 267 path planner module RobotPathPlanner. This latter communicates with a local navigation module 268 (e.g.robotGoto), which in turn is connected to the robot control module baseControl.

269
The diagram highlights the messages exchanged between the clients opened by the user application 270 and the server modules (i.e. localizationServer, Map2DServer and RobotPathPlanner).

271
In particular, it is should be noticed that there are cases in which a single command, such as 272 gotoTargetLocationbyName(), is processed by a client by generating multiple requests. Depen-273 ding on the command, these requests might be processed sequentially by the same server or by different