Vicinity and Footprint (#179)

Open-RMF Open-RMF General
1

Posted by @cwrx777:

With reference to this, I have a few questions:

  1. Does open-rmf currently support changing footprint after initialization, or during task execution, e.g. after the AMR picks up a shelf, there’ll be an increase in footprint, and after drop-off, the footprint will decrease.
  2. Consider AMR with size of 600 mm x 900 mm. Due to space constraints in the charging room, AMR manufacturer allows that the AMRs are placed about 200 mm with each other. Even if the vicinity is set to 450mm (the length of the AMR), this will be considered as collision. Is there a workaround to avoid the AMRs from being de-conflict when they are at charging stations?
2

Posted by @mxgrey:

Theoretically the footprint and vicinity can be modified at any time by sending a new ParticipantDescription in a RegisterParticipant request to the rmf_traffic/register_participant service. When a participant with the same name as the description has already been registered, this service request will just update its values, including the footprint and vicinity.

We have tested and confirmed that the service request correctly updates participant information when restarting a fleet adapter, but we have never tested the result of using that service request in the middle of performing a task, which is why the rmf_fleet_adapter API does not provide a stable API for changing those parameters. However there is nothing to stop you from using this service request manually in your fleet adapter integration code.

One issue you might need to watch out for is that changing these parameters could mean that the existing traffic plans for your robots are no longer valid or no longer optimal, and the traffic schedule would not recognize that because it only checks for conflicts when a new plan comes in. If you manually send that service request, you should also manually trigger the RobotUpdateHandle::replan function for the robot whose values you are changing.

3

Posted by @cwrx777:

Hi,

is Box footprint supported?

4

Posted by @mxgrey:

No, boxes are not currently supported because we encountered bugs in FCL’s collision detection between boxes and circles moving along splines. We may consider supporting boxes in the future when we eventually migrate to a different collision detection implementation.

5

Posted by @cwrx777:

What is the right place to call register_participant and replan?
Currently, I’m using if the robot has reached a certain waypoint, then call register participant and replan.
I put it in follow_new_path method inside the robot_state == RobotState.MOVING, after the state become WAITING, e.g.
reference.

elif self.state == RobotState.MOVING:
	self.sleep_for(0.1)
	# Check if we have reached the target
	with self._lock:
		if (self.api.navigation_completed(self.name)):
			self.node.get_logger().info(
				f"Robot [{self.name}] has reached its target "
				f"waypoint")
			self.state = RobotState.WAITING
			if (self.target_waypoint.graph_index is not None):
				self.on_waypoint = \
					self.target_waypoint.graph_index
				self.last_known_waypoint_index = \
					self.on_waypoint
					
				# ==> call register_participant service
				# ==> call replan
			else:
				self.on_waypoint = None  # still on a lane
6

Posted by @mxgrey:

I can’t think of any particular rules about where or how those functions should be called. We have never tested this use case, so I can’t offer a reference implementation.

I’d suggest extensively testing your implementation in simulation and on a real robot. If you do find that it works okay, we’d love to hear to about it.

7

Posted by @cwrx777:

Hi,

I encountered self.node.get_clock().now() in the fleet adapter keep returning the same value after the fleet adapter called rmf_traffic/register_participant. And this caused the sleep_for method to run indefinitely. What could be the cause of this?
The fleet adapter is run using ros2 launch rmf_demos_gz office.launch.xml.

The register participant service client is defined here and is called from within test_topic subscription callback.

Publishing test_topic

ros2 topic pub /test_topic std_msgs/msg/String "{data: 'data' }" --once

Fleet adapter log:

fleet_adapter-14] [INFO] [1662087709.369547085] [tinyRobot_command_handle]: now = Time(nanoseconds=48694000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.370874787] [tinyRobot_command_handle]: now = Time(nanoseconds=48694000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.376711396] [tinyRobot_command_handle]: now = Time(nanoseconds=48694000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.429095777] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[rmf_traffic_schedule-1] [INFO] [1662087709.450222310] [rmf_traffic_schedule_primary]: Registered participant [6] named [tinyRobot1] owned by [tinyRobot]
[rmf_traffic_schedule-1] [INFO] [1662087709.457917821] [rmf_traffic_schedule_primary]: Registered participant [6] named [tinyRobot1] owned by [tinyRobot]
[fleet_adapter-14] [INFO] [1662087709.470829341] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.472169743] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.480726957] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[schedule_visualizer-5] [WARN] [1662087709.491005772] [rmf_visualization_schedule_data_node]: Failed to update using patch for DB version 783; requesting new update
[rmf_traffic_schedule_monitor-2] [WARN] [1662087709.491341573] [rmf_traffic_schedule_backup]: Failed to update using patch for DB version 783; requesting new update
[fleet_adapter-14] [WARN] [1662087709.491945274] [tinyRobot_fleet_adapter]: Failed to update using patch for DB version 783; requesting new update
[fleet_adapter-14] [INFO] [1662087709.530965834] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.582385213] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.601845643] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.604629948] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.632182090] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.683809670] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.702836599] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.705606203] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.733062846] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.785256526] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.804027755] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.806943160] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.834297702] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.886773583] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)
[fleet_adapter-14] [INFO] [1662087709.905331212] [tinyRobot_command_handle]: now = Time(nanoseconds=48766000000, clock_type=ROS_TIME)

