Units for poses used in SMAC planner code

edit

Hello,

While studying the SMAC planner, I got confused by the various units/coords used to describe a "pose".*

So far I identified three frames in which coordinates of a pose are expressed:

• costmap's coordinates (x and y in costmap's pixels, theta in angle bin index (not radians))
• lattice grid space coordinates (x and y express in meters ? and theta in rad ?)
• world coordinates (regular meters and radian)

But I'm confused with this piece of code: https://github.com/ros-planning/navig...

    // Convert primitive pose into grid space if it should be checked
    prim_pose._x = initial_pose._x + (it->_x / grid_resolution);
    prim_pose._y = initial_pose._y + (it->_y / grid_resolution);
    // If reversing, invert the angle because the robot is backing into the primitive
    // not driving forward with it
    if (is_backwards) {
      prim_pose._theta = std::fmod(it->_theta + M_PI, 2.0 * M_PI);
    } else {
      prim_pose._theta = it->_theta;
    }
    if (collision_checker->inCollision(
        prim_pose._x,
        prim_pose._y,
        prim_pose._theta / bin_size /*bin in collision checker*/,
        traverse_unknown))
    {
      return false;
    }

Then:

• If my lattice grid_resolution = 0.02m as specified in my JSON motion primitive file.
• If my costmap's resolution is 0.1m.

Then inCollision() is here called using the wrong units ? (prim_pose being lattice grid coords, but inCollision() expecting costmap's coords ?)

Am I missing something ?

Thanks,