From 64f680410f24cee1798c4b118c0260a3f75f8167 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 2 Nov 2023 16:22:30 +0100 Subject: [PATCH] Clean localization --- .../bitbots_localization/ObservationModel.hpp | 9 +-- .../bitbots_localization/localization.hpp | 58 +++++++++++-------- bitbots_localization/src/ObservationModel.cpp | 8 +-- bitbots_localization/src/localization.cpp | 13 ++--- 4 files changed, 46 insertions(+), 42 deletions(-) diff --git a/bitbots_localization/include/bitbots_localization/ObservationModel.hpp b/bitbots_localization/include/bitbots_localization/ObservationModel.hpp index 5aed21a..b445b8c 100644 --- a/bitbots_localization/include/bitbots_localization/ObservationModel.hpp +++ b/bitbots_localization/include/bitbots_localization/ObservationModel.hpp @@ -54,8 +54,6 @@ class RobotPoseObservationModel : public particle_filter::ObservationModel> get_measurement_field_boundary() const; - void set_min_weight(double min_weight); - double get_min_weight() const override; void clear_measurement(); @@ -67,18 +65,17 @@ class RobotPoseObservationModel : public particle_filter::ObservationModel> &last_measurement, std::shared_ptr map, double element_weight) const; + // Measurements std::vector> last_measurement_lines_; - std::vector> last_measurement_goal_; - std::vector> last_measurement_field_boundary_; - double min_weight_ = 0; - + // Reference to the maps for the different classes std::shared_ptr map_lines_; std::shared_ptr map_goals_; std::shared_ptr map_field_boundary_; + // Parameters bitbots_localization::Params config_; }; }; // namespace bitbots_localization diff --git a/bitbots_localization/include/bitbots_localization/localization.hpp b/bitbots_localization/include/bitbots_localization/localization.hpp index 6f477e1..7420dca 100644 --- a/bitbots_localization/include/bitbots_localization/localization.hpp +++ b/bitbots_localization/include/bitbots_localization/localization.hpp @@ -147,12 +147,14 @@ class Localization : public rclcpp::Node { // Data structure to hold all parameters, which is build from the schema in the 'parameters.yaml' bitbots_localization::Params config_; + // Declare subscribers rclcpp::Subscription::SharedPtr line_point_cloud_subscriber_; rclcpp::Subscription::SharedPtr goal_subscriber_; rclcpp::Subscription::SharedPtr fieldboundary_subscriber_; rclcpp::Subscription::SharedPtr rviz_initial_pose_subscriber_; + // Declare publishers rclcpp::Publisher::SharedPtr pose_with_covariance_publisher_; rclcpp::Publisher::SharedPtr pose_particles_publisher_; rclcpp::Publisher::SharedPtr lines_publisher_; @@ -161,35 +163,43 @@ class Localization : public rclcpp::Node { rclcpp::Publisher::SharedPtr fieldboundary_ratings_publisher_; rclcpp::Publisher::SharedPtr field_publisher_; + // Declare services rclcpp::Service::SharedPtr reset_service_; rclcpp::Service::SharedPtr pause_service_; + + // Declare timers rclcpp::TimerBase::SharedPtr publishing_timer_; + + // Declare tf2 objects std::unique_ptr tfBuffer; std::shared_ptr tfListener; std::shared_ptr br; + // Declare particle filter components std::shared_ptr> resampling_; std::shared_ptr robot_pose_observation_model_; std::shared_ptr robot_motion_model_; + std::shared_ptr> robot_pf_; + + // Declare initial state distributions std::shared_ptr robot_state_distribution_start_left_; std::shared_ptr robot_state_distribution_start_right_; std::shared_ptr robot_state_distribution_right_half_; std::shared_ptr robot_state_distribution_left_half_; std::shared_ptr robot_state_distribution_position_; std::shared_ptr robot_state_distribution_pose_; - std::shared_ptr> robot_pf_; + + // Declare filter estimate RobotState estimate_; std::vector estimate_cov_; - bool resampled_ = false; - + // Declare input message buffers sm::msg::PointCloud2 line_pointcloud_relative_; sv3dm::msg::GoalpostArray goal_posts_relative_; sv3dm::msg::FieldBoundary fieldboundary_relative_; - sm::msg::CameraInfo cam_info_; + // Declare time stamps rclcpp::Time last_stamp_lines = rclcpp::Time(0); - rclcpp::Time last_stamp_lines_pc = rclcpp::Time(0); rclcpp::Time last_stamp_goals = rclcpp::Time(0); rclcpp::Time last_stamp_fb_points = rclcpp::Time(0); rclcpp::Time last_stamp_all_measurements = rclcpp::Time(0); @@ -198,21 +208,31 @@ class Localization : public rclcpp::Node { builtin_interfaces::msg::Time localization_tf_last_published_time_ = builtin_interfaces::msg::Time(rclcpp::Time(0, 0, RCL_ROS_TIME)); - /** - * Runs the filter for one step - */ - void run_filter_one_step(); + // Declare robot movement variables + geometry_msgs::msg::Vector3 linear_movement_; + geometry_msgs::msg::Vector3 rotational_movement_; + + // Keep track of the odometry transform in the last step + geometry_msgs::msg::TransformStamped previousOdomTransform_; + // Flag that checks if the robot is moving + bool robot_moved = false; + + // Keep track of the number of filter steps + int timer_callback_count_ = 0; + + // Maps for the different measurement classes std::shared_ptr lines_; std::shared_ptr goals_; std::shared_ptr field_boundary_; - std::shared_ptr corner_; - std::shared_ptr t_crossings_map_; - std::shared_ptr crosses_map_; - gmms::GaussianMixtureModel pose_gmm_; + // RNG that is used for the different sampling steps particle_filter::CRandomNumberGenerator random_number_generator_; - std_msgs::msg::ColorRGBA marker_color; + + /** + * Runs the filter for one step + */ + void run_filter_one_step(); /** * Publishes the position as a transform @@ -260,16 +280,6 @@ class Localization : public rclcpp::Node { * Gets the and convert motion / odometry information */ void getMotion(); - - geometry_msgs::msg::TransformStamped transformOdomBaseLink; - bool initialization = true; - - geometry_msgs::msg::Vector3 linear_movement_; - geometry_msgs::msg::Vector3 rotational_movement_; - geometry_msgs::msg::TransformStamped previousOdomTransform_; - bool new_linepoints_ = false; - bool robot_moved = false; - int timer_callback_count_ = 0; }; }; // namespace bitbots_localization diff --git a/bitbots_localization/src/ObservationModel.cpp b/bitbots_localization/src/ObservationModel.cpp index fd2e03a..3d18472 100644 --- a/bitbots_localization/src/ObservationModel.cpp +++ b/bitbots_localization/src/ObservationModel.cpp @@ -50,8 +50,8 @@ double RobotPoseObservationModel::measure(const RobotState &state) const { ((1 - scoring_config.field_boundary.factor) + scoring_config.field_boundary.factor * particle_weight_field_boundary)); - if (weight < min_weight_) { - weight = min_weight_; + if (weight < config_.particle_filter.weighting.min_weight) { + weight = config_.particle_filter.weighting.min_weight; } // reduce weight if particle is too far outside of the field: @@ -101,9 +101,7 @@ std::vector> RobotPoseObservationModel::get_measuremen return last_measurement_field_boundary_; } -void RobotPoseObservationModel::set_min_weight(double min_weight) { min_weight_ = min_weight; } - -double RobotPoseObservationModel::get_min_weight() const { return min_weight_; } +double RobotPoseObservationModel::get_min_weight() const { return config_.particle_filter.weighting.min_weight; } void RobotPoseObservationModel::clear_measurement() { last_measurement_lines_.clear(); diff --git a/bitbots_localization/src/localization.cpp b/bitbots_localization/src/localization.cpp index 0e86191..155e104 100644 --- a/bitbots_localization/src/localization.cpp +++ b/bitbots_localization/src/localization.cpp @@ -106,7 +106,6 @@ void Localization::updateParams(bool force_reload) { // Init observation model robot_pose_observation_model_.reset(new RobotPoseObservationModel(lines_, goals_, field_boundary_, config_)); - robot_pose_observation_model_->set_min_weight(config_.particle_filter.weighting.min_weight); // Init motion model auto drift_config = config_.particle_filter.drift; @@ -166,7 +165,6 @@ void Localization::updateParams(bool force_reload) { void Localization::run_filter_one_step() { timer_callback_count_++; - resampled_ = false; // Check for new parameters and recreate necessary components if needed updateParams(); @@ -194,7 +192,6 @@ void Localization::run_filter_one_step() { // Check if its resampling time! if (timer_callback_count_ % config_.particle_filter.resampling_interval == 0) { robot_pf_->resample(); - resampled_ = true; } // Publish transforms publish_transforms(); @@ -298,7 +295,7 @@ void Localization::reset_filter(int distribution, double x, double y, double ang void Localization::updateMeasurements() { // Sets the measurements in the observation model - if (line_pointcloud_relative_.header.stamp != last_stamp_lines_pc && config_.particle_filter.scoring.lines.factor) { + if (line_pointcloud_relative_.header.stamp != last_stamp_lines && config_.particle_filter.scoring.lines.factor) { robot_pose_observation_model_->set_measurement_lines_pc(line_pointcloud_relative_); } if (config_.particle_filter.scoring.goal.factor && goal_posts_relative_.header.stamp != last_stamp_goals) { @@ -310,15 +307,14 @@ void Localization::updateMeasurements() { } // Set timestamps to mark past messages - last_stamp_lines_pc = line_pointcloud_relative_.header.stamp; + last_stamp_lines = line_pointcloud_relative_.header.stamp; last_stamp_goals = goal_posts_relative_.header.stamp; last_stamp_fb_points = fieldboundary_relative_.header.stamp; // Maximum time stamp of the last measurements - last_stamp_all_measurements = std::max({last_stamp_lines_pc, last_stamp_goals, last_stamp_fb_points}); + last_stamp_all_measurements = std::max({last_stamp_lines, last_stamp_goals, last_stamp_fb_points}); } void Localization::getMotion() { - robot_moved = false; geometry_msgs::msg::TransformStamped transformStampedNow; try { @@ -356,6 +352,7 @@ void Localization::getMotion() { rotational_movement_.z / time_delta >= config_.misc.min_motion_angular; } else { RCLCPP_WARN(this->get_logger(), "Time step delta of zero encountered! This should not happen!"); + robot_moved = false; } // Set the variable for the transform of the previous step to the transform of the current step, because we finished @@ -423,6 +420,7 @@ void Localization::publish_transforms() { localization_transform.transform.rotation.z = q.z(); localization_transform.transform.rotation.w = q.w(); + // Check if a transform for the current timestamp was already published if (localization_tf_last_published_time_ != localization_transform.header.stamp) { // Do not resend a transform for the same timestamp localization_tf_last_published_time_ = localization_transform.header.stamp; @@ -446,6 +444,7 @@ void Localization::publish_transforms() { RCLCPP_DEBUG(this->get_logger(), "Transform %s", geometry_msgs::msg::to_yaml(map_odom_transform).c_str()); + // Check if a transform for the current timestamp was already published if (map_odom_tf_last_published_time_ != map_odom_transform.header.stamp) { // Do not resend a transform for the same timestamp map_odom_tf_last_published_time_ = map_odom_transform.header.stamp;