Full_control fleet adapter fails to add robots under restricted network

[marcoag]

Bug report

Required information:

• Operating system and version:
  Ubuntu 20.04
• OpenRMF installation type:
  Installed from source
• OpenRMF version or commit hash
  Latest version
• ROS distribution and version:
  ROS 2 foxy
• ROS installation type:
  Binaries
• Package or library, if applicable:
  rmf_fleet_adapter

Description of the bug

When running rmf_demos on network restricted scenarios (>10 Mbps) the full_control fleet adapter will fail when trying to add robots:

[full_control-16] full_control: /home/marcogg/open_rmf/src/rmf/rmf_ros2/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp:987: void rmf_fleet_adapter::agv::FleetUpdateHandle::add_robot(std::shared_ptr<rmf_fleet_adapter::agv::RobotCommandHandle>, const string&, const rmf_traffic::Profile&, rmf_traffic::agv::Plan::StartSet, std::function<void(std::shared_ptr<rmf_fleet_adapter::agv::RobotUpdateHandle>)>): Assertion !start.empty()' failed.

Steps to reproduce the bug

1. Make sure you're using cyclonedds as it generates higher traffic when running on localhost:
   export RMW_IMPLEMENTATION=rmw_cyclonedds_cp
2. Restrict the network using the traffic control queue discipline network emulator module form the kernel (make sure the module is available in your kernel):
   sudo tc qdisc change dev lo root netem rate 10mbps
3. Launch RMF using ros2 launch rmf_demos office_demo.launch
4. See the error displayed on the terminal

Expected behavior

The RMF office demo should run as per normal and full_control should be running.

Actual behavior

Full control fails to run as the assertion fails when tying to add the robot.

Additional information

After a quick look to the issue, it seems that:

1. The fleet adapter tries to add the robot with an empty rmf_traffic::agv::Plan::StartSet
2. Looking at the fleet->add_robot() function at full_control/main_cpp at line 470, it looks like this should be generated by rmf_traffic::agv::compute_plan_starts and for some reason is empty.
3. An initial blind shoot could be that gazebo is not ready yet and maybe the time used at the parameter rmf_traffic_ros2::convert(adapter->node()->now()) is not correct. If this is to be true then it might also mean that the demo will also fail in anything that makes gazebo start slowly like maybe some low resources computers.

[cnboonhan]

These are my investigations on a cloud instance of RMF Demos
***************************** BUG REPORT REPRODUCTION ***********************************
Full_control fleet adapter fails to add robots under restricted network #7
#7

Cloud instance:
RMF Demos built on 24 April
Ubuntu 20.04, ROS2 Foxy Binaries
1 GB RAM, 30GB Memory

Contents of Cloud instance:
$HOME/ros2_ws - RMF demos workspace
Office launch files have been modified slightly to launch fleet adapter separately - local branch debug/split-launch

Steps to reproduce

FAILURE SCENARIO:
ros2 launch rmf_demos office.launch.xml headless:=1 # Note, this does not happen all the time, try running a few times

FAILURE OUTPUT:
[full_control-13] [ERROR] [1619252591.116755067] [tinyRobot_fleet_adapter]: Unable to compute a StartSet for robot [tinyRobot2] using level_name [] and location [20.423693, -5.312098, -0.696098] specified in its RobotState message. This can happen if the level_name in the RobotState message does not match any of the map names in the navigation graph supplied or if the location reported in the RobotState message is far way from the navigation graph. This robot will not be added to the fleet [tinyRobot]. The following hint may help with debugging: None of the waypoints in the graph are on a map called [].
[full_control-13] [ERROR] [1619252591.117540992] [tinyRobot_fleet_adapter]: Unable to compute a StartSet for robot [tinyRobot1] using level_name [] and location [10.433054, -5.575095, 1.328612] specified in its RobotState message. This can happen if the level_name in the RobotState message does not match any of the map names in the navigation graph supplied or if the location reported in the RobotState message is far way from the navigation graph. This robot will not be added to the fleet [tinyRobot]. The following hint may help with debugging: None of the waypoints in the graph are on a map called [].

SUCCESS SCENARIO: # Splitting up the launch seems to help with failure
ros2 launch rmf_demos office_split_no_adapters.launch.xml headless:=1 # office.launch.xml without adapters
ros2 launch rmf_demos office_split_only_adapter.launch.xml # only the adapters
ctrl-c the office_split_only_adapter.launch.xml process # terminates successfully

FOLLOW UP:
We try to worsen the network traffic and try to break the success scenario

Run the following:
sudo tc qdisc delete dev lo root netem # remove any prior induced delays
ros2 launch rmf_demos office_split_no_adapters.launch.xml headless:=1 # office.launch.xml without adapters
sudo tc qdisc add dev lo root netem delay 200ms # incudes 200ms delay
ros2 launch rmf_demos office_split_only_adapter.launch.xml # seems to work
ctrl-c the office_split_only_adapter.launch.xml process # Causes crash

FAILURE OUTPUT ( On terminal of office_split_no_adapters.launch.xml )
[rmf_traffic_schedule-1] terminate called after throwing an instance of 'rclcpp::exceptions::RCLError'
[rmf_traffic_schedule-1] what(): failed to send response: error not set, at /tmp/binarydeb/ros-foxy-rcl-1.1.11/src/rcl/service.c:356
[ERROR] [rmf_traffic_schedule-1]: process has died [pid 72312, exit code -6, cmd '/home/ubuntu/ros2_ws/install/rmf_traffic_ros2/lib/rmf_traffic_ros2/rmf_traffic_schedule --ros-args --params-file /tmp/launch_params_jt8w054_'].

On further analysis it seems that this failure does not happen is using the default RMW_IMPLEMENTATION ( fast-DDS ).