search

open-rmf / rmf_task

RMF library for managing task allocations
Apache License 2.0
22 stars 22 forks source link

Unable to specify parking waypoint for `ParkRobotIndefinitely` request factory #115

Open CarlyyyChen opened 6 months ago

CarlyyyChen commented 6 months ago

Before proceeding, is there an existing issue or discussion for this?

OS and version

Ubuntu 22.04

Open-RMF installation type

Source build

Other Open-RMF installation methods

No response

Open-RMF version or commit hash

2.0.3

ROS distribution

Humble

ROS installation type

Source build

Other ROS installation methods

No response

Package or library, if applicable

No response

Description of the bug

I want to make some changes in ParkRobotFactory class (currently it is using dedicated_charging_waypoint when a parking waypoint is not passed in, I want to change it to a dedicated parking waypoint so that the robot will be able to park at a dedicated parking waypoint instead of its charger). In order to make that change, I have cloned the rmf_task package from your repo to local, together with your rmf_demos codes, and all packages are building fine. However, when I use ros2 launch rmf_demos_gz_classic office.launch.xml to launch your office demo, all navigation paths are not showing up in Rviz, and the robot is not spawned in the map either. In terminal there is a relevant error message as below.

[fleet_adapter-16] ImportError: /opt/ros/humble/lib/librmf_fleet_adapter.so: undefined symbol: _ZN8rmf_task4Task7BookingC1ENSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEENSt6chrono10time_pointINS8_3_V212steady_clockENS8_8durationIlSt5ratioILl1ELl1000000000EEEEEESt10shared_ptrIKNS_8PriorityEERKS7_SG_b [ERROR] [fleet_adapter-16]: process has died [pid 8350, exit code 1, cmd '/home/agsw/Desktop/teabot-http/install/rmf_demos_fleet_adapter/lib/rmf_demos_fleet_adapter/fleet_adapter -c /home/agsw/Desktop/teabot-http/install/rmf_demos/share/rmf_demos/config/20th_floor/tinyRobot_config.yaml -n /home/agsw/Desktop/teabot-http/install/rmf_demos_maps/share/rmf_demos_maps/maps/20th_floor/nav_graphs/0.yaml -sim --ros-args --params-file /tmp/launch_params_93ybb49j --params-file /tmp/launch_params_d36yp8ry'].

The local rmf_task package shall take priority over the underlay workspace (/opt/ros/humble). And I have tried to clone rmf_fleet_adapter_python package locally with the rmf_demos packages, it is working fine (builds fine, and Rviz launches fine). The issue is only with the rmf_task package. Hope you can shed some light on above error. Suggestions on other approaches to implement that change to ParkRobotFactory class are also welcome! Thanks much in advance.

Steps to reproduce the bug

  1. clone rmf_demos codes
  2. close rmf_task package, put it alongside with other packages in rmf_demos
  3. colcon build, and source install/setup.bash
  4. ros2 launch rmf_demos_gz_classic office.launch.xml to launch the office demo
  5. navigation paths and robots are not showing up in Rviz

Expected behavior

No response

Actual behavior

No response

Additional information or screenshots

This is the file structure in my workspace. image

Yadunund commented 6 months ago

Hi @CarlyyyChen,

Thanks for highlighting your problem.

Your experience highlights a problem in our EasyFullControl implementation which is used by the robot fleets in rmf_demos. See rmf_demos_fleet_adapter.

The issue is that as seen in the implementation linked above, we construct the ParkRobotIndefinitely factory without passing in a parking_waypoint. As a result, the default std::nullopt is passed which sets the parking waypoint as the charger waypoint and the robot always goes to it's charging waypoint assigned to the robot in the fleet config.

Here's my proposal for a more proper fix:

  1. Update the rmf_task::State class to get/set a dedicated_parking_waypoint field similar to the existing dedicated_charging_waypoint.
  2. Add a parking field to the config similar to charger.
  3. Add std::vector<std::size_t> parking_waypoints to EasyFullControl::RobotConfiguration. And have EasyFullControl::Configuration::from_config_file to parse the parking field from the config and append the right waypoint to RobotConfiguration.
  4. Add a RobotUpdateHandle::set_parking_waypoint() API here.
  5. Take the parking waypoint form the RobotConfig and set it for the RobotUpdateHandle returned FleetUpdateHandle::add_robot() is called here
  6. Update RobotContext to make and get an rmf_task::State with the dedicated_parking_waypoint property
  7. Finally, update the ParkRobotIndefinitely::make_request() to actually plan to the state.dedicated_parking_waypoint()

@mxgrey @arjo129 I think this approach might play nicely with the Reservation system integration but please share otherwise!

CarlyyyChen commented 6 months ago

Hi @Yadunund thanks for suggesting the plan. It is exactly the same as I have planned. But I was stuck at the very first step -- adding rmf_task package to local in order to change it. Appreciate if you can advise more on the ImportError and issue with Rviz!