Class Localization

Class Documentation

class bitbots_localization::Localization

Includes the ROS interface, configuration and main loop of the Bit-Bots RoboCup localization.

Public Functions

explicit Localization(rclcpp::Node::SharedPtr node)
bool set_paused_callback(const std::shared_ptr<bl::srv::SetPaused::Request> req, std::shared_ptr<bl::srv::SetPaused::Response> res)

Callback for the pause service

Parameters

  • req – Request.

  • res – Response.

bool reset_filter_callback(const std::shared_ptr<bl::srv::ResetFilter::Request> req, std::shared_ptr<bl::srv::ResetFilter::Response> res)

Callback for the filter reset service

Parameters

  • req – Request.

  • res – Response.

void updateParams(bool force_reload = false)

Checks if we have new params and if so reconfigures the filter

Parameters

force_reload – If true, the filter is reconfigured even if no new params are available

void LinePointcloudCallback(const sm::msg::PointCloud2 &msg)

Callback for the line point cloud measurements

Parameters

msg – Message containing the line point cloud.

void GoalPostsCallback(const sv3dm::msg::GoalpostArray &msg)

Callback for goal posts measurements

Parameters

msg – Message containing the goal posts.

void SetInitialPositionCallback(const gm::msg::PoseWithCovarianceStamped &msg)

Resets the state distribution of the state space

Parameters

distribution – The type of the distribution

void reset_filter(int distribution)
void reset_filter(int distribution, double x, double y)

Resets the state distribution of the state space

Parameters

  • distribution – The type of the distribution

  • x – Position of the new state distribution

  • y – Position of the new state distribution

void reset_filter(int distribution, double x, double y, double angle)

Resets the state distribution of the state space

Parameters

  • distribution – The type of the distribution

  • x – Position of the new state distribution

  • y – Position of the new state distribution

  • angle – Angle in which the particle distribution is centered

Private Functions

void run_filter_one_step()

Runs the filter for one step

void publish_transforms()

Publishes the position as a transform

void publish_pose_with_covariance()

Publishes the position as a message

void publish_debug()

Debug publisher

void publish_particle_markers()

Publishes the visualization markers for each particle

void publish_ratings()

Publishes the rating visualizations for each measurement class

void publish_debug_rating(const std::vector<std::pair<double, double>> &measurements, double scale, const char name[24], std::shared_ptr<Map> map, rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr &publisher)

Publishes the rating visualizations for a arbitrary measurement class

Parameters

  • measurements – all measurements of the measurement class

  • scale – scale of the markers

  • name – name of the class

  • map – map for this class

  • publisher – ros publisher for the type visualization_msgs::msg::Marker

void updateMeasurements()

Updates the measurements for all classes

void getMotion()

Gets the and convert motion / odometry information

Private Members

rclcpp::Node::SharedPtr node_
bitbots_localization::ParamListener param_listener_
bitbots_localization::Params config_
bitbots_localization::FieldDimensions field_dimensions_
std::string field_name_
rclcpp::Subscription<sm::msg::PointCloud2>::SharedPtr line_point_cloud_subscriber_
rclcpp::Subscription<sv3dm::msg::GoalpostArray>::SharedPtr goal_subscriber_
rclcpp::Subscription<gm::msg::PoseWithCovarianceStamped>::SharedPtr rviz_initial_pose_subscriber_
rclcpp::Publisher<gm::msg::PoseWithCovarianceStamped>::SharedPtr pose_with_covariance_publisher_
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pose_particles_publisher_
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr line_ratings_publisher_
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr goal_ratings_publisher_
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr field_publisher_
rclcpp::Service<bl::srv::ResetFilter>::SharedPtr reset_service_
rclcpp::Service<bl::srv::SetPaused>::SharedPtr pause_service_
rclcpp::TimerBase::SharedPtr publishing_timer_
std::shared_ptr<tf2_ros::Buffer> tf_buffer_
std::shared_ptr<tf2_ros::TransformListener> tf_listener_
std::shared_ptr<tf2_ros::TransformBroadcaster> br
std::shared_ptr<pf::ImportanceResampling<RobotState>> resampling_
std::shared_ptr<RobotPoseObservationModel> robot_pose_observation_model_
std::shared_ptr<RobotMotionModel> robot_motion_model_
std::shared_ptr<particle_filter::ParticleFilter<RobotState>> robot_pf_
std::shared_ptr<RobotStateDistributionOwnSideline> robot_state_distribution_own_sidelines
std::shared_ptr<RobotStateDistributionOwnHalf> robot_state_distribution_own_half_
std::shared_ptr<RobotStateDistributionOpponentHalf> robot_state_distribution_opponent_half
RobotState estimate_
std::vector<double> estimate_cov_
sm::msg::PointCloud2 line_pointcloud_relative_
sv3dm::msg::GoalpostArray goal_posts_relative_
rclcpp::Time last_stamp_lines = rclcpp::Time(0)
rclcpp::Time last_stamp_goals = rclcpp::Time(0)
rclcpp::Time last_stamp_fb_points = rclcpp::Time(0)
builtin_interfaces::msg::Time map_odom_tf_last_published_time_ = builtin_interfaces::msg::Time(rclcpp::Time(0, 0, RCL_ROS_TIME))
builtin_interfaces::msg::Time localization_tf_last_published_time_ = builtin_interfaces::msg::Time(rclcpp::Time(0, 0, RCL_ROS_TIME))
geometry_msgs::msg::Vector3 linear_movement_
geometry_msgs::msg::Vector3 rotational_movement_
geometry_msgs::msg::TransformStamped previousOdomTransform_
int timer_callback_count_ = 0
std::shared_ptr<Map> lines_
std::shared_ptr<Map> goals_
particle_filter::CRandomNumberGenerator random_number_generator_