Feature request

Feature description

In the collision_monitor plugin, the VelocityPolygon checks linear_min / linear_max only against cmd_vel.x, even when holonomic: true.
This makes it impossible for holonomic / omnidirectional robots to define different safety polygons based on lateral speed (motion along ±Y).

This limitation becomes very apparent when attempting to configure:

• Multiple speed-dependent polygons for left/right movement
• Separate safety zones for slow vs fast lateral motion
• Diagonal speed bands

In holonomic mode, the documentation implies that linear_min/max should apply to the resultant velocity of the robot (e.g., magnitude of the translational velocity), but the implementation does not currently do this.

Current Behavior

When calling VelocityPolygon::isInRange, the check is:

bool in_range =
  (cmd_vel_in.x <= linear_max &&
   cmd_vel_in.x >= linear_min &&
   cmd_vel_in.tw <= theta_max &&
   cmd_vel_in.tw >= theta_min);

Even in holonomic mode, the only additional logic is the direction check using:

const double direction = std::atan2(cmd_vel_in.y, cmd_vel_in.x);

This means lateral motion (left/right), where cmd_vel.x ≈ 0, will always pass only polygons whose linear range contains 0 — making it impossible to define speed-dependent lateral safety zones, such as:

• left_slow (0.3–0.5 m/s)

• left_fast (0.5–0.75 m/s)

For backward polygons, users must manually use negative linear_min/max, because the check is still tied to x instead of the magnitude of velocity.

Expected Behavior

For holonomic robots (holonomic: true), it is expected that:

• linear_min / linear_max apply to the resultant speed

double v = std::sqrt(cmd_vel_in.x * cmd_vel_in.x + cmd_vel_in.y * cmd_vel_in.y);

• Direction is determined by the atan2(y, x) check as it is today.

• Backward/sideward/diagonal motions can each have multiple speed bands.

• This enables richer safety behavior for omnidirectional robots such as Mecanum, Swerve, and other holonomic platforms.

Implementation considerations

Proposed Solution

• Move in_rage first check inside the holonomic_ conditional check
• Change the check condition:
  • If holonomic_ = True : calculate √(vx²+vy²)` and check the limits with the resultant speed
  • Else: check only vx as is currently done
• Keep parameters the same
• Explicitly say in the documentation that for holonomic robots, the linear_min and linear _max range should be positive, and backwards movement has to be set with the direction_start_angle and direction_end_angle parameters.
• Non holonomic behavior is kept the same

Current Implementation

bool VelocityPolygon::isInRange(
  const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon)
{
  bool in_range =
    (cmd_vel_in.x <= sub_polygon.linear_max_ && cmd_vel_in.x >= sub_polygon.linear_min_ &&
    cmd_vel_in.tw <= sub_polygon.theta_max_ && cmd_vel_in.tw >= sub_polygon.theta_min_);

  if (holonomic_) {
    // Additionally check if moving direction in angle range(start -> end) for holonomic case
    const double direction = std::atan2(cmd_vel_in.y, cmd_vel_in.x);
    if (sub_polygon.direction_start_angle_ <= sub_polygon.direction_end_angle_) {
      in_range &=
        (direction >= sub_polygon.direction_start_angle_ &&
        direction <= sub_polygon.direction_end_angle_);
    } else {
      in_range &=
        (direction >= sub_polygon.direction_start_angle_ ||
        direction <= sub_polygon.direction_end_angle_);
    }
  }

  return in_range;
}

Proposed Implementation

bool VelocityPolygon::isInRange(
  const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon)
{
  // 1. Always check angular range first
  bool in_range =
    (cmd_vel_in.tw <= sub_polygon.theta_max_ &&
     cmd_vel_in.tw >= sub_polygon.theta_min_);

  if (holonomic_) {
    // 2. For holonomic robots: use speed magnitude + direction
    const double direction = std::atan2(cmd_vel_in.y, cmd_vel_in.x);
    const double magnitude = std::sqrt(cmd_vel_in.x * cmd_vel_in.x +
                               cmd_vel_in.y * cmd_vel_in.y);

    // Linear range on speed magnitude
    in_range &= (magnitude <= sub_polygon.linear_max_ &&
                 magnitude >= sub_polygon.linear_min_);

    // Direction range
    if (sub_polygon.direction_start_angle_ <= sub_polygon.direction_end_angle_) {
      in_range &=
        (direction >= sub_polygon.direction_start_angle_ &&
         direction <= sub_polygon.direction_end_angle_);
    } else {
      in_range &=
        (direction >= sub_polygon.direction_start_angle_ ||
         direction <= sub_polygon.direction_end_angle_);
    }
  } else {
    // 3. Non-holonomic: keep x-based behavior
    in_range &=
      (cmd_vel_in.x <= sub_polygon.linear_max_ &&
       cmd_vel_in.x >= sub_polygon.linear_min_);
  }

  return in_range;
}

Other Posible Implementations

• Add linear limits for the y axis (and removing the direction angles)
• Allways work as holonomic robot, if it is non holonomic, cmd_vel will never have y component, if no direction angle is given asume forward, and add backawards parameter to keep limit range always positive.

Other

• I am still not very experienced, I may be missing something
• Thank you for your time and for contributing