Set of geometry_msgs::Point

edit

Firstly a set of 3D points is a point_cloud, is there a reason you're not using the existing point cloud messages for this? They would have the advantage of working with the visualizers in RVIZ as well as compatibility with a lot of existing nodes.

Secondly if you define a custom message with an member geometry_msgs::Point[] my_points then in C++ this will already be implemented as a std::vector<geometry_msgs::Point> so you don't need to cast anything. However the std::vector type will not help you remove duplicate points.

You'll need an algorithm to detect and remove duplicate points, how you do this will depend on how many points you're working with. If it's a few hundred then you can probably get away with simply comparing each new point to the set of existing unique points. However this is O(n^2) time complexity so will get very slow for larger point clouds. If you're working with large point clouds (anything over a thousand points) I'd recommend using PCL and producing a KD tree, the KD tree allows you to search for duplicate points significantly faster and will make a big difference for larger clouds. PCL will also give you functions to convert the result to a PointCloud2 ros message into the bargain.

Hope this gives you some ideas.

I'd recommend either

• Use PoseArray mgs and you can use a for loop to go through the position of pose of the PoseArray to see if poseArray.pose.position of x and y is equal to the x and y of the one you are gonna add in if so then move on if not then add.

OR

• Create your own msgs with a Point[] point array and go through that array.