Posted by @IshaaanVyas:
Configuration
- Laptop with Ubuntu 20.04 and Galactic+Noetic installed
- Using a real robot (similar to turtlebot) and using free fleet client and server to relay messages from the fleet adapter to the robot
I have repurposed the office simulation from rmf demos by replacing the office.building with the building file that I created from traffic editor myself. (Also confirmed that the correct building file and nav graphs are being used from the share folders)
Problem
Whenever my robot starts on a waypoint, the fleet adapter instantly crashes. It seems like it is trying to find a lane connecting the current waypoint to itself and failing:
[full_control-13] [SearchForPath] CRITICAL ERROR: Failed to find an acceptable greedy solution. Participant [fake_ros1_robot] owned by [fake_fleet] Requested path ([0] r:-0.00902051 | no lane) --> (0). Maximum cost: 0 | Leeway factor: 10 | Current cost: 1.27775 | Saturated: 0 (limit: 10000) | interrupted: 0
[full_control-13] linear | v: 0.5, a: 0.75
[full_control-13] angular | v: 0.6, a: 2
[full_control-13] full_control: /home/ishaan/rmf_ws/src/rmf/rmf_ros2/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/../services/../jobs/detail/impl_SearchForPath.hpp:146: rmf_fleet_adapter::jobs::SearchForPath::operator()(const Subscriber&, const Worker&) [with Subscriber = rxcpp::subscriber<rmf_fleet_adapter::jobs::SearchForPath::Result, rxcpp::observer<rmf_fleet_adapter::jobs::SearchForPath::Result, rxcpp::operators::detail::observe_on<rmf_fleet_adapter::jobs::SearchForPath::Result, rxcpp::observe_on_one_worker>::observe_on_observer<rxcpp::subscriber<rmf_fleet_adapter::jobs::SearchForPath::Result, rxcpp::observer<rmf_fleet_adapter::jobs::SearchForPath::Result, void, void, void, void> > >, void, void, void> >; Worker = rxcpp::schedulers::worker]::<lambda(const rmf_fleet_adapter::jobs::Planning::Result&)>: Assertion `false' failed.
[ERROR] [full_control-13]: process has died [pid 15801, exit code -6, cmd '/home/ishaan/rmf_ws/install/rmf_fleet_adapter/lib/rmf_fleet_adapter/full_control --ros-args -r __node:=fake_fleet_fleet_adapter --params-file /tmp/launch_params_htp76ou1 --params-file /tmp/launch_params_5dp3w51i --params-file /tmp/launch_params_m3j_j2_0 --params-file /tmp/launch_params_ppadsj7a --params-file /tmp/launch_params_ae7j3dv_ --params-file /tmp/launch_params_sgbdyef1 --params-file /tmp/launch_params_2t5kzyyq --params-file /tmp/launch_params_k3s0el3j --params-file /tmp/launch_params__d4gtx2o --params-file /tmp/launch_params_yn0h0o0f --params-file /tmp/launch_params_0agtdh7e --params-file /tmp/launch_params_rmdf6p4x --params-file /tmp/launch_params_h5mpe1ot --params-file /tmp/launch_params_4s99e47p --params-file /tmp/launch_params_l4y1m6w2 --params-file /tmp/launch_params_uhs3g7dz --params-file /tmp/launch_params_05zyn043 --params-file /tmp/launch_params_999ntd5c --params-file /tmp/launch_params_2vca692b --params-file /tmp/launch_params_y8g9d41v --params-file /tmp/launch_params_s4pb8s4j --params-file /tmp/launch_params__y2y61np --params-file /tmp/launch_params_ryb8rpru --params-file /tmp/launch_params_5y_my4mn --params-file /tmp/launch_params_z52jk0g3 --params-file /tmp/launch_params_bztmn4fg --params-file /tmp/launch_params_o0t1lgo7 --params-file /tmp/launch_params__oqaejsa --params-file /tmp/launch_params__370sw3d --params-file /tmp/launch_params_eyhw00ow --params-file /tmp/launch_params_lgv3xvqm --params-file /tmp/launch_params_3faal1a5'].
Whenever I start the robot while it not directly being on any waypoint, it starts moving towards the nearest waypoint (and it physically moves towards it as well) but upon reaching the waypoint there’s the same error and it is unable to find a greedy solution so it dies.
Here is a screenshot of the traffic editor file I am using:
Here is the full terminal output from the rmf side:
[INFO] [launch]: All log files can be found below /home/ishaan/.ros/log/2022-07-22-12-23-49-854596-ishaan-HP-ENVY-x360-Convertible-13-ay0xxx-15729
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [rmf_traffic_schedule-1]: process started with pid [15731]
[INFO] [rmf_traffic_schedule_monitor-2]: process started with pid [15733]
[INFO] [rmf_traffic_blockade-3]: process started with pid [15735]
[INFO] [building_map_server-4]: process started with pid [15737]
[INFO] [schedule_visualizer-5]: process started with pid [15739]
[INFO] [rmf_visualization_fleet_states-6]: process started with pid [15741]
[INFO] [rmf_visualization_building_systems-7]: process started with pid [15743]
[INFO] [rviz2-8]: process started with pid [15755]
[INFO] [door_supervisor-9]: process started with pid [15764]
[INFO] [lift_supervisor-10]: process started with pid [15776]
[INFO] [rmf_task_dispatcher-11]: process started with pid [15787]
[INFO] [simple_api_server-12]: process started with pid [15790]
[INFO] [full_control-13]: process started with pid [15801]
[INFO] [robot_state_aggregator-14]: process started with pid [15805]
[INFO] [gzserver-15]: process started with pid [15808]
[INFO] [gzclient-16]: process started with pid [15836]
[rmf_task_dispatcher-11] ~Initializing Dispatcher Node~
[rmf_traffic_schedule-1] [INFO] [1658463831.020402647] [rmf_traffic_schedule_primary]: Successfully loaded logfile .rmf_schedule_node.yaml
[rmf_traffic_schedule-1] [INFO] [1658463831.026124893] [rmf_traffic_schedule_primary]: Set up heartbeat on /rmf_traffic/heartbeat with liveliness lease duration of 1000 ms and deadline of 1000 ms
[rmf_traffic_schedule-1] [INFO] [1658463831.070111449] [rmf_traffic_schedule_primary]: Beginning traffic schedule node
[rmf_traffic_schedule-1] [INFO] [1658463831.073217424] [rmf_traffic_schedule_primary]: Registered new query [1]
[rmf_traffic_schedule_monitor-2] [INFO] [1658463831.022676991] [rmf_traffic_schedule_backup]: Set up heartbeat listener on /rmf_traffic/heartbeat with liveliness lease duration of 1000 ms
[rmf_traffic_schedule_monitor-2] [INFO] [1658463831.032760690] [rmf_traffic_schedule_backup]: Liveliness changed event:
[rmf_traffic_schedule_monitor-2] [INFO] [1658463831.033026424] [rmf_traffic_schedule_backup]: alive_count: 1
[rmf_traffic_schedule_monitor-2] [INFO] [1658463831.033044931] [rmf_traffic_schedule_backup]: not_alive_count: 0
[rmf_traffic_schedule_monitor-2] [INFO] [1658463831.033062600] [rmf_traffic_schedule_backup]: alive_count_change: 1
[rmf_traffic_schedule_monitor-2] [INFO] [1658463831.033079361] [rmf_traffic_schedule_backup]: not_alive_count_change: 0
[rmf_traffic_schedule_monitor-2] [INFO] [1658463831.042973939] [rmf_traffic_schedule_backup]: Handling new sync of 0 queries from primary node
[rmf_traffic_schedule_monitor-2] [INFO] [1658463831.073927956] [rmf_traffic_schedule_backup]: Handling new sync of 1 queries from primary node
[rmf_traffic_schedule_monitor-2] [INFO] [1658463831.075487508] [rmf_traffic_schedule_backup]: Got mirror for monitor node
[rmf_traffic_blockade-3] [INFO] [1658463831.033576119] [rmf_traffic_blockade_node]: Beginning traffic blockade node
[rmf_task_dispatcher-11] [INFO] [1658463831.144125202] [rmf_dispatcher_node]: Declared Time Window Param as: 2.000000 secs
[rmf_task_dispatcher-11] [INFO] [1658463831.144383253] [rmf_dispatcher_node]: Declared Terminated Tasks Max Size Param as: 100
[rmf_task_dispatcher-11] [INFO] [1658463831.144434305] [rmf_dispatcher_node]: Declared publish_active_tasks_period as: 2 secs
[rmf_traffic_schedule-1] [INFO] [1658463831.166045185] [rmf_traffic_schedule_primary]: A new mirror is tracking query ID [1]
[rmf_traffic_schedule_monitor-2] [INFO] [1658463831.166803975] [rmf_traffic_schedule_backup]: Handling new sync of 1 queries from primary node
[rmf_traffic_schedule_monitor-2] [INFO] [1658463831.167095967] [rmf_traffic_schedule_backup]: Mirror handling new sync of 1 queries from schedule node version [0]
[rmf_task_dispatcher-11] [INFO] [1658463831.182360808] [rmf_dispatcher_node]: Starting task dispatcher node
[schedule_visualizer-5] [INFO] [1658463831.235052601] [rmf_visualization_schedule_data_node]: rmf_visualization_schedule_data_node started...
[schedule_visualizer-5] [INFO] [1658463831.237798701] [rmf_visualization_schedule_data_node]: Websocket server started on port: 8006
[schedule_visualizer-5] [INFO] [1658463831.264892911] [rmf_visualization_schedule_data_node]: Mirror handling new sync of 1 queries from schedule node version [0]
[full_control-13] [INFO] [1658463831.290737091] [fake_fleet_fleet_adapter]: Parameter [discovery_timeout] set to: 60.000000
[rmf_traffic_schedule-1] [INFO] [1658463831.316321893] [rmf_traffic_schedule_primary]: A new mirror is tracking query ID [1]
[rmf_traffic_schedule_monitor-2] [INFO] [1658463831.317930542] [rmf_traffic_schedule_backup]: Handling new sync of 1 queries from primary node
[rmf_traffic_schedule_monitor-2] [INFO] [1658463831.318154512] [rmf_traffic_schedule_backup]: Mirror handling new sync of 1 queries from schedule node version [0]
[schedule_visualizer-5] [INFO] [1658463831.319329467] [rmf_visualization_schedule_data_node]: Mirror handling new sync of 1 queries from schedule node version [0]
[full_control-13] [INFO] [1658463831.393010162] [fake_fleet_fleet_adapter]: Parameter [linear_velocity] set to: 0.500000
[full_control-13] [INFO] [1658463831.393359631] [fake_fleet_fleet_adapter]: Parameter [angular_velocity] set to: 0.600000
[full_control-13] [INFO] [1658463831.393745137] [fake_fleet_fleet_adapter]: Parameter [linear_acceleration] set to: 0.750000
[full_control-13] [INFO] [1658463831.394112764] [fake_fleet_fleet_adapter]: Parameter [angular_acceleration] set to: 2.000000
[full_control-13] [INFO] [1658463831.394451758] [fake_fleet_fleet_adapter]: Parameter [footprint_radius] set to: 0.300000
[full_control-13] [INFO] [1658463831.394780974] [fake_fleet_fleet_adapter]: Parameter [vicinity_radius] set to: 0.500000
[full_control-13] [INFO] [1658463831.395082465] [fake_fleet_fleet_adapter]: Parameter [reversible] set to: 1
[full_control-13] The fleet [fake_fleet] has the following named waypoints:
[full_control-13] -- docking
[full_control-13] -- end
[full_control-13] -- startend
[full_control-13] [INFO] [1658463831.441891803] [fake_fleet_fleet_adapter]: Parameter [battery_voltage] set to: 12.000000
[full_control-13] [INFO] [1658463831.442456094] [fake_fleet_fleet_adapter]: Parameter [battery_capacity] set to: 24.000000
[full_control-13] [INFO] [1658463831.443000621] [fake_fleet_fleet_adapter]: Parameter [battery_charging_current] set to: 5.000000
[full_control-13] [INFO] [1658463831.443611843] [fake_fleet_fleet_adapter]: Parameter [mass] set to: 20.000000
[full_control-13] [INFO] [1658463831.444092817] [fake_fleet_fleet_adapter]: Parameter [inertia] set to: 10.000000
[full_control-13] [INFO] [1658463831.444534613] [fake_fleet_fleet_adapter]: Parameter [friction_coefficient] set to: 0.220000
[full_control-13] [INFO] [1658463831.444954688] [fake_fleet_fleet_adapter]: Parameter [ambient_power_drain] set to: 20.000000
[full_control-13] [INFO] [1658463831.445405911] [fake_fleet_fleet_adapter]: Parameter [tool_power_drain] set to: 0.000000
[full_control-13] [INFO] [1658463831.445868798] [fake_fleet_fleet_adapter]: Parameter [drain_battery] set to: 0
[full_control-13] [INFO] [1658463831.446367441] [fake_fleet_fleet_adapter]: Parameter [recharge_threshold] set to: 0.000000
[full_control-13] [INFO] [1658463831.447170089] [fake_fleet_fleet_adapter]: Parameter [recharge_soc] set to: 1.000000
[full_control-13] [INFO] [1658463831.447841302] [fake_fleet_fleet_adapter]: Fleet is not configured to perform any finishing request
[full_control-13] [INFO] [1658463831.459007839] [fake_fleet_fleet_adapter]: Parameter [delay_threshold] set to: 15.000000
[full_control-13] [INFO] [1658463831.463653184] [fake_fleet_fleet_adapter]: Starting Fleet Adapter
[full_control-13] [INFO] [1658463831.468406777] [fake_fleet_fleet_adapter]: Mirror handling new sync of 1 queries from schedule node version [0]
[rmf_traffic_schedule-1] [INFO] [1658463831.502924418] [rmf_traffic_schedule_primary]: Registered participant [2] named [fake_ros1_robot] owned by [fake_fleet]
[full_control-13] [INFO] [1658463831.511967810] [fake_fleet_fleet_adapter]: Added a robot named [fake_ros1_robot] with participant ID [2]
[full_control-13] [WARN] [1658463831.622766005] [fake_fleet_fleet_adapter]: Failed to update using patch for DB version 12; requesting new update
[gzclient-16] Gazebo multi-robot simulator, version 11.10.2
[gzclient-16] Copyright (C) 2012 Open Source Robotics Foundation.
[gzclient-16] Released under the Apache 2 License.
[gzclient-16] http://gazebosim.org
[gzclient-16]
[gzserver-15] Gazebo multi-robot simulator, version 11.10.2
[gzserver-15] Copyright (C) 2012 Open Source Robotics Foundation.
[gzserver-15] Released under the Apache 2 License.
[gzserver-15] http://gazebosim.org
[gzserver-15]
[rmf_visualization_fleet_states-6] [INFO] [1658463831.698421929] [fleet_state_visualizer]: hello i am fleet state visualizer
[rmf_visualization_building_systems-7] [INFO] [1658463831.770272380] [building_systems_visualizer]: Building systems visualizer started...
[rviz2-8] [INFO] [1658463831.813672646] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-8] [INFO] [1658463831.814465517] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-8] [INFO] [1658463831.844439916] [rviz2]: Stereo is NOT SUPPORTED
[simple_api_server-12] Set Server IP to: 0.0.0.0
[simple_api_server-12] Set Server port to: 0.0.0.0:8083
[simple_api_server-12] Set RMF Websocket port to: localhost:7878
[simple_api_server-12] Starting Websocket Server
[simple_api_server-12] Starting RMF_Demos API Server: 0.0.0.0:8083, with ws://localhost:7878
[simple_api_server-12] * Serving Flask app "rmf_demos_panel.simple_api_server" (lazy loading)
[simple_api_server-12] * Environment: production
[simple_api_server-12] WARNING: This is a development server. Do not use it in a production deployment.
[simple_api_server-12] Use a production WSGI server instead.
[simple_api_server-12] * Debug mode: off
[schedule_visualizer-5] [INFO] [1658463832.036440080] [rmf_visualization_schedule_marker_publisher]: Rviz parameters updated
[building_map_server-4] [INFO] [1658463832.069197105] [building_map_server]: loading map path: /home/ishaan/rmf_ws/install/rmf_demos_maps/share/rmf_demos_maps/office/office.building.yaml
[building_map_server-4] [INFO] [1658463832.088710750] [building_map_server]: opening: /home/ishaan/rmf_ws/install/rmf_demos_maps/share/rmf_demos_maps/office/flrplncropped.png
[building_map_server-4] [INFO] [1658463832.121715629] [building_map_server]: read 219926 byte image
[building_map_server-4] [INFO] [1658463832.123966159] [building_map_server]: unable to generate GeoJSON for this map.
[building_map_server-4] [INFO] [1658463832.131012953] [building_map_server]: publishing map...
[schedule_visualizer-5] [INFO] [1658463832.134679657] [rmf_visualization_schedule_marker_publisher]: Received map "deletesoon" containing 1 level(s)
[schedule_visualizer-5] [INFO] [1658463832.135375313] [rmf_visualization_schedule_marker_publisher]: Level cache updated
[schedule_visualizer-5] [INFO] [1658463832.135875982] [rmf_visualization_schedule_marker_publisher]: Loading floorplan Image: flrplncropped png
[building_map_server-4] [INFO] [1658463832.135918025] [building_map_server]: ready to serve map: "deletesoon" Ctrl+C to exit...
[rviz2-8] [INFO] [1658463832.373788345] [rviz2]: Trying to create a map of size 1305 x 1182 using 1 swatches
[rviz2-8] [ERROR] [1658463832.402833410] [rviz2]: Vertex Program:rviz/glsl120/indexed_8bit_image.vert Fragment Program:rviz/glsl120/indexed_8bit_image.frag GLSL link result :
[rviz2-8] active samplers with a different type refer to the same texture image unit
[gzclient-16] [INFO] [1658463834.225564894] [gazebo_ros_node]: ROS was initialized without arguments.
[gzclient-16] [Msg] Waiting for master.
[gzclient-16] [Msg] Connected to gazebo master @ http://127.0.0.1:11345
[gzclient-16] [Msg] Publicized address: 192.168.11.248
[gzclient-16] ToggleCharging::ToggleCharging()
[gzclient-16] ToggleFloors::ToggleFloors()
[gzclient-16] ToggleFloors::Load()
[gzclient-16] Error [Element.cc:914] Missing element description for [model]
[gzclient-16]
[gzclient-16] libcurl: (6) Could not resolve host: fuel.ignitionrobotics.org
[gzclient-16] ToggleFloors::Load found a floor element: [L1]->[deletesoon_L1]
[gzclient-16] [Wrn] [ModelDatabase.cc:212] Unable to connect to model database using [http://models.gazebosim.org//database.config]. Only locally installed models will be available.
[gzclient-16] [Wrn] [Publisher.cc:135] Queue limit reached for topic /gazebo/world/user_camera/pose, deleting message. This warning is printed only once.
[full_control-13] [SearchForPath] CRITICAL ERROR: Failed to find an acceptable greedy solution. Participant [fake_ros1_robot] owned by [fake_fleet] Requested path ([0] r:-0.00902051 | no lane) --> (0). Maximum cost: 0 | Leeway factor: 10 | Current cost: 1.27775 | Saturated: 0 (limit: 10000) | interrupted: 0
[full_control-13] linear | v: 0.5, a: 0.75
[full_control-13] angular | v: 0.6, a: 2
[full_control-13] full_control: /home/ishaan/rmf_ws/src/rmf/rmf_ros2/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/../services/../jobs/detail/impl_SearchForPath.hpp:146: rmf_fleet_adapter::jobs::SearchForPath::operator()(const Subscriber&, const Worker&) [with Subscriber = rxcpp::subscriber<rmf_fleet_adapter::jobs::SearchForPath::Result, rxcpp::observer<rmf_fleet_adapter::jobs::SearchForPath::Result, rxcpp::operators::detail::observe_on<rmf_fleet_adapter::jobs::SearchForPath::Result, rxcpp::observe_on_one_worker>::observe_on_observer<rxcpp::subscriber<rmf_fleet_adapter::jobs::SearchForPath::Result, rxcpp::observer<rmf_fleet_adapter::jobs::SearchForPath::Result, void, void, void, void> > >, void, void, void> >; Worker = rxcpp::schedulers::worker]::<lambda(const rmf_fleet_adapter::jobs::Planning::Result&)>: Assertion `false' failed.
[ERROR] [full_control-13]: process has died [pid 15801, exit code -6, cmd '/home/ishaan/rmf_ws/install/rmf_fleet_adapter/lib/rmf_fleet_adapter/full_control --ros-args -r __node:=fake_fleet_fleet_adapter --params-file /tmp/launch_params_htp76ou1 --params-file /tmp/launch_params_5dp3w51i --params-file /tmp/launch_params_m3j_j2_0 --params-file /tmp/launch_params_ppadsj7a --params-file /tmp/launch_params_ae7j3dv_ --params-file /tmp/launch_params_sgbdyef1 --params-file /tmp/launch_params_2t5kzyyq --params-file /tmp/launch_params_k3s0el3j --params-file /tmp/launch_params__d4gtx2o --params-file /tmp/launch_params_yn0h0o0f --params-file /tmp/launch_params_0agtdh7e --params-file /tmp/launch_params_rmdf6p4x --params-file /tmp/launch_params_h5mpe1ot --params-file /tmp/launch_params_4s99e47p --params-file /tmp/launch_params_l4y1m6w2 --params-file /tmp/launch_params_uhs3g7dz --params-file /tmp/launch_params_05zyn043 --params-file /tmp/launch_params_999ntd5c --params-file /tmp/launch_params_2vca692b --params-file /tmp/launch_params_y8g9d41v --params-file /tmp/launch_params_s4pb8s4j --params-file /tmp/launch_params__y2y61np --params-file /tmp/launch_params_ryb8rpru --params-file /tmp/launch_params_5y_my4mn --params-file /tmp/launch_params_z52jk0g3 --params-file /tmp/launch_params_bztmn4fg --params-file /tmp/launch_params_o0t1lgo7 --params-file /tmp/launch_params__oqaejsa --params-file /tmp/launch_params__370sw3d --params-file /tmp/launch_params_eyhw00ow --params-file /tmp/launch_params_lgv3xvqm --params-file /tmp/launch_params_3faal1a5'].
I have confirmed that the fleet states are constantly being sent correctly, even after the fleet adapter dies and everything on the free fleet side is working as intended.
Any help on this matter would be greatly appreciated, Thanks in advance!
Chosen answer
Answer chosen by @IshaaanVyas at 2022-08-02T03:16:09Z.
Answered by @Yadunund:
Hi there,
-
Which waypoint does the robot start at/move towards? Can you confirm that this waypoint is actually connected to the navigation graph? For example, if it starts at the intersection of the diagonals, can you check whether that wayoint is actually connected to the navigation graph? ie, it is not a dangling waypoint that is underneath a lane.
-
Is your Robot <=> RMF transformation accurate? You will need to update the transformation values in the free fleet server config file. For a sanity check, if you compare the coordinate of the robot reported in its FleetState msg with the actual RMF coordinate of that location (can be inferred from Traffic Editor), what is the error?