search

open-rmf / free_fleet

A free fleet management system.
Apache License 2.0
156 stars 65 forks source link

RMF integration Issue #135

Closed MatteoCarlone closed 1 year ago

MatteoCarlone commented 1 year ago

Required information:

Description

I'm working on a create3 robot in a gazebo simulation environment, and nav2 tested and working. I'm able launch correctly the ff server and client and also submit goals -> the following are the terminals ouputs:

FF Server

[free_fleet_server_ros2-1] Greetings from free_fleet_server_ros2
[free_fleet_server_ros2-1] ROS 2 SERVER CONFIGURATION
[free_fleet_server_ros2-1]   fleet name: mci_fleet
[free_fleet_server_ros2-1]   update state frequency: 20.0
[free_fleet_server_ros2-1]   publish state frequency: 2.0
[free_fleet_server_ros2-1]   TOPICS
[free_fleet_server_ros2-1]     fleet state: fleet_states
[free_fleet_server_ros2-1]     mode request: robot_mode_requests
[free_fleet_server_ros2-1]     path request: robot_path_requests
[free_fleet_server_ros2-1]     destination request: robot_destination_requests
[free_fleet_server_ros2-1] SERVER-CLIENT DDS CONFIGURATION
[free_fleet_server_ros2-1]   dds domain: 42
[free_fleet_server_ros2-1]   TOPICS
[free_fleet_server_ros2-1]     robot state: robot_state
[free_fleet_server_ros2-1]     mode request: mode_request
[free_fleet_server_ros2-1]     path request: path_request
[free_fleet_server_ros2-1]     destination request: destination_request
[free_fleet_server_ros2-1] COORDINATE TRANSFORMATION
[free_fleet_server_ros2-1]   translation x (meters): 0.000
[free_fleet_server_ros2-1]   translation y (meters): 0.000
[free_fleet_server_ros2-1]   rotation (radians): 0.000
[free_fleet_server_ros2-1]   scale: 1.000
[free_fleet_server_ros2-1] [INFO] [1682082649.195530283] [mci_server_node]: registered a new robot: [robot]

FF Client

[free_fleet_client_ros2-1] [INFO] [1682082729.961931728] [mci_client_node]: Greetings from mci_client_node
[free_fleet_client_ros2-1] ROS 2 CLIENT CONFIGURATION
[free_fleet_client_ros2-1]   fleet name: mci_fleet
[free_fleet_client_ros2-1]   robot name: robot
[free_fleet_client_ros2-1]   robot model: robot_model
[free_fleet_client_ros2-1]   level name: map
[free_fleet_client_ros2-1]   wait timeout: 10.0
[free_fleet_client_ros2-1]   update request frequency: 10.0
[free_fleet_client_ros2-1]   publish state frequency: 1.0
[free_fleet_client_ros2-1]   maximum distance to first waypoint: 17.0
[free_fleet_client_ros2-1]   TOPICS
[free_fleet_client_ros2-1]     battery state: /battery_state
[free_fleet_client_ros2-1]     move base server: /navigate_to_pose
[free_fleet_client_ros2-1]     docking trigger server: 
[free_fleet_client_ros2-1]   ROBOT FRAMES
[free_fleet_client_ros2-1]     map frame: map
[free_fleet_client_ros2-1]     robot frame: base_footprint
[free_fleet_client_ros2-1] CLIENT-SERVER DDS CONFIGURATION
[free_fleet_client_ros2-1]   dds domain: 42
[free_fleet_client_ros2-1]   TOPICS
[free_fleet_client_ros2-1]     robot state: robot_state
[free_fleet_client_ros2-1]     mode request: mode_request
[free_fleet_client_ros2-1]     path request: path_request
[free_fleet_client_ros2-1]     destination request: destination_request
[free_fleet_client_ros2-1] [INFO] [1682082729.965220144] [mci_client_node]: waiting for connection with navigation action server: /navigate_to_pose
[free_fleet_client_ros2-1] [INFO] [1682082732.103828787] [mci_client_node]: connected with move base action server: /navigate_to_pose
[free_fleet_client_ros2-1] [INFO] [1682082732.115180647] [mci_client_node]: starting update timer.
[free_fleet_client_ros2-1] [INFO] [1682082732.115283924] [mci_client_node]: starting publish timer.
[free_fleet_client_ros2-1] [INFO] [1682082815.014114023] [mci_client_node]: received a Destination command, x: 1.00, y: 0.70, yaw: 0.00
[free_fleet_client_ros2-1] [INFO] [1682082815.014322522] [mci_client_node]: sending next goal.
[free_fleet_client_ros2-1] [INFO] [1682082815.014989892] [mci_client_node]: Goal accepted by server, waiting for result
[free_fleet_client_ros2-1] [ERROR] [1682082815.015294781] [mci_client_node]: Goal was canceled
[free_fleet_client_ros2-1] [INFO] [1682082815.113969278] [mci_client_node]: sending next goal.
[free_fleet_client_ros2-1] [INFO] [1682082815.114338979] [mci_client_node]: Goal accepted by server, waiting for result
[free_fleet_client_ros2-1] [INFO] [1682082815.115032935] [mci_client_node]: Distance remaining: 0.235408
[free_fleet_client_ros2-1] [INFO] [1682082820.204779101] [mci_client_node]: Distance remaining: 0.225000
[free_fleet_client_ros2-1] [INFO] [1682082822.874965753] [mci_client_node]: current goal state: SUCCEEEDED.

