Registering a New Node as Part of the TinyRobot Fleet Adapter (#35)

Open-RMF Open-RMF General
1

Posted by @tongkiat:

I’m trying to write a node to work with the given /TinyRobot_fleet_adapter. I’ll publish my state to the /robot_state, and I’ll subscribe to the /robot_mode_requests, the /robot_path_requests, and the /robot_pause_requests.

I wrote a node named “TinyRobot_123” and started publishing its state to the /robot_state. The /TinyRobot_state_aggregator is publishing my node’s state to the /fleet_states. But, somehow the /TinyRobot_fleet_adapter crashed.

Is there a way to register my node with the adapter without crashing it?

2

Posted by @mxgrey:

It sounds like you’re trying to use the “legacy” fleet driver API to integrate a robot into RMF. I would very strongly discourage this and instead look over the integration instructions that can be found here.

That being said, I do find it somewhat concerning that the legacy fleet adapter would be crashing. If you’re able to tell us anything more about the crash (e.g. Does it print anything to the terminal before crashing? What information are you publishing in your robot state messages?) then I can look into debugging it. But generally, I don’t recommend pursuing this path for integration since it’s outdated and will not be supported into the future.

3

Posted by @tongkiat:

Thanks for your prompt reply. Yes, I’ll look at the integration instructions you provided above.

The info I was publishing to /robot_state is as follows:

name: TinyRobot_123
model: ''
task_id: ''
seq: 1
mode:
  mode: 1
  mode_request_id: 0
battery_percent: 100.0
location:
  t:
    sec: 21
    nanosec: 569000000
  x: 53.08000183105469
  y: -45.52000045776367
  yaw: -3.0899999141693115
  level_name: L3
  index: 0
path: []

The error I got from the fleet adapter is something like this:

[full_control-1] full_control: /home/user/rmf_ws/src/rmf/rmf_ros2/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/../services/../jobs/detail/impl_SearchForPath.hpp:145: 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-1]: process has died [pid 12030, exit code -6, cmd '/home/user/rmf_ws/install/rmf_fleet_adapter/lib/rmf_fleet_adapter/full_control --ros-args -r __node:=TinyRobot_fleet_adapter --params-file /tmp/launch_params_oq3f7dit --params-file /tmp/launch_params_deab6os7 --params-file /tmp/launch_params_rd52_913 --params-file /tmp/launch_params_zphoty5b --params-file /tmp/launch_params_g04f2w9t --params-file /tmp/launch_params__40erd8d --params-file /tmp/launch_params_572g2o78 --params-file /tmp/launch_params_gxrc4d_w --params-file /tmp/launch_params_tsm3t68a --params-file /tmp/launch_params_pq22ndmk --params-file /tmp/launch_params_moilm32_ --params-file /tmp/launch_params_xq2ch6xz --params-file /tmp/launch_params_yr87owcb --params-file /tmp/launch_params_jgbkfx35 --params-file /tmp/launch_params_7vq_9y_l --params-file /tmp/launch_params_tv6s7yen --params-file /tmp/launch_params_0emd8deb --params-file /tmp/launch_params_m_0mmal_ --params-file /tmp/launch_params_37yrl8ih --params-file /tmp/launch_params_i8nvh_0z --params-file /tmp/launch_params_jc57zbyt --params-file /tmp/launch_params_ro1vhz6i --params-file /tmp/launch_params_axjqfsa7 --params-file /tmp/launch_params_y0xh3pkl --params-file /tmp/launch_params_hv5g4q8_ --params-file /tmp/launch_params_ucb3_rhe --params-file /tmp/launch_params_x8w0n9fg --params-file /tmp/launch_params_ywery6w1'].

Thank you. :smiley:

4

Posted by @mxgrey:

There should have been some output from the [full_control-1] process that comes before the line you pasted here. It should say something like

[SearchForPath] CRITICAL ERROR: Failed to find an acceptable greedy solution. ...

and then print out some other information.

