Merging my costmap updates, including changes to costmap creation, changes to the gaussian costmap generation, as well as the addition of DirectionalCostmaps

[LucaKro]

Costmaps

Costmaps are now generated slightly differently:

Before, the "size" and "resolution" parameters both seemed to affect the physical size of the generated costmap, with the "size" basically determining how many poses are generated in side the costmap, and "resolution" determining how big each voxel was. I found this to be very unintuitive and not very useful, as changing the resolution quickly caused the costmap to become unreasonably big or small.

Now, size determines the physical size of the costmap in the map in centimeters, and resolution determines the size of the voxels inside that costmap. Smaller resolution means more poses and smaller voxels, larger resolution means less poses and larger voxels.

Due to this change, the merge function of the costmap class needed to be adjusted, but this also allowed us to remove 1 constraint:
before we were constrained as follows:

1. The Costmaps need to have the same size
2. The Costmaps need to have the same x and y coordinates in the origin
3. The Costmaps need to have the same resolution

In the new version, we were able to remove the first constraint, meaning now the costmaps do not need to be the same size, which allows for more complex costmap generation.

---

LocationCostmaps

GaussianCostmap

Two new parameter were added to the "GaussianCostmap" class:
The new "circular" parameter allows us to generate a circle instead of a square costmap if we wish to do so, as testing seemed to indicate that the corners of the square usually were not used anyways, and this allows us to remove a few potential failureposes already before calling the IK solver.

Additionally, the gaussian costmap was updated to now also receive a “distance” parameter, that allows us to specify the distance of the peak of the gaussian to the center of the costmap, which essentially creates a ring around the object pose, where the most prioritized poses are “distance” away from the object. A distance of 0 results in a costmap like it was generated prior to this update.

DirectionalCostmap

Adds a new “DirectionalCostmap”, which points in the direction of the approach pose. The direction of the costmap is determined by the grasp used to pick up the object, (e.g., front, back, left, right), as well as the placing pose.

Weights

Weights are now used appropriately (as seen in the above image, some poses have a higher z coordinate than others. that z value represents the weight of that pose, normalized to a value between 0 and 0.4 (can be changed via parameter)). These weights also have an impact on the pose generation, as the sampling process of the poses is now randomized, with poses with a higher weight having a higher probability to be sampled.

New Operator

In addition to that, a new operator __mul__ was added (used current_costmap *= priority_area_costmap) which allows us to prioritize a certain area of the current costmap, without completely removing poses that are not prioritized.

---

Costmap Publisher

We can now publish the costmap as a visual marker in rviz by calling the publish() function of any given costmap.

Generation of the Costmap

1. Generate a occupancy costmap around the target pose
   

2. Generate a circular gaussian costmap around the target pose, with a radius of the estimated maximum reach of the robots arm
   

3. Merge both maps to generate the final costmap, which has all poses in close proximity to the object, while also avoiding potential obstacles
   

4. If we are trying to place an object, we may also generate a directional costmap
   
   And then prioritize the poses of our costmap from step 3 that overlap with the poses of the directional costmap
   

[tomsch420]

For this PR we should sit together because costmaps should be probabilistic models that support full inference.

[tomsch420]

stick to rst doc style

[tomsch420]

style