Send Destination

ros2 run ff_examples_ros2 send_destination_request.py -f mci_fleet -r robot -x 1.0 -y 0.7 --yaw 0.0 -i 34724
fleet_name: mci_fleet
robot_name: robot
x: 1.0
y: 0.7
yaw: 0.0
level_name: B1
task_id: 34724
topic_name: robot_destination_requests
all done!

Questions

Now to integrate this with RMF I did the following:

----

- fleet adapter
    basically taken from [HERE](https://github.com/osrf/rmf_demos/blob/master/demos/launch/include/adapters/tinyRobot_adapter.launch.xml)

that's the failing output that I get when starting rmf up

[INFO] [launch]: Default logging verbosity is set to INFO [INFO] [rmf_traffic_schedule-1]: process started with pid [479366] [INFO] [rmf_traffic_blockade-2]: process started with pid [479368] [INFO] [building_map_server-3]: process started with pid [479370] [INFO] [schedule_visualizer_node-4]: process started with pid [479372] [INFO] [fleetstates_visualizer_node-5]: process started with pid [479374] [INFO] [rmf_visualization_building_systems-6]: process started with pid [479376] [INFO] [navgraph_visualizer_node-7]: process started with pid [479378] [INFO] [floorplan_visualizer_node-8]: process started with pid [479380] [INFO] [obstacle_visualizer_node-9]: process started with pid [479382] [INFO] [rviz2-10]: process started with pid [479384] [INFO] [rmf_task_dispatcher-11]: process started with pid [479386] [INFO] [simple_api_server-12]: process started with pid [479388] [INFO] [full_control-13]: process started with pid [479406] [INFO] [robot_state_aggregator-14]: process started with pid [479420] [rviz2-10] Warning: Ignoring XDG_SESSION_TYPE=wayland on Gnome. Use QT_QPA_PLATFORM=wayland to run on Wayland anyway. [rmf_task_dispatcher-11] ~Initializing Dispatcher Node~ [fleetstates_visualizer_node-5] [INFO] [1682099569.808648972] [fleet_states_visualizer]: Configuring fleet_states_visualizer... [fleetstates_visualizer_node-5] [INFO] [1682099569.808784827] [fleet_states_visualizer]: Setting parameter initial_map_name to L1 [fleetstates_visualizer_node-5] [INFO] [1682099569.808820468] [fleet_states_visualizer]: Setting parameter fleet_state_nose_scale to 0.500000 [rmf_traffic_schedule-1] [INFO] [1682099569.815119965] [rmf_traffic_schedule_node_5992fdaa_ada6_4107_af03_b2b4c5f17823]: Successfully loaded logfile .rmf_schedule_node.yaml [rmf_traffic_schedule-1] [INFO] [1682099569.819879851] [rmf_traffic_schedule_node_5992fdaa_ada6_4107_af03_b2b4c5f17823]: Set up heartbeat on /rmf_traffic/heartbeat with liveliness lease duration of 10000 ms and deadline of 10000 ms [obstacle_visualizer_node-9] [INFO] [1682099569.829036516] [rmf_obstacle_visualizer]: Beginning RMF obstacle visualizer node [fleetstates_visualizer_node-5] [INFO] [1682099569.831865232] [fleet_states_visualizer]: Running fleet_states_visualizer... [schedule_visualizer_node-4] [INFO] [1682099569.868008501] [schedule_visualizer_node]: Setting parameter rate to 10.000000 [schedule_visualizer_node-4] [INFO] [1682099569.868165197] [schedule_visualizer_node]: Setting parameter path_width to 0.200000 [schedule_visualizer_node-4] [INFO] [1682099569.868207909] [schedule_visualizer_node]: Setting parameter initial_map_name to map [schedule_visualizer_node-4] [INFO] [1682099569.868243006] [schedule_visualizer_node]: Setting parameter wait_secs to 10 [schedule_visualizer_node-4] [INFO] [1682099569.868279605] [schedule_visualizer_node]: Setting parameter port to 8006 [schedule_visualizer_node-4] [INFO] [1682099569.868310943] [schedule_visualizer_node]: Setting parameter retained_history_count to 50 [rmf_task_dispatcher-11] [INFO] [1682099569.929603733] [rmf_dispatcher_node]: Declared Time Window Param as: 2.000000 secs [rmf_task_dispatcher-11] [INFO] [1682099569.929881155] [rmf_dispatcher_node]: Declared Terminated Tasks Max Size Param as: 100 [rmf_task_dispatcher-11] [INFO] [1682099569.929921377] [rmf_dispatcher_node]: Declared publish_active_tasks_period as: 2 secs [rmf_task_dispatcher-11] [INFO] [1682099569.929954764] [rmf_dispatcher_node]: Use timestamp with task_id: false [rmf_task_dispatcher-11] [INFO] [1682099569.945550109] [rmf_dispatcher_node]: Starting task dispatcher node [navgraph_visualizer_node-7] [INFO] [1682099570.194217228] [navgraph_visualizer]: Setting parameter initial_map_name to map [navgraph_visualizer_node-7] [INFO] [1682099570.194661246] [navgraph_visualizer]: Setting parameter lane_width to 0.500000 [navgraph_visualizer_node-7] [INFO] [1682099570.194706734] [navgraph_visualizer]: Setting parameter lane_transparency to 0.600000 [navgraph_visualizer_node-7] [INFO] [1682099570.194728621] [navgraph_visualizer]: Setting parameter waypoint_scale to 1.300000 [navgraph_visualizer_node-7] [INFO] [1682099570.194745749] [navgraph_visualizer]: Setting parameter text_scale to 0.700000 [navgraph_visualizer_node-7] [INFO] [1682099570.216793033] [navgraph_visualizer]: NavGraph visualizer is running... [rmf_traffic_schedule-1] [INFO] [1682099570.218437268] [rmf_traffic_schedule_node_5992fdaa_ada6_4107_af03_b2b4c5f17823]: Beginning traffic schedule node [rmf_traffic_blockade-2] [INFO] [1682099570.232294176] [rmf_traffic_blockade_node]: Beginning traffic blockade node [full_control-13] [INFO] [1682099570.277464260] [mci_fleet_fleet_adapter]: Parameter [discovery_timeout] set to: 60.000000 [rmf_visualization_building_systems-6] [INFO] [1682099570.334363118] [building_systems_visualizer]: Building systems visualizer started... [rmf_visualization_building_systems-6] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: HistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST is deprecated. Use HistoryPolicy.KEEP_LAST instead. [rmf_visualization_building_systems-6] warnings.warn( [rmf_visualization_building_systems-6] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE is deprecated. Use ReliabilityPolicy.RELIABLE instead. [rmf_visualization_building_systems-6] warnings.warn( [rmf_visualization_building_systems-6] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL is deprecated. Use DurabilityPolicy.TRANSIENT_LOCAL instead. [rmf_visualization_building_systems-6] warnings.warn( [rviz2-10] [INFO] [1682099570.428700236] [rviz2]: Stereo is NOT SUPPORTED [rviz2-10] [INFO] [1682099570.428766000] [rviz2]: OpenGl version: 4.6 (GLSL 4.6) [rviz2-10] [INFO] [1682099570.444632519] [rviz2]: Stereo is NOT SUPPORTED [floorplan_visualizer_node-8] [INFO] [1682099570.565447763] [floorplan_visualizer]: Configuring floorplan_visualizer... [floorplan_visualizer_node-8] [INFO] [1682099570.565665170] [floorplan_visualizer]: Setting parameter initial_map_name to map [floorplan_visualizer_node-8] [INFO] [1682099570.575944154] [floorplan_visualizer]: Running floorplan_visualizer... [building_map_server-3] [INFO] [1682099570.740936065] [building_map_server]: loading map path: /home/matteocarlone/ff_mci_ws/install/ff_examples_ros2/share/ff_examples_ros2/maps/ff.building.yaml [building_map_server-3] [INFO] [1682099570.743431284] [building_map_server]: opening: /home/matteocarlone/ff_mci_ws/install/ff_examples_ros2/share/ff_examples_ros2/maps/sim_arena.png [building_map_server-3] [INFO] [1682099570.744086296] [building_map_server]: read 592 byte image [building_map_server-3] [INFO] [1682099570.744706538] [building_map_server]: unable to generate GeoJSON for this map. [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' [simple_api_server-12] Debug mode: off [simple_api_server-12] Exception in thread Thread-2 (broadcast_states): [simple_api_server-12] Traceback (most recent call last): [simple_api_server-12] File "/usr/lib/python3.10/threading.py", line 1016, in _bootstrap_inner [simple_api_server-12] self.run() [simple_api_server-12] File "/usr/lib/python3.10/threading.py", line 953, in run [simple_api_server-12] self._target(*self._args, *self._kwargs) [simple_api_server-12] File "/home/matteocarlone/ws_rmf_demos/install/rmf_demos_panel/lib/python3.10/site-packages/rmf_demos_panel/simple_api_server.py", line 135, in broadcast_states [simple_api_server-12] socketio.emit('task_status', tasks, broadcast=True, namespace=ns) [simple_api_server-12] File "/home/matteocarlone/.local/lib/python3.10/site-packages/flask_socketio/init.py", line 462, in emit [simple_api_server-12] self.server.emit(event, args, namespace=namespace, to=to, [simple_api_server-12] TypeError: Server.emit() got an unexpected keyword argument 'broadcast' [building_map_server-3] [INFO] [1682099570.780837658] [building_map_server]: publishing map... [building_map_server-3] [INFO] [1682099570.781793542] [building_map_server]: ready to serve map: "ff" Ctrl+C to exit... [rmf_traffic_schedule-1] [INFO] [1682099571.139763078] [rmf_traffic_schedule_node_5992fdaa_ada6_4107_af03_b2b4c5f17823]: Registered new query [1] [schedule_visualizer_node-4] [INFO] [1682099571.144327194] [schedule_data_node]: Registering to query topic rmf_traffic/query_update_1 [schedule_visualizer_node-4] [INFO] [1682099571.153465683] [schedule_data_node]: [rmf_traffic_ros2::MirrorManager::request_update] Requesting changes for query ID [1] since beginning of recorded history [rmf_traffic_schedule-1] [INFO] [1682099571.191508858] [rmf_traffic_schedule_node_5992fdaa_ada6_4107_af03_b2b4c5f17823]: A new mirror is tracking query ID [1] [schedule_visualizer_node-4] [INFO] [1682099571.198724487] [schedule_visualizer_node]: Successfully connected to rmf_traffic_schedule node. [full_control-13] [INFO] [1682099571.275803026] [mci_fleet_fleet_adapter]: Registering to query topic rmf_traffic/query_update_1 [navgraph_visualizer_node-7] [INFO] [1682099571.340311138] [navgraph_visualizer]: Publishing navgraphs on level L1 [full_control-13] [INFO] [1682099571.362359912] [mci_fleet_fleet_adapter]: Parameter [linear_velocity] set to: 0.500000 [full_control-13] [INFO] [1682099571.362596370] [mci_fleet_fleet_adapter]: Parameter [angular_velocity] set to: 0.600000 [full_control-13] [INFO] [1682099571.362969028] [mci_fleet_fleet_adapter]: Parameter [linear_acceleration] set to: 0.750000 [full_control-13] [INFO] [1682099571.363164118] [mci_fleet_fleet_adapter]: Parameter [angular_acceleration] set to: 2.000000 [full_control-13] [INFO] [1682099571.363331082] [mci_fleet_fleet_adapter]: Parameter [footprint_radius] set to: 0.150000 [full_control-13] [INFO] [1682099571.363445683] [mci_fleet_fleet_adapter]: Parameter [vicinity_radius] set to: 0.200000 [full_control-13] [INFO] [1682099571.363595511] [mci_fleet_fleet_adapter]: Parameter [reversible] set to: 1 [full_control-13] The fleet [mci_fleet] has the following named waypoints: [full_control-13] -- waypoint [full_control-13] -- start [full_control-13] [INFO] [1682099571.394862937] [mci_fleet_fleet_adapter]: Parameter [battery_voltage] set to: 12.000000 [full_control-13] [INFO] [1682099571.396088615] [mci_fleet_fleet_adapter]: Parameter [battery_capacity] set to: 24.000000 [full_control-13] [INFO] [1682099571.396992555] [mci_fleet_fleet_adapter]: Parameter [battery_charging_current] set to: 5.000000 [full_control-13] [INFO] [1682099571.397349704] [mci_fleet_fleet_adapter]: Parameter [mass] set to: 20.000000 [full_control-13] [INFO] [1682099571.397564011] [mci_fleet_fleet_adapter]: Parameter [inertia] set to: 10.000000 [full_control-13] [INFO] [1682099571.397771473] [mci_fleet_fleet_adapter]: Parameter [friction_coefficient] set to: 0.220000 [full_control-13] [INFO] [1682099571.398040366] [mci_fleet_fleet_adapter]: Parameter [ambient_power_drain] set to: 0.000000 [full_control-13] [INFO] [1682099571.398299185] [mci_fleet_fleet_adapter]: Parameter [tool_power_drain] set to: 0.000000 [full_control-13] [INFO] [1682099571.398493780] [mci_fleet_fleet_adapter]: Parameter [drain_battery] set to: 0 [full_control-13] [INFO] [1682099571.399221824] [mci_fleet_fleet_adapter]: Parameter [recharge_threshold] set to: 0.100000 [full_control-13] [INFO] [1682099571.399540091] [mci_fleet_fleet_adapter]: Parameter [recharge_soc] set to: 1.000000 [full_control-13] [INFO] [1682099571.399705641] [mci_fleet_fleet_adapter]: Fleet is not configured to perform any finishing request [full_control-13] [INFO] [1682099571.400439151] [mci_fleet_fleet_adapter]: Parameter [delay_threshold] set to: 15.000000 [full_control-13] [INFO] [1682099571.418287728] [mci_fleet_fleet_adapter]: Starting Fleet Adapter [full_control-13] [INFO] [1682099571.419099294] [mci_fleet_fleet_adapter]: Mirror handling new sync of 1 queries from schedule node [5992fdaa-ada6-4107-af03-b2b4c5f17823] [rmf_traffic_schedule-1] terminate called after throwing an instance of 'rclcpp::exceptions::RCLError' [rmf_traffic_schedule-1] what(): failed to send response: client will not receive response, at ./src/rmw_response.cpp:149, at ./src/rcl/service.c:314 [rviz2-10] Warning: Invalid frame ID "" passed to canTransform argument source_frame - in tf2 frame_ids cannot be empty [rviz2-10] at line 93 in ./src/buffer_core.cpp [rviz2-10] Warning: Invalid frame ID "" passed to canTransform argument source_frame - in tf2 frame_ids cannot be empty [rviz2-10] at line 93 in ./src/buffer_core.cpp [rviz2-10] [INFO] [1682099571.542762862] [rviz2]: Trying to create a map of size 73 x 125 using 1 swatches [rviz2-10] [ERROR] [1682099571.592001177] [rviz2]: Vertex Program:rviz/glsl120/indexed_8bit_image.vert Fragment Program:rviz/glsl120/indexed_8bit_image.frag GLSL link result : [rviz2-10] active samplers with a different type refer to the same texture image unit [full_control-13] [ERROR] [1682099571.692869663] [mci_fleet_fleet_adapter]: Unable to compute a StartSet for robot [robot] using level_name [L1] and location [0.019110, -0.027133, -0.001876] 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 [mci_fleet]. The following hint may help with debugging: The closest waypoint on the navigation graph [start] is a distance of [64.919612m] from the robot. [ERROR] [rmf_traffic_schedule-1]: process has died [pid 479366, exit code -6, cmd '/opt/ros/humble/lib/rmf_traffic_ros2/rmf_traffic_schedule --ros-args --params-file /tmp/launch_params_gfyjapk0'].


Do you know what I'm missing? I feel that I haven't really understood the role of the fleet adapter, of the transformation of coordinates from the robot and rmf that are supposed to be made in the server launch file and also how the ff client-server architecture manage the ros-namespaces.

Thanka a lot in advance for a possible help! 

### Image
![Screenshot from 2023-04-21 19-54-42](https://user-images.githubusercontent.com/81308076/233703135-3f415097-1acb-41ef-9e65-7afb3242a1fc.png)