| 59 | } |
| 60 | |
| 61 | void MotionModel::updateParticles( |
| 62 | std::vector<Particle> & particles, |
| 63 | geometry_msgs::msg::Twist::SharedPtr cmd_msg, |
| 64 | rclcpp::Time current_time) |
| 65 | { |
| 66 | double dt = current_time.seconds() - last_message_time_.seconds(); |
| 67 | if (dt > 1.0) { |
| 68 | last_message_time_ = current_time; |
| 69 | // very small motion can help with convergence |
| 70 | dt = 0.01; |
| 71 | } |
| 72 | for (Particle & particle : particles) { |
| 73 | updateParticle(particle, dt, cmd_msg); |
| 74 | } |
| 75 | last_message_time_ = current_time; |
| 76 | } |
| 77 | |
| 78 | bool MotionModel::getEnabled(const rclcpp::Time & current_time) |
| 79 | { |