-
Notifications
You must be signed in to change notification settings - Fork 25
Add timer-based propagation functionality to AMCL #492
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
base: main
Are you sure you want to change the base?
Conversation
Signed-off-by: fbattocchia <[email protected]>
025df5c to
8479fd0
Compare
Signed-off-by: fbattocchia <[email protected]>
glpuga
left a comment
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Great work @fbattocchia ! left a few comments.
beluga_amcl/src/amcl_node.cpp
Outdated
| // Force a propagation-only update (without sensor data) | ||
| particle_filter_->update_propagation(base_pose_in_odom.value()); | ||
|
|
||
| RCLCPP_INFO(get_logger(), "Forced propagation update executed"); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Maybe this log will be too noisy, logging this message at high rate?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
+1. Consider demoting it to DEBUG.
| // tf thread. The message filter we are using avoids the need for it. | ||
| tf2::convert( | ||
| tf_buffer_ | ||
| ->lookupTransform( | ||
| get_parameter("odom_frame_id").as_string(), get_parameter("base_frame_id").as_string(), | ||
| tf2_ros::fromMsg(laser_scan->header.stamp)) | ||
| get_parameter("odom_frame_id").as_string(), get_parameter("base_frame_id").as_string(), time) | ||
| .transform, | ||
| base_pose_in_odom); | ||
| return base_pose_in_odom; | ||
| } catch (const tf2::TransformException& error) { | ||
| RCLCPP_ERROR(get_logger(), "Could not transform from odom to base: %s", error.what()); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Good call wrapping this in a function.
beluga_amcl/src/amcl_node.cpp
Outdated
| } | ||
|
|
||
| // Get base pose in odom frame at laser scan timestamp | ||
| auto base_pose_in_odom = get_base_pose_in_odom(tf2_ros::fromMsg(laser_scan->header.stamp)); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I wonder if it can happen that we get a lidar message with a timestamp that is previous to a get_base_pose_in_odom() update.
Say, we execute get_base_pose_in_odom() at 10, 20, 30, 40, 50 and then we get a lidar message with timestamp 45. We might be adding a little bit more noise then.
Maybe when we update the motion model from a separate thread then we should either
- Only update if the time stamp is later (no noise added then). If we don't, particle state might be up to one timer event behind sensor data.
- Repeat the value of the latest get_base_pose_in_odom() odom value we updated (so that no extra noise is ever added. We will always be up to one timer event behind sensor timestamp.
- Always update. If we get "late" lidar messages, we might "roll back" motion from the particle state in between timer activations and in the end, end up adding more noise.
I tend to like more 1 or 2. 3 might add a lot more noise if the late arrivers are recurrent.
Say we update motion at 40hz, get lidar messages at 10hz, and lidar messages always end up arriving after the timer for a later timestamp already updated the filter. 1 in four message will add false motion and increase noise.
Thoughts @fbattocchia @hidmic ?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Say, we execute get_base_pose_in_odom() at 10, 20, 30, 40, 50 and then we get a lidar message with timestamp 45. We might be adding a little bit more noise then.
I mean, we would be "rolling back" motion from 50 to 45, then forwarding again from 45 to 60 in the next timer event, thus adding extra "motion" and thus noise (proportional to motion).
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I wonder if it can happen that we get a lidar message with a timestamp that is previous to a get_base_pose_in_odom() update.
That's a very good point, and yes, this can absolutely happen. It's an out of sequence measurement (OOSM) case. A principled, general solution is probably out of scope for your purposes though (e.g. https://ieeexplore.ieee.org/document/8455401, but there is quite a bit of research on the topic).
I tend to like more 1 or 2. 3 might add a lot more noise if the late arrivers are recurrent.
🤔 What if we don't touch the filter itself? If we let it evolve at lidar rates, we can still propagate the latest distribution up to current time on timer callback. Our control deltas wouldn't be any larger than they are at lidar rates. You could even do that deterministically on the latest estimate by applying an unscented transform with the motion nonlinear function (which we don't have, we just have a sampler, but it could be added) and spare the cost of sampled propagation.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
What if we don't touch the filter itself? If we let it evolve at lidar rates...
I'm not sure I follow. Can you provide an example of what it would look like?
Another option is to do the the updates lazily:
We don't update the filter in the timer callback, we just take note of the latest odometry motion at that time and queue it for later. When the lidar message arrives, we process the queue all the way up to the last timestamp that is before the lidar timestamp and leave any remaining one in the queue for later.
We transfer the work of updating the filter to the lidar callback and force an ordered update sequence.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Ahh, I see now. I thought you wanted estimates at a rate higher than the lidar rate, but this is about processing odometry at a higher rate than lidar rate. If it were the former, what I meant is that you can always predict forward from the latest estimate (which is almost the same as relying on odometry given a fixed map to odom transform but for the explicit uncertainty modelling). But I sense it's the latter, so +100 to lazy updates. Maybe we can collect all odometry updates in a bigger control window.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think it's a good idea to implement lazily updates.
beluga_ros/src/amcl.cpp
Outdated
| std::visit( | ||
| [&, this](auto& policy, auto& motion_model) { | ||
| particles_ |= beluga::actions::propagate(policy, motion_model(control_action_window_ << base_pose_in_odom)) | | ||
| beluga::actions::normalize(policy); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
We don't need to normalize, since we only affected distribution density, but we left the weights unchanged.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
+1
beluga_ros/test/test_amcl.cpp
Outdated
| TEST(TestAmcl, UpdatePropagationWithNoParticles) { | ||
| auto amcl = make_amcl(); | ||
| amcl.update_propagation(Sophus::SE2d{}); | ||
| ASSERT_EQ(amcl.particles().size(), 0); | ||
| } | ||
|
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I get that this manages to make the coverage checker think that the function was tested, but it does not look like its functionally testing anything. Let's do a propagation step with a few particles in the set.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Yes, I did the test to achieve coverage, I agree with it doesn't make much sense. What I need to test is that it doesn't do anything because there are no particles. I changed the function's logic so I wouldn't have to do that check. (Another option would be to return a bool and verify the return in the test.)
beluga_amcl/src/amcl_node.cpp
Outdated
| descriptor.floating_point_range[0].from_value = 0.0; | ||
| descriptor.floating_point_range[0].to_value = 100.0; | ||
| descriptor.floating_point_range[0].step = 0.0; | ||
| declare_parameter("increase_propagation", rclcpp::ParameterValue(0.0), descriptor); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@fbattocchia nit:
| declare_parameter("increase_propagation", rclcpp::ParameterValue(0.0), descriptor); | |
| declare_parameter("propagation_rate", rclcpp::ParameterValue(0.0), descriptor); |
perhaps?
beluga_amcl/src/amcl_node.cpp
Outdated
| // Force a propagation-only update (without sensor data) | ||
| particle_filter_->update_propagation(base_pose_in_odom.value()); | ||
|
|
||
| RCLCPP_INFO(get_logger(), "Forced propagation update executed"); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
+1. Consider demoting it to DEBUG.
beluga_amcl/src/amcl_node.cpp
Outdated
| } | ||
|
|
||
| // Get current base pose in odom frame (latest available) | ||
| auto base_pose_in_odom = get_base_pose_in_odom(tf2::TimePointZero); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@fbattocchia nit: can be const
| * | ||
| * \param base_pose_in_odom Base pose in the odometry frame. | ||
| */ | ||
| void update_propagation(Sophus::SE2d base_pose_in_odom); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@fbattocchia consider making this just another update overload.
| void update_propagation(Sophus::SE2d base_pose_in_odom); | |
| void update(const Sophus::SE2d& base_pose_in_odom); |
It is updating after all, it just uses less data.
beluga_ros/src/amcl.cpp
Outdated
| std::visit( | ||
| [&, this](auto& policy, auto& motion_model) { | ||
| particles_ |= beluga::actions::propagate(policy, motion_model(control_action_window_ << base_pose_in_odom)) | | ||
| beluga::actions::normalize(policy); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
+1
beluga_amcl/src/amcl_node.cpp
Outdated
| } | ||
|
|
||
| // Get base pose in odom frame at laser scan timestamp | ||
| auto base_pose_in_odom = get_base_pose_in_odom(tf2_ros::fromMsg(laser_scan->header.stamp)); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I wonder if it can happen that we get a lidar message with a timestamp that is previous to a get_base_pose_in_odom() update.
That's a very good point, and yes, this can absolutely happen. It's an out of sequence measurement (OOSM) case. A principled, general solution is probably out of scope for your purposes though (e.g. https://ieeexplore.ieee.org/document/8455401, but there is quite a bit of research on the topic).
I tend to like more 1 or 2. 3 might add a lot more noise if the late arrivers are recurrent.
🤔 What if we don't touch the filter itself? If we let it evolve at lidar rates, we can still propagate the latest distribution up to current time on timer callback. Our control deltas wouldn't be any larger than they are at lidar rates. You could even do that deterministically on the latest estimate by applying an unscented transform with the motion nonlinear function (which we don't have, we just have a sampler, but it could be added) and spare the cost of sampled propagation.
f4e4640 to
1571ebe
Compare
Signed-off-by: fbattocchia <[email protected]>
Signed-off-by: fbattocchia <[email protected]>
beluga_amcl/src/amcl_node.cpp
Outdated
| while (!odometry_motion_buffer_.empty()) { | ||
| const auto& [odom_time, odom_pose] = odometry_motion_buffer_.front(); | ||
| if (odom_time >= laser_scan_stamp) { | ||
| break; | ||
| } | ||
| particle_filter_->update(odom_pose); | ||
| odometry_motion_buffer_.pop_front(); | ||
| } |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@fbattocchia meta: it's a bit of a bummer that the feature exists outside core Beluga but this is simple enough. How would you go about adding (some) support for this at the motion model level if we had to?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
yes, I could define an interface in the Beluga motion model to accept a sequence of movements (for example, apply_odometry_sequence or update_odometry_buffer_until) and process them all together
beluga_amcl/src/amcl_node.cpp
Outdated
| return; | ||
| } | ||
|
|
||
| const auto now = this->now(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@fbattocchia now() will likely be different than the latest transform time. Consider using the latter.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
On a more general note, it's starting to look like time is a relevant quantity we don't often propagate and we probably should.
c066d6a to
4f7b10c
Compare
Signed-off-by: fbattocchia <[email protected]>
glpuga
left a comment
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Great work! a few comments.
beluga_amcl/src/amcl_node.cpp
Outdated
|
|
||
| { | ||
| auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); | ||
| descriptor.description = "Set true to enable increased propagation, and set false to disable it."; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
| descriptor.description = "Set true to enable increased propagation, and set false to disable it."; | |
| descriptor.description = "Set true to enable odometry-driven filter propagation."; |
beluga_amcl/src/amcl_node.cpp
Outdated
| { | ||
| auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); | ||
| descriptor.description = "Set true to enable increased propagation, and set false to disable it."; | ||
| declare_parameter("propagation_rate", rclcpp::ParameterValue(false), descriptor); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
We need to change the name of the parameter.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
maybe use_odometry_propagation?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Better odometry_driven_propagation
beluga_amcl/test/test_amcl_node.cpp
Outdated
| EXPECT_EQ(amcl_node_->has_odom_sub(), true); | ||
| } | ||
|
|
||
| TEST_F(TestNode, PropagationRate) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Related t changing the parameter name.
| // Check that odometry_motion_buffer_ was consumed up to the lidar timestamp | ||
| EXPECT_EQ(amcl_node_->odometry_motion_buffer().size(), 0); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Add two more odometry messages: one with a timestamp equal than the one in the lidar, and one later, and test that we consume all but the one with the later timestamp.
Receiving lidar and odometry messages with the same timestamp is a frequent thing in simulators.
| }; | ||
|
|
||
| /// Type Buffer for queued odometry motions (timestamp, pose) | ||
| using OdometryMotion = std::pair<tf2::TimePoint, Sophus::SE2d>; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
nit: while pairs and tuples are great for function-local anonymous data types, it might be better to just use a struct for this.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Nevermind, if we move this into a private type in beluga_amcl, then a tuple is ok.
beluga_ros/src/amcl.cpp
Outdated
| void Amcl::process_buffered_odometry_until(std::deque<OdometryMotion>& buffer, const tf2::TimePoint& until) { | ||
| while (!buffer.empty()) { | ||
| const auto& [odom_time, odom_pose] = buffer.front(); | ||
| if (odom_time >= until) | ||
| break; | ||
| update(odom_pose); | ||
| buffer.pop_front(); | ||
| } | ||
| } |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
it sounds to me like this belongs in beluga_amcl.cpp, justl ike the OdometryMotion datatype, since implementing the odometry message queue is that file's concern.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
My only comment here, and it is as meta as #492 (comment) was, is that processing a longer window of odometry measurements is something that could be pushed to the core motion model. This is like conditioning the proposal distribution on a control sequence that is longer than two elements.
Really, it is a non-issue here. Keep it simple. It can be explored in follow-up work.
beluga_ros/src/amcl.cpp
Outdated
| void Amcl::process_buffered_odometry_until(std::deque<OdometryMotion>& buffer, const tf2::TimePoint& until) { | ||
| while (!buffer.empty()) { | ||
| const auto& [odom_time, odom_pose] = buffer.front(); | ||
| if (odom_time >= until) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I would still process it if it's ==
| particle_filter_.reset(); | ||
| likelihood_field_pub_.reset(); // attaching likelihood_field_pub_ lifespan to particle_filter_ lifespan | ||
| enable_tf_broadcast_ = false; | ||
| odometry_motion_buffer_.clear(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
We should also clear this whenever the particle filter is initialized/created.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I'm clearing the buffer in do_activate() where the odometry subscription is performed
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Yes, but that only happens when the node starts. The filter might be reinitialized multiple times afterwards, whenever the map is updated or when the user sends a pose intialization message. Look for calls to initialize_from_estimate() in amcl_node.cpp
af0a591 to
ce8b3f4
Compare
Signed-off-by: fbattocchia <[email protected]>
Signed-off-by: fbattocchia <[email protected]>
hidmic
left a comment
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Great work @fbattocchia! Left a few minor comments here and there.
I defer the approval to @glpuga.
beluga_amcl/README.md
Outdated
| |------------------|-------------------------------------------|-----------------------------------------------------------------------------| | ||
| | `map` | `nav_msgs/OccupancyGrid` | Input topic for map updates. | | ||
| | `scan` | `sensor_msgs/LaserScan` | Input topic for laser scan updates. | | ||
| | `odom` | `nav_msgs/Odometry` | Input topic for odometry updates (when use_odometry_propagation is enabled). | |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@fbattocchia nit:
| | `odom` | `nav_msgs/Odometry` | Input topic for odometry updates (when use_odometry_propagation is enabled). | | |
| | `odom` | `nav_msgs/Odometry` | Input topic for odometry updates (when use_odometry_propagation is enabled). | |
and align the other rows.
|
|
||
| #include <beluga/beluga.hpp> | ||
| #include <beluga_ros/amcl.hpp> | ||
| #include <deque> |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@fbattocchia nit: this one goes up with the other C++ standard headers. Wonder why the linter didn't complain 🤔
beluga_amcl/src/amcl_node.cpp
Outdated
|
|
||
| { | ||
| auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); | ||
| descriptor.description = "Set true to enable odometry-driven filter propagation."; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@fbattocchia nit:
| descriptor.description = "Set true to enable odometry-driven filter propagation."; | |
| descriptor.description = "Enable odometry-driven filter propagation."; |
beluga_amcl/src/amcl_node.cpp
Outdated
| odometry_motion_buffer_.emplace_back(time, base_pose_in_odom); | ||
| } | ||
|
|
||
| void AmclNode::process_buffered_odometry_until(std::deque<OdometryMotion>& buffer, const tf2::TimePoint& until) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@fbattocchia nit^2: do we need to pass the buffer explicitly? The call site would read better if we didn't.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@hidmic The parameter remained from when I implemented this function in amcl_ros and I forgot to remove it. Thank you for pointing that out.
beluga_amcl/test/test_amcl_node.cpp
Outdated
| } | ||
|
|
||
| TEST_F(TestNode, OdomSubNotCreatedWhenPropagationDisabled) { | ||
| amcl_node_->set_parameter(rclcpp::Parameter{"use_odometry_propagation", false}); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@fbattocchia nit: shall we check that the default value (when unset) is false too?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@hidmic No, I don't verify that it's the default value, but if I delete this line, I can verify two things: that the default value is false and that the odometry subscription is not created if it is false.
beluga_amcl/test/test_amcl_node.cpp
Outdated
| EXPECT_EQ(amcl_node_->odometry_motion_buffer().size(), 0); | ||
| } | ||
|
|
||
| TEST_F(TestNode, OdometryBufferConsumedWhenOdomTimestampsMajorThanLidar) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@fbattocchia nit:
| TEST_F(TestNode, OdometryBufferConsumedWhenOdomTimestampsMajorThanLidar) { | |
| TEST_F(TestNode, OdometryBufferConsumedWhenOdomTimestampsGreaterThanLidar) { |
glpuga
left a comment
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Great job! Final comments.
| /// Odometry updates subscription. | ||
| rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_; | ||
|
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
nit, but for you to know:
In this node it does not really matter because this is a lifecycle node and thus subscribers get destroyed before the wrapping class does, but its a good habit to put anything that has a callback int the very last position in the class members declarations.
The reason is so that whenever the class gets destroyed, those are the first to be destroyed, and that includes rendering them unable to execute their callbacks.
If they are not last, some datastructure their callbacks depend on might be destroyed first concurrently with something triggering the callback, and that will usually cause a crash.
This is a frequent cause of flaky tests and nodes that randomly seem to crash when being stopped.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
it's good to know
beluga_amcl/src/amcl_node.cpp
Outdated
| get_parameter("odom_topic").as_string(), rclcpp::SensorDataQoS(), | ||
| std::bind(&AmclNode::odometry_callback, this, std::placeholders::_1), common_subscription_options_); | ||
|
|
||
| RCLCPP_INFO(get_logger(), "Subscribed to odom_topic: %s", odom_sub_->get_topic_name()); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
| RCLCPP_INFO(get_logger(), "Subscribed to odom_topic: %s", odom_sub_->get_topic_name()); | |
| RCLCPP_INFO(get_logger(), "Subscribed to odometry topic: %s", odom_sub_->get_topic_name()); |
beluga_amcl/src/amcl_node.cpp
Outdated
| if (get_parameter("use_odometry_propagation").as_bool()) { | ||
| const auto laser_scan_stamp = tf2_ros::fromMsg(laser_scan->header.stamp); | ||
| process_buffered_odometry_until(odometry_motion_buffer_, laser_scan_stamp); | ||
| } |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
nit, but if odometry driven prop is off, the queue will be empty anyway.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@glpuga Removing the if statement would make the code more compact, but in my opinion, less explicit. Keeping it helps clarify that this action is only performed when use_odometry_propagation is enabled, which can be beneficial for someone reading or maintaining the code. But replacing the if with a comment can serve the same purpose, so I'll do that.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Ok, I'm ok with it staying.
Signed-off-by: fbattocchia <[email protected]>
glpuga
left a comment
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Great job @fbattocchia, LGTM. However, don't merge yet!
@hidmic I'd like to have a devel or experimental branch in the repository to buffer changes. I'd rather no longer hit main with changes we have not measured to ensure they have an objective reason to exist and to ensure they don't represent a regression by some other measure.
We should only graduate stuff from devel to main after evaluation.
🤔 I'm on the fence. We don't provide stability guarantees on |
Proposed changes
This feature allows for increased propagation rates independent of sensor updates.
This improvement allows the implementation of the speed-based motion model
Type of change
Checklist