Zone-based behavior tree control using colored map regions

[brayanpa]

Feature request

Feature description

Hi! I've been thinking about implementing a new functionality that allows drawing zones on the map — something similar to the speed filter. The idea is to define zones by color, for example:

You draw a colored image (I was thinking of using .png format), and create the corresponding YAML file:

zones:
  elevs_zone_2: [255, 255, 0]
  elev_2_1: [0, 0, 255]
  elev_2_2: [0, 0, 240]
  elev_2_3: [0, 0, 230]
  
  elev_3_1: [0, 0, 220]
  elev_3_2: [0, 0, 210]
  elev_3_3: [0, 0, 200]
  
  door1: [0, 255, 0]
  door2: [0, 240, 0]
  door3: [0, 230, 0]
  door4: [0, 220, 0]
  
  delivery_zone_1: [190, 0, 255]
  delivery_zone_2: [180, 0, 240]

Each zone corresponds to a specific RGB color. By reading the color of the pixel that matches the position of base_link, it becomes easy to determine whether the robot is located within a particular zone. This approach could enable services like is_robot_in_zone("ZONE_NAME"), which can be used in a behavior tree to trigger specific actions depending on the robot's current zone. For example, if navigation fails in the "door" zone, the system could fall back to a behavior that requests the door to open. Similarly, if the robot enters the "elevator" zone, it could trigger a service to call the elevator.

I've even thought about implementing a behavior tree node to navigate to a zone.

These are just some ideas I have — I’d really like to hear more points of view. Do you think this would be a useful feature? How would you suggest implementing it? I’m open to all suggestions.

[SteveMacenski]

That's another way that data could be represented, yes! Though if you had less than 255 unique types of things, you could just use the grayscale values similar to what we do for the keepout/speed and other costmap filters.

The costmap filters can be used to do things like what you suggest: you have the robot's pose and use that to match the zone to the robot pose in order to trigger some action. We do that in our current examples with keepout zones and keep zones. There's been incomplete proposals for others such as binary zones (i.e. trigger an arbitrary event by value). A behavior tree node could listen for those messages to enact something, for sure 😄

I'm not entirely sure what your proposal is. It sounds like you have an implementation idea (using RGB to represent zones), but not the use-case for it. Are you suggesting we move the Speed / Keepout / Costmap Filters to use RGB rather than grayscale image types so that they can represent more numerous types of things and/or information? Or expose this information in the behavior tree separate of the costmap filters / costmap2D engine?

Or perhaps you weren't aware that you could use the costmap filters today for this reason (or you need more than 255 unique values)?

[brayanpa]

Thanks for the clarification!

My idea of using RGB instead of grayscale was mainly for user-friendliness — it’s easier to visually distinguish different zones when looking at the image. But in terms of functionality, grayscale works perfectly for my use case, since I’m far from reaching the 255-zone limit.

You're right. I wasn’t aware that the current costmap filters could already be used to trigger specific actions in the behavior tree. What I had in mind was essentially:

• Being able to know what zone the robot is currently in (via a name or ID),
• Using that information as a condition or trigger inside a behavior tree.

I’ll dig into the current costmap filter functionality and see how I can leverage it for this.

Thanks again!

[SteveMacenski]

Great! I think it should be easy enough to have the cost value from the costmap filter remap into a topic name for a publication of a message. I.e. when color / cost 10 is found, then it triggers a service my_10_service or topic that is mapped to it as a parameter. It would just need params that indicate what cost triggers what service/topic name, which should be easy enough to code up in a loop for all possible values from 0-255 (default to no action).

That would be a great "BehaviorTrigger" costmap filter contribution to the stack if you were interested in contributing that!