Fix/mode adapter error - Githubissues

This PR addresses the added robot mode MODE_ADAPTER_ERROR that is defined here. The fixes have tested it in simulation with instructions to recreate below. These fixes are quick and dirty, and will be reworked in a better manner during the full re-write of Free Fleet that is underway.

Added handling of MODE_ADAPTER_ERROR, in the domain of Free Fleet, it is named MODE_REQUEST_ERROR as there is no information about adapters yet
When first waypoint of a path request is too far away (distance more than max_dist_to_first_waypoint in configuration), MODE_REQUEST_ERROR will be triggered, and robot will be commanded to stay put and wait for next valid request
cleaned up client's logging, only when warnings arises, eg. first waypoint of path command too far

Test

Prepare the ROS1 and ROS2 workspaces according to instructions from the README, be sure the checkout this branch before building each workspace, and the develop branch of rmf_core for the ROS2 workspace.

Launch the multi-turtlebot3 simulation from the ROS1 workspace,

source ~/client_ws/devel/setup.bash
export TURTLEBOT3_MODEL=burger; roslaunch ff_examples_ros1 multi_turtlebot3_ff.launch

Launch the server from the ROS2 workspace, it should register all 3 robots in simulation as clients,

source ~/server_ws/install/setup.bash
ros2 launch ff_examples_ros2 turtlebot3_world_ff.launch.xml

Send a path request that has its first waypoint too far from the bottom-most turtlebot3, tb3_0,

source ~/server_ws/install/setup.bash
ros2 run ff_examples_ros2 send_path_request.py \
    -f turtlebot3 -r tb3_0 -i random_id \
    -p '[{"x": 5.9149, "y": -4.046, "yaw": 0.0, "level_name": ""}, {"x": 6.611, "y": -2.685, "yaw": 0.0, "level_name": ""}]'

On the ROS1 terminal, you should see a warning that the distance is too far, while the robot in simulation doesn't move.

Check the robot's mode using ros2 topic echo /fleet_states, the robot tb3_0's mode should now be 8, which corresponds to MODE_ADAPTER_ERROR.

Now, send a valid path request that is within reach to the same robot,

source ~/server_ws/install/setup.bash
ros2 run ff_examples_ros2 send_path_request.py \
    -f turtlebot3 -r tb3_0 -i another_random_id \
    -p '[{"x": -6.10281848907, "y": 2.64789390564, "yaw": 0.0, "level_name": ""}, {"x": -4.11846208572, "y": 1.303637743, "yaw": 0.0, "level_name": ""}]'

The request should be received by the client, the robot should start navigating to the first waypoint in the request, while its mode should not be 8 anymore

open-rmf / free_fleet

Fix/mode adapter error #41

Test