Points Testing 2

A program building off of the concept from my other repo points_testing. Implements the Ramer Douglas Peucker algorithm to downsample a set of Poses (containing Position and Orientation) in order to simplify a path bound by two different constraints. These constriants are the maximum perpendicular distance that the original points can vary from the corrected points, and the maximum angle about any axis that the original orientations can vary from the corrected orientations.
Currently reads the points from a file, but in the pose_array_input branch it can take in a geometry_msgs/PoseArray

Dependecies

Code is written to work for ROS 2 Foxy/Humble
Imports:

• NumPy
• rclpy
• ROS standard message library

Program Outline

This code is set up as a ROS 2 Service.
This service has 3 requests:

• input_file (string) - The PoseArray to be corrected
• epsilon (float64) - (meters) The maximum amount that the perpendicular distance between the original points and the corrected points can vary
• angle_threshold (float64) - (degrees) The maximum amount about any axis that the original orientations can vary from the corrected orientations

The service has 1 response:

• corrected_poses (geometry_msgs/PoseArray) - The downsample PoseArray

When the service is called, the code follows this logic to downsample the poses:

1. The code first finds the maximum perpendicular distance that the curve is from a line drawn between the start and end points, and then the maximum rotation about an axis for that range
2. The code then checks if the maximum distance is greater than the epsilon value, if it is, the list of poses is split at that index, and steps 1-2 will be repeated until a pose is found with a maximum distance of less than epsilon
3. Once a segment has been determined as valid for distance from the original points, the segment is then evaluated for how much the orientation varies from the original set of poses. If a point is found where the rotation relative to each axis is greater than the threshold, the pose list will be split at that point's index, and steps 1-3 will be repeated until an orientation is found that is less than the threshold
4. Once all positions are checked and valid, the code will move on to the next segment until the end of the list is reached and the result array is outputted

Here is an example of a raster dataset that is downsampled. The orientations of the original raster were set to vary up to 40 degrees Original PoseArray: 2995 poses, Corrected PoseArray: 8 poses
Time to complete: ~1 second
Epsilon: 0.05m
Angle Threshold: 35 degrees

Console Output:

Note: The first section of orientations shows the difference between the correction poses, and the second set with the Delta values shows the maximum orientation from the original dataset between the two poses

Changes from original repo

points_testing was originally based in ROS and was called using launch files, the new code is based in ROS 2 and is done through a service to provide better interaction with other parts of the project.
The original code could only handle downsampling positions and had no ability to downsample orientations. With the update to the code to take in Poses, the code can now fully downsample a PoseArray