Edited by @cwrx777 at 2022-09-02T03:39:52Z

8

Posted by @mxgrey:

This wouldn’t have anything to do with RMF.

Most likely the issue is that you’re calling spin_until_future_complete inside of a subscription callback, which would be getting run inside of a spinning executor. I’m pretty certain that recursive spinning is not something that rclpy supports.

I’ll also say that I can’t be certain if rclpy will work correctly in a fleet adapter integration Python script. I’m concerned that there could be some conflict between the C++ rclcpp connection to rcl and the Python script’s rclpy connection to rcl because both rclcpp and rclpy use some objects with static lifetimes. It might be fine, but I don’t know if this has been tested by anyone.

At a minimum to fix your problem you should not use spin_until_future_complete. I’d suggest using call_async instead. If there’s no conflict between the fleet adapter’s rclcpp and your script’s rclpy then that should help. Just make sure to call rclpy.spin_once in a threaded loop, like in your update function.

9

Posted by @cwrx777:

Hi @mxgrey ,

is .rmf_schedule_node.yaml expected to be updated if there is change in the footprint, after rmf_traffic/register_participant service is called?

Steps to replicate:

  1. delete .rmf_schedule_node.yaml
  2. Launch office demo. .rmf_schedule_node.yaml should be created and it contains tinyRobot1 with the footprint specified in its config file.
- operation: Add
  participant_description:
    name: tinyRobot1
    group: tinyRobot
    responsiveness: Responsive
    profile:
      footprint:
        type: Circle
        index: 0
      vicinity:
        type: Circle
        index: 1
      shape_context:
        - 0.3
        - 0.5
  1. Change tinyRobot1 footprint by manually editing the entry of tinyRobot1 in .rmf_schedule_node.yaml and save.
  2. Call rmf_traffic/register_participant service from CLI:
ros2 service call /rmf_traffic/register_participant rmf_traffic_msgs/srv/RegisterParticipant "{ description:   { name: 'tinyRobot1' ,   owner: 'tinyRobot' ,   responsiveness: 2 ,   profile:     { footprint: { type: 2 ,       index: 0  },  vicinity:  { type: 2, index: 1 } ,    shape_context: { circles: [ { radius: 0.55},{radius: 0.78} ] }   }   } }"

Expectation:

  1. Entry for tinyRobot1 in .rmf_schedule_node.yaml should be updated with the new footprint. However, it is reverted to the value right after the demo is launched.
10

Posted by @mxgrey:

When you launch your demo, the fleet adapter is likely registering the robot with vicinity and footprint values that you’re giving to the fleet adapter from a configuration file or perhaps specified in your launch file. Therefore when the fleet adapter launches and registers its robots, anything that was saved in the .rmf_schedule_node.yaml will be overwritten.

The .rmf_schedule_node.yaml is nothing but an on-disk backup of registered participants. It is not a source of truth, and it is not meant to be manually edited. It only exists to help us have some persistence between runs of the traffic schedule.

11

Posted by @cwrx777:

Hi @mxgrey ,

what is the correct way to confirm that the footprint has been updated? is it possible to query the source of truth ?

12

Posted by @mxgrey:

Do you want to see an update as a human observer, or do you want a program to recognize that the change has happened?

As a human you can watch the yellow dot in the rviz schedule visualizer.

A program could do one of the following:

If all you really care about is seeing the footprint and/or vicinity, then I suggest the second option because it will be far less overhead than creating an entire schedule mirror.

Edited by @mxgrey at 2022-09-07T14:28:58Z

13

Posted by @mxgrey:

As a quick and easy command line approach, you can do

ros2 topic echo --qos-reliability reliable --qos-durability transient_local /rmf_traffic/participants

