114 task landmark server v2

[jorgenfj]

Pull request for the landmark server. Should review these two before accepting this: https://github.com/vortexntnu/vortex-msgs/pull/5 https://github.com/vortexntnu/vortex-msgs/pull/4

For the pose publisher used for visualization purposes I am unsure of what frame to use for the poses and if a transform is necessary.

Here are some dummy landmarks used for testing: ros2 topic pub -1 /landmarks_in vortex_msgs/msg/LandmarkArray '{ landmarks: [ { header: {stamp: {sec: 0, nanosec: 0}, frame_id: "map"}, landmark_type: "boat", id: 1, action: 1, type: 1, odom: { pose: { pose: { position: {x: 1.0, y: 1.0, z: 0.0}, orientation: {x: 0.1, y: 0.0, z: 0.2, w: 1.0} }, covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] }, twist: { twist: { linear: {x: 0.0, y: 0.5, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0} }, covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] }, child_frame_id: "boat" }, shape: { type: 1, dimensions: [1.0, 2.0, 3.0] } }, { header: {stamp: {sec: 0, nanosec: 0}, frame_id: "map"}, landmark_type: "buoy", id: 3, action: 1, type: 0, odom: { pose: { pose: { position: {x: 3.0, y: 0.0, z: 0.0}, orientation: {x: 0.0, y: 0.2, z: 0.0, w: 1.0} }, covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] }, twist: { twist: { linear: {x: 1.0, y: 2.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0} }, covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] }, child_frame_id: "buoy" }, shape: { type: 0, dimensions: [2.0] } } ] }'

ros2 action send_goal /landmark_filter vortex_msgs/action/FilteredLandmarks '{landmark_types: ["boat"], distance: 0.0, frame_id: "your_frame_id"}' --feedback