Account for measurement delay in localization #650
Conversation
| @@ -146,7 +146,7 @@ body_behavior: | |||
| dribble_ball_distance_threshold: 0.5 | |||
| dribble_kick_angle: 0.6 | |||
|
|
|||
| kick_decision_smoothing: 5.0 | |||
| kick_decision_smoothing: 1.0 | |||
There was a problem hiding this comment.
While testing I noticed that this smoothing is not really needed anymore btw.
Sry for being unrelated here.
There was a problem hiding this comment.
so remove this parameter alltogether later?
| @@ -25,6 +25,9 @@ def perform(self, reevaluate=False): | |||
| ball_position = self.blackboard.world_model.get_ball_position_uv() | |||
|
|
|||
| self.publish_debug_data("ball_position", {"u": ball_position[0], "v": ball_position[1]}) | |||
| self.publish_debug_data( | |||
| "smoothing_close", f"{self.no_near_decisions} ({self.no_near_decisions / self.smoothing * 10}%)" | |||
There was a problem hiding this comment.
It smooths the ball close decision.
| @@ -146,7 +146,7 @@ body_behavior: | |||
| dribble_ball_distance_threshold: 0.5 | |||
| dribble_kick_angle: 0.6 | |||
|
|
|||
| kick_decision_smoothing: 5.0 | |||
| kick_decision_smoothing: 1.0 | |||
There was a problem hiding this comment.
so remove this parameter alltogether later?
|
|
||
| #include <bitbots_localization/RobotState.hpp> | ||
| #include <bitbots_localization/map.hpp> | ||
| #include <bitbots_localization/tools.hpp> | ||
| #include <sensor_msgs/msg/point_cloud2.hpp> | ||
| #include <sensor_msgs/point_cloud2_iterator.hpp> | ||
| #include <soccer_vision_3d_msgs/msg/field_boundary.hpp> | ||
| #include <soccer_vision_3d_msgs/msg/goalpost.hpp> |
There was a problem hiding this comment.
You removed the field boundary because it is not used anymore. As far as I know, goal posts arent used either, do you want to remove them too (also in the localization.cpp and so on).
There was a problem hiding this comment.
The field boundary performs quite poorly in the localization and we might include the goalpost with a higher chance in the near future (although not planned).
| @@ -47,7 +47,6 @@ | |||
| #include <rclcpp/rclcpp.hpp> | |||
| #include <sensor_msgs/msg/camera_info.hpp> | |||
| #include <sensor_msgs/msg/point_cloud2.hpp> | |||
| #include <soccer_vision_3d_msgs/msg/field_boundary.hpp> | |||
| #include <soccer_vision_3d_msgs/msg/goalpost_array.hpp> | |||
There was a problem hiding this comment.
same here, are goalposts even needed?
| @@ -27,9 +27,6 @@ Localization::Localization(std::shared_ptr<rclcpp::Node> node) | |||
| goal_subscriber_ = node->create_subscription<sv3dm::msg::GoalpostArray>( | |||
There was a problem hiding this comment.
same here, goal posts needed?
| @@ -203,11 +190,9 @@ void Localization::LinePointcloudCallback(const sm::msg::PointCloud2 &msg) { lin | |||
|
|
|||
| void Localization::GoalPostsCallback(const sv3dm::msg::GoalpostArray &msg) { goal_posts_relative_ = msg; } | |||
| @@ -355,15 +353,9 @@ void Localization::getMotion() { | |||
| linear_movement_.y = 0; | |||
| RCLCPP_WARN(node_->get_logger(), "Robot moved an unreasonable amount, dropping motion."); | |||
| } | |||
|
|
|||
| // Check if robot moved | |||
There was a problem hiding this comment.
is robot_moved not checked anymore? Is this not important?
|
I tested this in sim on my machine and it worked. |
Summary
Until now we rated the current state of the localization particles always with a delayed measurement. The particles are moved using the odometry "instantly" and are therefore not where the localization expects them to be at a different place compared to when the measurement was taken. Therefore, particles that lag behind were preferred, as they better matched the observations, which were assumed to be also instantly.
The solution is to store the observation timestamp and move all particles temporary back by the amount we have walked since the measurement, as determined by the odometry before rating them. This solution works also for multiple asynchronous measurement sources.
I hope that this enables more aggressive path planning parameters with less oscillations, as the localization state is not delayed as much.
I also did some cleanup, like simplifying the motion model math significantly and removing the field boundary input (we never used it) as well as other unused features I encountered.
Here is a plot of the robots center of mass and the position estimated by the localization. It needs to be noted that the robot walks very fast and the vision had an artificial pipeline delay of 1 second to test the effectiveness of the approach:
Checklist
colcon build