Note that the quality and durability settings are necessary to make sure you get the most recent message that was published, because this message is only published when a change occurs.

14

Posted by @cwrx777:

Hi @mxgrey ,

I did the following test:

  1. run office demo
  2. ros topic echo on /rmf_traffic/participants as suggested.
ros2 topic echo --qos-reliability reliable --qos-durability transient_local /rmf_traffic/participants
  1. use CLI to call /rmf_traffic/register_participant to update tinyRobot1 profile. Result: the profile of tinyRobot1 in the /rmf_traffic/participants message remained the same as initial value.
ros2 service call /rmf_traffic/register_participant rmf_traffic_msgs/srv/RegisterParticipant "{ description:   { name: 'tinyRobot1' ,   owner: 'tinyRobot' ,   responsiveness: 2 ,   profile:     { footprint: { type: 2 ,       index: 0  },  vicinity:  { type: 2, index: 1 } ,    shape_context: { circles: [ { radius: 0.55},{radius: 0.70} ] }   }   } }"
  1. use CLI to call /rmf_traffic/register_participant to add a new robot, e.g. tinyRobot3. Result: tinyRobot3 was listed in the participant list in the /rmf_traffic/participants message, with profile as specified in the request payload. (the robot was also added in the .rmf_schedule_node.yml.)
ros2 service call /rmf_traffic/register_participant rmf_traffic_msgs/srv/RegisterParticipant "{ description:   { name: 'tinyRobot3' ,   owner: 'tinyRobot' ,   responsiveness: 2 ,   profile:     { footprint: { type: 2 ,       index: 0  },  vicinity:  { type: 2, index: 1 } ,    shape_context: { circles: [ { radius: 0.55},{radius: 0.78} ] }   }   } }"
  1. use CLI to call /rmf_traffic/register_participant to update tinyRobot3 foorprint. Result: tinyRobot3 profile in the /rmf_traffic/participants message was updated accordingly. (the robot was also updated in the .rmf_schedule_node.yml.)
ros2 service call /rmf_traffic/register_participant rmf_traffic_msgs/srv/RegisterParticipant "{ description:   { name: 'tinyRobot3' ,   owner: 'tinyRobot' ,   responsiveness: 2 ,   profile:     { footprint: { type: 2 ,       index: 0  },  vicinity:  { type: 2, index: 1 } ,    shape_context: { circles: [ { radius: 0.123},{radius: 0.456} ] }   }   } }"

I suspect there is a bug in add_or_retrieve_participant or in its underlying code.

Edited by @cwrx777 at 2022-09-08T10:19:38Z

15

Posted by @mxgrey:

Please specify the exact commands you used. It is very possible that the inconsistency is because your first attempt failed to perform discovery in time, and a change in DDS quality settings is needed.

16

Posted by @cwrx777:

@mxgrey ,
Updated my above reply with the commands.

17

Posted by @mxgrey:

I just realized what the problem is. I wouldn’t exactly call it a bug, but in rmf_traffic_ros2 there’s a mechanism where the fleet adapter will automatically correct the schedule description of any its participants if it sees that what the schedule has doesn’t match what the fleet adapter wants. This is an intentional design because the fleet adapter is meant to be the authoritative source of truth for participant descriptions. I apologize for my earlier misleading statements; I had forgotten about this automatic correction mechanism.

The only way changing the footprint or vicinity will be possible is if we add an API to do it. I’ll see about making a PR for that soon.

18

Posted by @mxgrey:

If you use these two PRs:

You should be able to update the footprint and vicinity of your robot by calling unstable_change_participant_profile(footprint_radius, vicinity_radius) on the RobotUpdateHandle in your fleet adapter.

I just whipped this up a moment ago and I can’t test it because I’m running other tests at the moment, but if you can give these PRs a try and let me know if they work for you then we can consider merging them.

I’m going to leave the profile changing function in the unstable API for now until we have an opportunity to test it extensively on our end.

19

Posted by @osama-z-salah:

between boxes and circles moving along splines

Hi @mxgrey , does this mean that if I only use boxes, it will work correctly? Is the issue only occurring when combining circles and boxes?

20

Posted by @mxgrey:

Is the issue only occurring when combining circles and boxes?

The investigation happened quite a few years ago at this point so I don’t remember for certain.

But I wouldn’t be surprised if there are subtle issues with box-box collisions as well. Even circle-circle collisions needed fixes, which is why we have a fork of fcl in the rmf_traffic repo.

Given the risks involved, we’re going to stay locked into only circles until we can introduce a new collision library in the next generation.