Add amcl3 demo #9
Conversation
Signed-off-by: Pablo Vela <[email protected]>
Signed-off-by: Pablo Vela <[email protected]>
Signed-off-by: Pablo Vela <[email protected]>
Signed-off-by: Pablo Vela <[email protected]>
Signed-off-by: Pablo Vela <[email protected]>
Signed-off-by: Pablo Vela <[email protected]>
| repositories: | ||
| beluga: | ||
| type: git | ||
| url: https://github.com/pvela2017/beluga.git |
There was a problem hiding this comment.
@pvela2017 this needs to change before merging.
docker/images/jazzy/Dockerfile
Outdated
| libopenvdb-dev \ | ||
| python3-pip \ | ||
| mc \ | ||
| ros-jazzy-pcl-ros \ |
There was a problem hiding this comment.
@pvela2017 why adding this here instead of in the corresponding package.xml?
| ] | ||
|
|
||
| return LaunchDescription(nodes) | ||
|
|
There was a problem hiding this comment.
@pvela2017 nit: missing EOF newline, here and elsewhere.
| @@ -0,0 +1,120 @@ | |||
| #!/usr/bin/env python3 | |||
|
|
|||
There was a problem hiding this comment.
@pvela2017 hmm, why do we need to publish markers? Why not just use RViz displays for poses?
There was a problem hiding this comment.
I like the line type visualization
| @@ -0,0 +1,63 @@ | |||
| #!/usr/bin/env python3 | |||
|
|
|||
There was a problem hiding this comment.
@pvela2017 meta: I wonder if it wouldn't be simpler to modify the dataset (which we have already modified) to include odom to base_link transforms.
|
|
||
| // Initial position | ||
| const auto pose = Sophus::SE3d{ | ||
| Sophus::SO3d{/*kInitialPoseYaw, kInitialPosePitch, kInitialPoseRoll*/}, |
There was a problem hiding this comment.
@pvela2017 nit: remove commented out code.
| point_color.b = 1.0; | ||
| point_color.a = 1.0; | ||
|
|
||
| for (openvdb::FloatGrid::ValueOnCIter iter = grid->cbeginValueOn(); iter; ++iter) { |
There was a problem hiding this comment.
@pvela2017 the condition expression is unusual, is there no cendValueOn()?
There was a problem hiding this comment.
As discussed in the meeting it is ok.
OpenVDB iterators are not STL-compatible in that one can always request an iterator that points to the beginning of a collection of elements (nodes, voxels, etc.), but one usually cannot request an iterator that points to the end of the collection. (This is because finding the end might require a full tree traversal.)
| visualization_msgs::msg::Marker marker_msg; | ||
| std_msgs::msg::ColorRGBA point_color; | ||
| openvdb::CoordBBox bbox = grid->evalActiveVoxelBoundingBox(); | ||
| double min_z, max_z; |
There was a problem hiding this comment.
@pvela2017 nit: better define and assign on the same line, then it can be const
| marker_msg.colors.push_back(point_color); | ||
| } | ||
|
|
||
| double size = grid->transform().voxelSize()[0]; |
There was a problem hiding this comment.
@pvela2017 nit: can be const
| "localization/beluga_demo_bearing_localization" | ||
| "localization/beluga_demo_fiducial_localization" | ||
| "localization/beluga_demo_lidar_localization" | ||
| ) |
There was a problem hiding this comment.
@pvela2017 I'm a bit ambivalent about this, but I can't think of a better solution. CC @glpuga for thoughts.
Signed-off-by: Pablo Vela <[email protected]>
Signed-off-by: Pablo Vela <[email protected]>
Signed-off-by: Pablo Vela <[email protected]>
Signed-off-by: Pablo Vela <[email protected]>
Signed-off-by: Pablo Vela <[email protected]>
Signed-off-by: Pablo Vela <[email protected]>
Signed-off-by: Pablo Vela <[email protected]>
Proposed changes
Added a demo for the AMCL3 algorithm based on the likelihood sensor model 3D (Ekumen-OS/beluga#440)
Type of change
Checklist
Put an
xin the boxes that apply. This is simply a reminder of what we will require before merging your code.Additional comments