This error shouldn’t happen if you’re using any recent version of RMF (i.e. any release or version that has come out since January). If you’re encountering this error when you use the latest main branches of rmf_traffic and rmf_ros2, then something very very strange is happening. If you encounter this again, it may help us debug the problem if you can provide all the terminal output for the error.

5

Posted by @tongkiat:

Sorry, I changed the environment and setup slightly. Now, I’m launching the full_control node, named “play_fleet”, directly. The state_aggregator is accepting robots with the prefix “playbot”.

I’ve updated all the repos and rebuilt everything. This is the log file I got.

1619606760.9193244 [INFO] [launch]: All log files can be found below /home/user/.ros/log/2021-04-28-18-45-59-916738-user-94851
1619606759.9195228 [INFO] [launch]: Default logging verbosity is set to INFO
1619606759.9876983 [INFO] [rmf_traffic_schedule-1]: process started with pid [94853]
1619606759.9878483 [INFO] [robot_state_aggregator-2]: process started with pid [94855]
1619606759.9879396 [INFO] [full_control-3]: process started with pid [94857]
1619606760.0153234 [rmf_traffic_schedule-1] [INFO] [1619606760.015075663] [rmf_traffic_schedule_node]: Successfully loaded logfile .rmf_schedule_node.yaml
1619606760.0227554 [rmf_traffic_schedule-1] [INFO] [1619606760.022531226] [rmf_traffic_schedule_node]: Beginning traffic schedule node
1619606760.0302696 [full_control-3] [INFO] [1619606760.030015150] [play_fleet_adapter]: Parameter [discovery_timeout] set to: 10.000000
1619606760.0324509 [rmf_traffic_schedule-1] [INFO] [1619606760.032290412] [rmf_traffic_schedule_node]: [1] Registered query
1619606760.0437527 [full_control-3] [INFO] [1619606760.043460226] [play_fleet_adapter]: Parameter [linear_velocity] set to: 0.200000
1619606760.0438812 [full_control-3] [INFO] [1619606760.043517505] [play_fleet_adapter]: Parameter [angular_velocity] set to: 0.200000
1619606760.0441232 [full_control-3] [INFO] [1619606760.043574823] [play_fleet_adapter]: Parameter [linear_acceleration] set to: 0.500000
1619606760.0441985 [full_control-3] [INFO] [1619606760.043598822] [play_fleet_adapter]: Parameter [angular_acceleration] set to: 1.000000
1619606760.0442765 [full_control-3] [INFO] [1619606760.043617435] [play_fleet_adapter]: Parameter [footprint_radius] set to: 0.300000
1619606760.0444093 [full_control-3] [INFO] [1619606760.043664476] [play_fleet_adapter]: Parameter [vicinity_radius] set to: 1.000000
1619606760.0444660 [full_control-3] [INFO] [1619606760.043685434] [play_fleet_adapter]: Parameter [reversible] set to: 1
1619606760.0463479 [full_control-3] The fleet [play_fleet] has the following named waypoints:
1619606760.0463848 [full_control-3]  -- WP12
1619606760.0464110 [full_control-3]  -- guest3
1619606760.0464346 [full_control-3]  -- guest2
1619606760.0464571 [full_control-3]  -- WP1
1619606760.0464787 [full_control-3]  -- WP6
1619606760.0465004 [full_control-3]  -- guest4
1619606760.0465217 [full_control-3]  -- WP11
1619606760.0465431 [full_control-3]  -- TinyRobot_start
1619606760.0465643 [full_control-3]  -- WP2
1619606760.0465858 [full_control-3]  -- WP3
1619606760.0466259 [full_control-3]  -- WP7
1619606760.0466473 [full_control-3]  -- guest1
1619606760.0466812 [full_control-3]  -- WP4
1619606760.0467021 [full_control-3]  -- WP10
1619606760.0467424 [full_control-3]  -- Lift3
1619606760.0467765 [full_control-3]  -- guest7
1619606760.0467975 [full_control-3]  -- l1_restroom
1619606760.0468183 [full_control-3]  -- dining
1619606760.0468390 [full_control-3]  -- WP5
1619606760.0468595 [full_control-3]  -- WP8
1619606760.0468805 [full_control-3]  -- cafe
1619606760.0469012 [full_control-3]  -- WP9
1619606760.0500102 [full_control-3] [INFO] [1619606760.049795781] [play_fleet_adapter]: Parameter [battery_voltage] set to: 12.000000
1619606760.0501337 [full_control-3] [INFO] [1619606760.049824496] [play_fleet_adapter]: Parameter [battery_capacity] set to: 24.000000
1619606760.0503550 [full_control-3] [INFO] [1619606760.049933133] [play_fleet_adapter]: Parameter [battery_charging_current] set to: 5.000000
1619606760.0504425 [full_control-3] [INFO] [1619606760.049997494] [play_fleet_adapter]: Parameter [mass] set to: 20.000000
1619606760.0505114 [full_control-3] [INFO] [1619606760.050106031] [play_fleet_adapter]: Parameter [inertia] set to: 10.000000
1619606760.0505667 [full_control-3] [INFO] [1619606760.050137008] [play_fleet_adapter]: Parameter [friction_coefficient] set to: 0.220000
1619606760.0506282 [full_control-3] [INFO] [1619606760.050164082] [play_fleet_adapter]: Parameter [ambient_power_drain] set to: 20.000000
1619606760.0506861 [full_control-3] [INFO] [1619606760.050186612] [play_fleet_adapter]: Parameter [tool_power_drain] set to: 0.000000
1619606760.0507748 [full_control-3] [INFO] [1619606760.050202730] [play_fleet_adapter]: Parameter [drain_battery] set to: 0
1619606760.0508356 [full_control-3] [INFO] [1619606760.050218245] [play_fleet_adapter]: Parameter [recharge_threshold] set to: 0.200000
1619606760.0510619 [full_control-3] [INFO] [1619606760.050369166] [play_fleet_adapter]: Parameter [delay_threshold] set to: 10.000000
1619606760.0513265 [full_control-3] [INFO] [1619606760.051210923] [play_fleet_adapter]: Starting Fleet Adapter
1619606760.4262409 [rmf_traffic_schedule-1] [INFO] [1619606760.425672225] [rmf_traffic_schedule_node]: Registered participant [0] named [playbot_8080] owned by [play_fleet]
1619606760.4314501 [full_control-3] [INFO] [1619606760.430970903] [play_fleet_adapter]: Added a robot named [playbot_8080] with participant ID [0]
1619606760.4381654 [full_control-3] [SearchForPath] CRITICAL ERROR: Failed to find an acceptable greedy solution. Participant [playbot_8080] owned by [play_fleet] Requested path ([5] r:-3.09 | no lane) --> (5). Maximum cost: 0 | Leeway factor: 10 | Current cost: 8.45765 | Saturated: 0 (limit: 10000) | interrupted: 0
1619606760.4384818 [full_control-3] linear | v: 0.2, a: 0.5
1619606760.4386051 [full_control-3] angular | v: 0.2, a: 1
1619606760.4386873 [full_control-3] full_control: /home/user/rmf_ws/src/rmf/rmf_ros2/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/../services/../jobs/detail/impl_SearchForPath.hpp:145: 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.
1619606760.5648527 [ERROR] [full_control-3]: process has died [pid 94857, exit code -6, cmd '/home/user/rmf_ws/install/rmf_fleet_adapter/lib/rmf_fleet_adapter/full_control --ros-args -r __node:=play_fleet_adapter --params-file /tmp/launch_params__0lvq4qr --params-file /tmp/launch_params_uc9_x27r --params-file /tmp/launch_params_lcx7wgyv --params-file /tmp/launch_params_e0bap4ly --params-file /tmp/launch_params_s6jfomj1 --params-file /tmp/launch_params_unwq_r5q --params-file /tmp/launch_params_h4qvcw75 --params-file /tmp/launch_params_giz1trlb --params-file /tmp/launch_params_n7tdtyyy --params-file /tmp/launch_params_2x9jpsbv --params-file /tmp/launch_params_wmgcwidj --params-file /tmp/launch_params_nw7x6tt0 --params-file /tmp/launch_params_2uw2h9is --params-file /tmp/launch_params_litcw7b4 --params-file /tmp/launch_params_realj_j3 --params-file /tmp/launch_params_ksw3kq0y --params-file /tmp/launch_params_2ob6qglk --params-file /tmp/launch_params_h0ox1t78 --params-file /tmp/launch_params_9l_dd9_t --params-file /tmp/launch_params_1kv_9mc5 --params-file /tmp/launch_params_iqmmpag5 --params-file /tmp/launch_params_sxflli3v --params-file /tmp/launch_params_95rkczw3 --params-file /tmp/launch_params_vpxcsdo5 --params-file /tmp/launch_params_4l84y2_j --params-file /tmp/launch_params_mfbp1_cf --params-file /tmp/launch_params_528ue8bp'].
1619606913.7851038 [WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
1619606913.7898216 [rmf_traffic_schedule-1] [INFO] [1619606913.784879364] [rclcpp]: signal_handler(signal_value=2)
1619606913.7902119 [rmf_traffic_schedule-1] [INFO] [1619606913.785290098] [rmf_traffic_schedule_node]: Closing down traffic schedule node
1619606913.7906702 [robot_state_aggregator-2] [INFO] [1619606913.784845107] [rclcpp]: signal_handler(signal_value=2)
1619606913.7932818 [INFO] [robot_state_aggregator-2]: process has finished cleanly [pid 94855]
1619606913.8218358 [INFO] [rmf_traffic_schedule-1]: process has finished cleanly [pid 94853]
6

Posted by @mxgrey:

Thanks for providing this additional information. This does help me have a better idea of how the crash could’ve happened.

Can I check: Is there any command (e.g. task request) that you ran which led to this crash, or did the crash happen just after launching the demo?

Also, if you’re able to share the packages that trigger this crash, it would be very very helpful for us to debug and test the solution for this problem.

7

Posted by @eliasdc:

I’m having the same problem with the legacy fleet driver API.

[full_control-13] [SearchForPath] CRITICAL ERROR: Failed to find an acceptable greedy solution. Participant [test_robot] owned by [fleet_proto] Requested path ([5] r:-1.54586 | no lane) --> (5). Maximum cost: 0 | Leeway factor: 10 | Current cost: 0.257846 | Saturated: 0 (limit: 10000) | interrupted: 0
[full_control-13] linear | v: 0.5, a: 0.5
[full_control-13] angular | v: 1.5, a: 1.5
[full_control-13] full_control: ...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.

This happens directly on boot, with none bidirectional lanes. The robot is unable to plan the initial location. If I change the robot’s initial position it sometimes does work and is able to plan. My max offset from a waypoint is 10m so this should be possible as my test environment is much smaller.

edit 1:
Just tested from the same location with bidirectional lanes and a reversible robot and I receive the same error. I do have in my lanes two vertices close to each other just to force the orientation on the robot on a specific vertex.

edit 2:
New test with the new driver API and still having the same problem. I assume it is because two points are close to each other. Instant fail on launch when the robot is positioned around those two close points.

[rmf_traffic_schedule-1] [INFO] [1652170399.567188574] [rmf_traffic_schedule_primary]: Registered participant [14] named [robot_test] owned by [proto]
[server_node-13] [INFO] [1652170399.580196495] [fleet_server]: Added a robot named [robot_test] with participant ID [14]
[server_node-13] Info [Manager.cpp:handle_robot_state:87] Registered new robot: [robot_test]
[server_node-13] [SearchForPath] CRITICAL ERROR: Failed to find an acceptable greedy solution. Participant [robot_test] owned by [proto] Requested path ([0] r:1.58528 | no lane) --> (0). Maximum cost: 0 | Leeway factor: 10 | Current cost: 0 | Saturated: 0 (limit: 10000) | interrupted: 0
[server_node-13] linear | v: 0.5, a: 0.5
[server_node-13] angular | v: 1.5, a: 1.5
[server_node-13] server_node: /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.

Edited by @eliasdc at 2022-05-10T08:19:29Z