From 8a0f304faf55f1aef28528cb800fa2b201d0e6de Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 24 Oct 2023 22:05:09 +0200 Subject: [PATCH] Use generate_parameter_library and cleanup parameters --- bitbots_localization/CMakeLists.txt | 14 +- bitbots_localization/config/config.yaml | 128 ++++----- .../config/fields/bangkok/config.yaml | 42 +-- .../config/fields/feldraum/config.yaml | 40 +-- .../config/fields/gazebo/config.yaml | 43 +-- .../config/fields/spl/config.yaml | 39 +-- .../config/fields/webots/config.yaml | 39 +-- .../bitbots_localization/MotionModel.hpp | 4 +- .../bitbots_localization/ObservationModel.hpp | 7 +- .../include/bitbots_localization/config.hpp | 83 ------ .../bitbots_localization/localization.hpp | 34 +-- .../launch/localization.launch | 12 +- bitbots_localization/package.xml | 1 + bitbots_localization/src/MotionModel.cpp | 10 +- bitbots_localization/src/ObservationModel.cpp | 41 +-- bitbots_localization/src/config.cpp | 171 ------------ bitbots_localization/src/localization.cpp | 253 ++++++++++-------- bitbots_localization/src/parameters.yml | 241 +++++++++++++++++ 18 files changed, 547 insertions(+), 655 deletions(-) delete mode 100644 bitbots_localization/include/bitbots_localization/config.hpp delete mode 100644 bitbots_localization/src/config.cpp create mode 100644 bitbots_localization/src/parameters.yml diff --git a/bitbots_localization/CMakeLists.txt b/bitbots_localization/CMakeLists.txt index 510b08e4..1e558a0a 100644 --- a/bitbots_localization/CMakeLists.txt +++ b/bitbots_localization/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(backward_ros REQUIRED) find_package(Boost COMPONENTS filesystem REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(cv_bridge REQUIRED) +find_package(generate_parameter_library REQUIRED) find_package(image_transport REQUIRED) find_package(nav_msgs REQUIRED) find_package(particle_filter REQUIRED) @@ -23,6 +24,10 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(visualization_msgs REQUIRED) +generate_parameter_library( + localization_parameters # cmake target name for the parameter library + src/parameters.yml) + rosidl_generate_interfaces(${PROJECT_NAME} "srv/ResetFilter.srv" "srv/SetPaused.srv" DEPENDENCIES builtin_interfaces) @@ -61,7 +66,7 @@ enable_bitbots_docs() include_directories(include) ## Declare a C++ library -set(SOURCES src/config.cpp src/localization.cpp src/map.cpp src/MotionModel.cpp +set(SOURCES src/localization.cpp src/map.cpp src/MotionModel.cpp src/ObservationModel.cpp src/RobotState.cpp src/StateDistribution.cpp src/tools.cpp) @@ -88,13 +93,12 @@ ament_target_dependencies(localization tf2 tf2_geometry_msgs tf2_ros - visualization_msgs -) + visualization_msgs) rosidl_get_typesupport_target(cpp_typesupport_target - ${PROJECT_NAME} "rosidl_typesupport_cpp") + ${PROJECT_NAME} "rosidl_typesupport_cpp") -target_link_libraries(localization "${cpp_typesupport_target}") +target_link_libraries(localization "${cpp_typesupport_target}" localization_parameters) install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) diff --git a/bitbots_localization/config/config.yaml b/bitbots_localization/config/config.yaml index 55544308..279bf550 100644 --- a/bitbots_localization/config/config.yaml +++ b/bitbots_localization/config/config.yaml @@ -1,81 +1,51 @@ bitbots_localization: ros__parameters: - - ######## - # MISC # - ######## - - init_mode: 0 - - ############# - # ROS-Stuff # - ############# - - line_topic: 'line_relative' - line_pointcloud_topic: 'line_mask_relative_pc' - goal_topic: 'goals_simulated' - fieldboundary_topic: 'field_boundary_relative' - fieldboundary_in_image_topic: 'field_boundary_in_image' - - particle_publishing_topic: 'pose_particles' - - publishing_frequency: 10 - - ################# - # Visualization # - ################# - - debug_visualization: true - - - ################### - # Particle Filter # - ################### - - - particle_number: 300 - resampling_interval: 2 - - diffusion_x_std_dev: 0.8 - diffusion_y_std_dev: 0.8 - diffusion_t_std_dev: 0.9 - diffusion_multiplicator: 0.001 - starting_diffusion: 0.05 - starting_steps_with_higher_diffusion: 40 - - drift_distance_to_direction: 2.0 - drift_rotation_to_direction: 0.0 - drift_distance_to_distance: 0.1 - drift_rotation_to_distance: 0.2 - drift_distance_to_rotation: 0.0 - drift_rotation_to_rotation: 3.0 - - max_rotation: 0.45 - max_translation: 0.09 - - min_weight: 0.01 - min_resampling_weight: 0.1 - out_of_field_weight_decrease: 0.01 - out_of_field_range: 0.5 # in m - percentage_best_particles: 50 - - distance_factor : 0.5 - lines_factor: 1.0 - goals_factor: 0.0 - field_boundary_factor: 0.0 - corners_factor : 0.0 - t_crossings_factor: 0.0 - crosses_factor : 0.0 - - line_element_confidence: 0.01 - goal_element_confidence: 0.0 - field_boundary_element_confidence: 0.0 - corner_element_confidence: 0.0 - t_crossing_element_confidence: 0.0 - cross_element_confidence: 0.0 - - min_motion_linear: 0.0 - min_motion_angular: 0.0 - filter_only_with_motion: false - - measurement_out_of_map_punishment: 10.0 + misc: + init_mode: 0 + percentage_best_particles: 50 + min_motion_linear: 0.0 + min_motion_angular: 0.0 + filter_only_with_motion: false + ros: + line_pointcloud_topic: 'line_mask_relative_pc' + goal_topic: 'goals_simulated' + fieldboundary_topic: 'field_boundary_relative' + particle_publishing_topic: 'pose_particles' + debug_visualization: true + particle_filter: + particle_number: 300 + rate: 10 + resampling_interval: 2 + diffusion: + x_std_dev: 0.8 + y_std_dev: 0.8 + t_std_dev: 0.9 + multiplier: 0.001 + starting_multiplier: 0.05 + starting_steps_with_higher_diffusion: 40 + drift: + distance_to_direction: 2.0 + rotation_to_direction: 0.0 + distance_to_distance: 0.1 + rotation_to_distance: 0.2 + distance_to_rotation: 0.0 + rotation_to_rotation: 3.0 + max_rotation: 0.45 + max_translation: 0.09 + weighting: + min_weight: 0.01 + particle_reset_weight: 0.1 + out_of_field_weight_decrease: 0.01 + out_of_field_range: 0.5 + scoring: + lines_factor: 1.0 + goal_factor: 0.0 + field_boundary_factor: 0.0 + confidences: + line_element: 0.01 + goal_element: 0.0 + field_boundary_element: 0.0 + + + + diff --git a/bitbots_localization/config/fields/bangkok/config.yaml b/bitbots_localization/config/fields/bangkok/config.yaml index e419223b..683ab622 100644 --- a/bitbots_localization/config/fields/bangkok/config.yaml +++ b/bitbots_localization/config/fields/bangkok/config.yaml @@ -2,36 +2,18 @@ # RoboCup 2022 Bangkok # ######################## -# Settings override for the webots simulator field +# Settings override for the Webots simulator field bitbots_localization: ros__parameters: - - ############################# - # Initial pose of the robot # - ############################# - - # Initialize at sideline - # TODO better parameters - initial_robot_x1: 2.5 - initial_robot_y1: 3.0 - initial_robot_t1: -1.5 - initial_robot_x2: 2.5 - initial_robot_y2: -3.0 - initial_robot_t2: 1.57 - - # Inertial single point position - initial_robot_x: -2.0 - initial_robot_y: 0.0 - initial_robot_t: 0.0 - - ################## - # Field settings # - ################## - - # Field size - field_x: 9.0 - field_y: 6.0 - - # Padding - field_padding: 1.0 + field: + size: + x: 9.0 + y: 6.0 + padding: 1.0 + initialization: + single_pose: + x: -2.0 + y: 0.0 + t: 0.0 + \ No newline at end of file diff --git a/bitbots_localization/config/fields/feldraum/config.yaml b/bitbots_localization/config/fields/feldraum/config.yaml index 07dd320e..3955e0af 100644 --- a/bitbots_localization/config/fields/feldraum/config.yaml +++ b/bitbots_localization/config/fields/feldraum/config.yaml @@ -6,32 +6,14 @@ bitbots_localization: ros__parameters: - - ############################# - # Initial pose of the robot # - ############################# - - # Initialize at sideline - # TODO better parameters - initial_robot_x1: 2.5 - initial_robot_y1: 3.0 - initial_robot_t1: -1.5 - initial_robot_x2: 2.5 - initial_robot_y2: -3.0 - initial_robot_t2: 1.57 - - # Inertial single point position - initial_robot_x: -2.0 - initial_robot_y: 0.0 - initial_robot_t: 0.0 - - ################## - # Field settings # - ################## - - # Field size - field_x: 5.45 - field_y: 3.88 - - # Padding - field_padding: 0.7 #0.1 + field: + size: + x: 5.45 + y: 3.88 + padding: 0.7 + initialization: + single_pose: + x: -2.0 + y: 0.0 + t: 0.0 + \ No newline at end of file diff --git a/bitbots_localization/config/fields/gazebo/config.yaml b/bitbots_localization/config/fields/gazebo/config.yaml index 2c6b2c48..9fdbebd5 100644 --- a/bitbots_localization/config/fields/gazebo/config.yaml +++ b/bitbots_localization/config/fields/gazebo/config.yaml @@ -6,36 +6,13 @@ bitbots_localization: ros__parameters: - - ############################# - # Initial pose of the robot # - ############################# - - # Initialize at sideline - # TODO better parameters - initial_robot_x1: 2.5 - initial_robot_y1: 3.0 - initial_robot_t1: -1.5 - initial_robot_x2: 2.5 - initial_robot_y2: -3 - initial_robot_t2: 1.57 - - # Inertial single point position - initial_robot_x: -2.0 - initial_robot_y: 0.0 - initial_robot_t: 0.0 - - ######## - # Maps # - ######## - - ################## - # Field settings # - ################## - - # Field size - field_x: 9.0 - field_y: 6.0 - - # Padding - field_padding: 1.0 + field: + size: + x: 9.0 + y: 6.0 + padding: 1.0 + initialization: + single_pose: + x: -2.0 + y: 0.0 + t: 0.0 diff --git a/bitbots_localization/config/fields/spl/config.yaml b/bitbots_localization/config/fields/spl/config.yaml index 0de43dba..aac7a5e0 100644 --- a/bitbots_localization/config/fields/spl/config.yaml +++ b/bitbots_localization/config/fields/spl/config.yaml @@ -6,32 +6,13 @@ bitbots_localization: ros__parameters: - - ############################# - # Initial pose of the robot # - ############################# - - # Initialize at sideline - # TODO better parameters - initial_robot_x1: 2.5 - initial_robot_y1: 3.0 - initial_robot_t1: -1.5 - initial_robot_x2: 2.5 - initial_robot_y2: -3.0 - initial_robot_t2: 1.57 - - # Inertial single point position - initial_robot_x: -2.0 - initial_robot_y: 0.0 - initial_robot_t: 0.0 - - ################## - # Field settings # - ################## - - # Field size - field_x: 9.0 - field_y: 6.0 - - # Padding - field_padding: 0.7 + field: + size: + x: 9.0 + y: 6.0 + padding: 0.7 + initialization: + single_pose: + x: -2.0 + y: 0.0 + t: 0.0 diff --git a/bitbots_localization/config/fields/webots/config.yaml b/bitbots_localization/config/fields/webots/config.yaml index 7f816e00..bf2ed915 100644 --- a/bitbots_localization/config/fields/webots/config.yaml +++ b/bitbots_localization/config/fields/webots/config.yaml @@ -6,32 +6,13 @@ bitbots_localization: ros__parameters: - - ############################# - # Initial pose of the robot # - ############################# - - # Initialize at sideline - # TODO better parameters - initial_robot_x1: 2.5 - initial_robot_y1: 3.0 - initial_robot_t1: -1.5 - initial_robot_x2: 2.5 - initial_robot_y2: -3.0 - initial_robot_t2: 1.57 - - # Inertial single point position - initial_robot_x: -2.0 - initial_robot_y: 0.0 - initial_robot_t: 0.0 - - ################## - # Field settings # - ################## - - # Field size - field_x: 9.0 - field_y: 6.0 - - # Padding - field_padding: 1.0 + field: + size: + x: 9.0 + y: 6.0 + padding: 1.0 + initialization: + single_pose: + x: -2.0 + y: 0.0 + t: 0.0 diff --git a/bitbots_localization/include/bitbots_localization/MotionModel.hpp b/bitbots_localization/include/bitbots_localization/MotionModel.hpp index 9069bfac..d767c4ed 100644 --- a/bitbots_localization/include/bitbots_localization/MotionModel.hpp +++ b/bitbots_localization/include/bitbots_localization/MotionModel.hpp @@ -31,7 +31,7 @@ class RobotMotionModel : public particle_filter::MovementModel { */ RobotMotionModel(const particle_filter::CRandomNumberGenerator &random_number_generator, double diffuse_xStdDev, - double diffuse_yStdDev, double diffuse_tStdDev, double diffuse_multiplicator, + double diffuse_yStdDev, double diffuse_tStdDev, double diffuse_multiplier, Eigen::Matrix drift_cov); /** @@ -48,7 +48,7 @@ class RobotMotionModel : public particle_filter::MovementModel { */ void diffuse(RobotState &state) const override; - double diffuse_multiplicator_; + double diffuse_multiplier_; protected: private: diff --git a/bitbots_localization/include/bitbots_localization/ObservationModel.hpp b/bitbots_localization/include/bitbots_localization/ObservationModel.hpp index f715c91d..5aed21a5 100644 --- a/bitbots_localization/include/bitbots_localization/ObservationModel.hpp +++ b/bitbots_localization/include/bitbots_localization/ObservationModel.hpp @@ -8,7 +8,6 @@ #include #include -#include #include #include #include @@ -19,6 +18,8 @@ #include #include +#include "localization_parameters.hpp" + namespace sm = sensor_msgs; namespace bl = bitbots_localization; namespace sv3dm = soccer_vision_3d_msgs; @@ -30,7 +31,7 @@ class RobotPoseObservationModel : public particle_filter::ObservationModel map_lines, std::shared_ptr map_goals, - std::shared_ptr map_field_boundary, std::shared_ptr config); + std::shared_ptr map_field_boundary, const bitbots_localization::Params &config); /** * @@ -78,7 +79,7 @@ class RobotPoseObservationModel : public particle_filter::ObservationModel map_goals_; std::shared_ptr map_field_boundary_; - std::shared_ptr config_; + bitbots_localization::Params config_; }; }; // namespace bitbots_localization diff --git a/bitbots_localization/include/bitbots_localization/config.hpp b/bitbots_localization/include/bitbots_localization/config.hpp deleted file mode 100644 index 07684eac..00000000 --- a/bitbots_localization/include/bitbots_localization/config.hpp +++ /dev/null @@ -1,83 +0,0 @@ -// -// Created by judith on 08.03.19. -// - -#ifndef BITBOTS_LOCALIZATION_CONFIG_H -#define BITBOTS_LOCALIZATION_CONFIG_H - -#include -#include - -namespace bitbots_localization { -/** - * @class Config - * @brief Stores a configuration of the localization - */ -class Config { - public: - /** - * @param parameters of the localization. - */ - explicit Config(std::vector parameters); - - /** - * @param parameters of the localization. - */ - void update_params(std::vector parameters); - - int init_mode = 0; - std::string line_pointcloud_topic = ""; - std::string goal_topic = ""; - std::string fieldboundary_topic = ""; - std::string particle_publishing_topic = ""; - int publishing_frequency = 10; - bool debug_visualization = true; - int particle_number = 1; - int resampling_interval = 0; - double diffusion_x_std_dev = 0.0; - double diffusion_y_std_dev = 0.0; - double diffusion_t_std_dev = 0.0; - double diffusion_multiplicator = 0.0; - double starting_diffusion = 0.0; - int starting_steps_with_higher_diffusion = 0; - double drift_distance_to_direction = 0.0; - double drift_rotation_to_direction = 0.0; - double drift_distance_to_distance = 0.0; - double drift_rotation_to_distance = 0.0; - double drift_distance_to_rotation = 0.0; - double drift_rotation_to_rotation = 0.0; - double max_rotation = 0.0; - double max_translation = 0.0; - double min_weight = 0.0; - double min_resampling_weight = 0.0; - double out_of_field_weight_decrease = 0.0; - double out_of_field_range = 0.0; - int percentage_best_particles = 0; - double distance_factor = 0.0; - double lines_factor = 0.0; - double goals_factor = 0.0; - double field_boundary_factor = 0.0; - double line_element_confidence = 0.0; - double goal_element_confidence = 0.0; - double field_boundary_element_confidence = 0.0; - double min_motion_linear = 0.0; - double min_motion_angular = 0.0; - bool filter_only_with_motion = false; - double measurement_out_of_map_punishment = 0.0; - // Field secific settings - double field_x = 0; - double field_y = 0; - double field_padding = 0; - double initial_robot_x1 = 0.0; - double initial_robot_y1 = 0.0; - double initial_robot_t1 = 0.0; - double initial_robot_x2 = 0.0; - double initial_robot_y2 = 0.0; - double initial_robot_t2 = 0.0; - double initial_robot_x = 0.0; - double initial_robot_y = 0.0; - double initial_robot_t = 0.0; -}; -}; // namespace bitbots_localization - -#endif // BITBOTS_LOCALIZATION_CONFIG_H diff --git a/bitbots_localization/include/bitbots_localization/localization.hpp b/bitbots_localization/include/bitbots_localization/localization.hpp index 34e24080..35309b39 100644 --- a/bitbots_localization/include/bitbots_localization/localization.hpp +++ b/bitbots_localization/include/bitbots_localization/localization.hpp @@ -1,5 +1,5 @@ // -// Created by judith on 08.03.19. +// Created by Judith on 08.03.19. // #ifndef BITBOTS_LOCALIZATION_LOCALIZATION_H @@ -24,7 +24,6 @@ #include #include #include -#include #include #include #include @@ -56,6 +55,8 @@ #include #include +#include "localization_parameters.hpp" + namespace sm = sensor_msgs; namespace gm = geometry_msgs; namespace pf = particle_filter; @@ -90,25 +91,25 @@ class Localization : public rclcpp::Node { std::shared_ptr res); /** - * Callback for new parameters - * @param parameters the updated parameters. + * Checks if we have new params and if so reconfigures the filter + * @param force_reload If true, the filter is reconfigured even if no new params are available */ - rcl_interfaces::msg::SetParametersResult onSetParameters(const std::vector ¶meters); + void updateParams(bool force_reload = false); /** - * Callback for the line point cloud messurements + * Callback for the line point cloud measurements * @param msg Message containing the line point cloud. */ void LinePointcloudCallback(const sm::msg::PointCloud2 &msg); /** - * Callback for goal posts messurements + * Callback for goal posts measurements * @param msg Message containing the goal posts. */ void GoalPostsCallback(const sv3dm::msg::GoalpostArray &msg); // TODO /** - * Callback for the relative field boundary messurements + * Callback for the relative field boundary measurements * @param msg Message containing the field boundary points. */ void FieldboundaryCallback(const sv3dm::msg::FieldBoundary &msg); @@ -140,14 +141,17 @@ class Localization : public rclcpp::Node { void reset_filter(int distribution, double x, double y, double angle); private: + // Declare parameter listener and struct from the generate_parameter_library + bitbots_localization::ParamListener param_listener_; + // Datastructure to hold all parameters, which is build from the schema in the 'parameters.yaml' + bitbots_localization::Params config_; + rclcpp::Subscription::SharedPtr line_point_cloud_subscriber_; rclcpp::Subscription::SharedPtr goal_subscriber_; rclcpp::Subscription::SharedPtr fieldboundary_subscriber_; rclcpp::Subscription::SharedPtr rviz_initial_pose_subscriber_; - OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; - rclcpp::Publisher::SharedPtr pose_with_covariance_publisher_; rclcpp::Publisher::SharedPtr pose_particles_publisher_; rclcpp::Publisher::SharedPtr lines_publisher_; @@ -193,8 +197,6 @@ 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)); - std::string odom_frame_, base_footprint_frame_, map_frame_, publishing_frame_; - /** * Runs the filter for one step */ @@ -209,9 +211,7 @@ class Localization : public rclcpp::Node { gmms::GaussianMixtureModel pose_gmm_; particle_filter::CRandomNumberGenerator random_number_generator_; - std::shared_ptr config_; std_msgs::msg::ColorRGBA marker_color; - bool first_configuration_ = true; /** * Publishes the position as a transform @@ -234,13 +234,13 @@ class Localization : public rclcpp::Node { void publish_particle_markers(); /** - * Publishes the rating visualizations for each meassurement class + * Publishes the rating visualizations for each measurement class */ void publish_ratings(); /** - * Publishes the rating visualizations for a abitray messurement class - * @param measurements all measurements of the messurement class + * Publishes the rating visualizations for a arbitrary measurement class + * @param measurements all measurements of the measurement class * @param scale scale of the markers * @param name name of the class * @param map map for this class diff --git a/bitbots_localization/launch/localization.launch b/bitbots_localization/launch/localization.launch index 29d4a5f1..49aa8eac 100644 --- a/bitbots_localization/launch/localization.launch +++ b/bitbots_localization/launch/localization.launch @@ -1,7 +1,7 @@ - + @@ -17,12 +17,12 @@ - + - - - - + + + + diff --git a/bitbots_localization/package.xml b/bitbots_localization/package.xml index e29e9cda..22ae6f1c 100644 --- a/bitbots_localization/package.xml +++ b/bitbots_localization/package.xml @@ -15,6 +15,7 @@ builtin_interfaces cv_bridge dynamic_stack_decider + generate_parameter_library geometry_msgs image_transport libopencv-dev diff --git a/bitbots_localization/src/MotionModel.cpp b/bitbots_localization/src/MotionModel.cpp index b50add75..d6c53ba2 100644 --- a/bitbots_localization/src/MotionModel.cpp +++ b/bitbots_localization/src/MotionModel.cpp @@ -8,9 +8,9 @@ namespace bitbots_localization { RobotMotionModel::RobotMotionModel(const particle_filter::CRandomNumberGenerator &random_number_generator, double diffuse_xStdDev, double diffuse_yStdDev, double diffuse_tStdDev, - double diffuse_multiplicator, Eigen::Matrix drift_cov) + double diffuse_multiplier, Eigen::Matrix drift_cov) : particle_filter::MovementModel(), - diffuse_multiplicator_(diffuse_multiplicator), + diffuse_multiplier_(diffuse_multiplier), random_number_generator_(random_number_generator), diffuse_xStdDev_(diffuse_xStdDev), diffuse_yStdDev_(diffuse_yStdDev), @@ -45,9 +45,9 @@ void RobotMotionModel::drift(RobotState &state, geometry_msgs::msg::Vector3 line } void RobotMotionModel::diffuse(RobotState &state) const { - state.setXPos(state.getXPos() + sample(diffuse_xStdDev_) * diffuse_multiplicator_); - state.setYPos(state.getYPos() + sample(diffuse_yStdDev_) * diffuse_multiplicator_); - double theta = state.getTheta() + sample(diffuse_tStdDev_) * diffuse_multiplicator_; + state.setXPos(state.getXPos() + sample(diffuse_xStdDev_) * diffuse_multiplier_); + state.setYPos(state.getYPos() + sample(diffuse_yStdDev_) * diffuse_multiplier_); + double theta = state.getTheta() + sample(diffuse_tStdDev_) * diffuse_multiplier_; state.setTheta(theta); } diff --git a/bitbots_localization/src/ObservationModel.cpp b/bitbots_localization/src/ObservationModel.cpp index ccb60d0f..d6fcdf9e 100644 --- a/bitbots_localization/src/ObservationModel.cpp +++ b/bitbots_localization/src/ObservationModel.cpp @@ -8,7 +8,7 @@ namespace bitbots_localization { RobotPoseObservationModel::RobotPoseObservationModel(std::shared_ptr map_lines, std::shared_ptr map_goals, std::shared_ptr map_field_boundary, - std::shared_ptr config) + const bitbots_localization::Params &config) : particle_filter::ObservationModel() { map_lines_ = map_lines; map_goals_ = map_goals; @@ -33,29 +33,34 @@ double RobotPoseObservationModel::calculate_weight_for_class( } double RobotPoseObservationModel::measure(const RobotState &state) const { - double particle_weight_lines = - calculate_weight_for_class(state, last_measurement_lines_, map_lines_, config_->line_element_confidence); - double particle_weight_goal = - calculate_weight_for_class(state, last_measurement_goal_, map_goals_, config_->goal_element_confidence); - double particle_weight_field_boundary = calculate_weight_for_class( - state, last_measurement_field_boundary_, map_field_boundary_, config_->field_boundary_element_confidence); - - double weight = - (((1 - config_->lines_factor) + config_->lines_factor * particle_weight_lines) * - ((1 - config_->goals_factor) + config_->goals_factor * particle_weight_goal) * - ((1 - config_->field_boundary_factor) + config_->field_boundary_factor * particle_weight_field_boundary)); + double particle_weight_lines = calculate_weight_for_class(state, last_measurement_lines_, map_lines_, + config_.particle_filter.confidences.line_element); + double particle_weight_goal = calculate_weight_for_class(state, last_measurement_goal_, map_goals_, + config_.particle_filter.confidences.goal_element); + double particle_weight_field_boundary = + calculate_weight_for_class(state, last_measurement_field_boundary_, map_field_boundary_, + config_.particle_filter.confidences.field_boundary_element); + + // Get relevant config values + auto scoring_config = config_.particle_filter.scoring; + + // Calculate weight for the particle + double weight = (((1 - scoring_config.lines_factor) + scoring_config.lines_factor * particle_weight_lines) * + ((1 - scoring_config.goal_factor) + scoring_config.goal_factor * particle_weight_goal) * + ((1 - scoring_config.field_boundary_factor) + + scoring_config.field_boundary_factor * particle_weight_field_boundary)); if (weight < min_weight_) { weight = min_weight_; } // reduce weight if particle is too far outside of the field: - float range = config_->out_of_field_range; - if (state.getXPos() > (config_->field_x + config_->field_padding) / 2 + range || - state.getXPos() < -(config_->field_x + config_->field_padding) / 2 - range || - state.getYPos() > (config_->field_y + config_->field_padding) / 2 + range || - state.getYPos() < -(config_->field_y + config_->field_padding) / 2 - range) { - weight = weight - config_->out_of_field_weight_decrease; + float range = config_.particle_filter.weighting.out_of_field_range; + if (state.getXPos() > (config_.field.size.x + config_.field.padding) / 2 + range || + state.getXPos() < -(config_.field.size.x + config_.field.padding) / 2 - range || + state.getYPos() > (config_.field.size.y + config_.field.padding) / 2 + range || + state.getYPos() < -(config_.field.size.y + config_.field.padding) / 2 - range) { + weight = weight - config_.particle_filter.weighting.out_of_field_weight_decrease; } return weight; // exponential? diff --git a/bitbots_localization/src/config.cpp b/bitbots_localization/src/config.cpp deleted file mode 100644 index 62240c55..00000000 --- a/bitbots_localization/src/config.cpp +++ /dev/null @@ -1,171 +0,0 @@ -// -// Created by judith on 08.03.19. -// - -#include "bitbots_localization/config.hpp" - -namespace bitbots_localization { - -Config::Config(std::vector parameters) { Config::update_params(parameters); } - -constexpr unsigned int hash(const char* str, int h = 0) { return !str[h] ? 5381 : (hash(str, h + 1) * 33) ^ str[h]; } - -void Config::update_params(std::vector parameters) { - rclcpp::Logger LOGGER = rclcpp::get_logger("localization_param_loading"); - for (rclcpp::Parameter param : parameters) { - RCLCPP_DEBUG(LOGGER, "Currently loading param: %s", param.get_name().c_str()); - switch (hash(param.get_name().c_str())) { - case hash("init_mode"): - this->init_mode = param.as_int(); - break; - case hash("line_pointcloud_topic"): - this->line_pointcloud_topic = param.as_string(); - break; - case hash("goal_topic"): - this->goal_topic = param.as_string(); - break; - case hash("fieldboundary_topic"): - this->fieldboundary_topic = param.as_string(); - break; - case hash("particle_publishing_topic"): - this->particle_publishing_topic = param.as_string(); - break; - case hash("publishing_frequency"): - this->publishing_frequency = param.as_int(); - break; - case hash("debug_visualization"): - this->debug_visualization = param.as_bool(); - break; - case hash("particle_number"): - this->particle_number = param.as_int(); - break; - case hash("resampling_interval"): - this->resampling_interval = param.as_int(); - break; - case hash("diffusion_x_std_dev"): - this->diffusion_x_std_dev = param.as_double(); - break; - case hash("diffusion_y_std_dev"): - this->diffusion_y_std_dev = param.as_double(); - break; - case hash("diffusion_t_std_dev"): - this->diffusion_t_std_dev = param.as_double(); - break; - case hash("starting_diffusion"): - this->starting_diffusion = param.as_double(); - break; - case hash("starting_steps_with_higher_diffusion"): - this->starting_steps_with_higher_diffusion = param.as_int(); - break; - case hash("drift_distance_to_direction"): - this->drift_distance_to_direction = param.as_double(); - break; - case hash("drift_rotation_to_direction"): - this->drift_rotation_to_direction = param.as_double(); - break; - case hash("drift_distance_to_distance"): - this->drift_distance_to_distance = param.as_double(); - break; - case hash("drift_rotation_to_distance"): - this->drift_rotation_to_distance = param.as_double(); - break; - case hash("drift_distance_to_rotation"): - this->drift_distance_to_rotation = param.as_double(); - break; - case hash("drift_rotation_to_rotation"): - this->drift_rotation_to_rotation = param.as_double(); - break; - case hash("max_rotation"): - this->max_rotation = param.as_double(); - break; - case hash("max_translation"): - this->max_translation = param.as_double(); - break; - case hash("min_weight"): - this->min_weight = param.as_double(); - break; - case hash("min_resampling_weight"): - this->min_resampling_weight = param.as_double(); - break; - case hash("out_of_field_weight_decrease"): - this->out_of_field_weight_decrease = param.as_double(); - break; - case hash("out_of_field_range"): - this->out_of_field_range = param.as_double(); - break; - case hash("percentage_best_particles"): - this->percentage_best_particles = param.as_int(); - break; - case hash("distance_factor"): - this->distance_factor = param.as_double(); - break; - case hash("lines_factor"): - this->lines_factor = param.as_double(); - break; - case hash("goals_factor"): - this->goals_factor = param.as_double(); - break; - case hash("field_boundary_factor"): - this->field_boundary_factor = param.as_double(); - break; - case hash("line_element_confidence"): - this->line_element_confidence = param.as_double(); - break; - case hash("goal_element_confidence"): - this->goal_element_confidence = param.as_double(); - break; - case hash("field_boundary_element_confidence"): - this->field_boundary_element_confidence = param.as_double(); - break; - case hash("min_motion_linear"): - this->min_motion_linear = param.as_double(); - break; - case hash("min_motion_angular"): - this->min_motion_angular = param.as_double(); - break; - case hash("filter_only_with_motion"): - this->filter_only_with_motion = param.as_bool(); - break; - case hash("measurement_out_of_map_punishment"): - this->measurement_out_of_map_punishment = param.as_double(); - break; - case hash("field_x"): - this->field_x = param.as_double(); - break; - case hash("field_y"): // field params from blckboard TODO - this->field_y = param.as_double(); - break; - case hash("field_padding"): - this->field_padding = param.as_double(); - break; - case hash("initial_robot_x1"): - this->initial_robot_x1 = param.as_double(); - break; - case hash("initial_robot_y1"): - this->initial_robot_y1 = param.as_double(); - break; - case hash("initial_robot_t1"): - this->initial_robot_t1 = param.as_double(); - break; - case hash("initial_robot_x2"): - this->initial_robot_x2 = param.as_double(); - break; - case hash("initial_robot_y2"): - this->initial_robot_y2 = param.as_double(); - break; - case hash("initial_robot_t2"): - this->initial_robot_t2 = param.as_double(); - break; - case hash("initial_robot_x"): - this->initial_robot_x = param.as_double(); - break; - case hash("initial_robot_y"): - this->initial_robot_y = param.as_double(); - break; - case hash("initial_robot_t"): - this->initial_robot_t = param.as_double(); - break; - } - } -} -} // namespace bitbots_localization diff --git a/bitbots_localization/src/localization.cpp b/bitbots_localization/src/localization.cpp index 5cfcc706..131f746a 100644 --- a/bitbots_localization/src/localization.cpp +++ b/bitbots_localization/src/localization.cpp @@ -6,9 +6,9 @@ namespace bitbots_localization { Localization::Localization() - : Node("bitbots_localization", - rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides( - true)), + : Node("bitbots_localization"), + param_listener_(get_node_parameters_interface()), + config_(param_listener_.get_params()), tfBuffer(std::make_unique(this->get_clock())), tfListener(std::make_shared(*tfBuffer, this)), br(std::make_shared(this)) { @@ -16,8 +16,9 @@ Localization::Localization() std::this_thread::sleep_for(std::chrono::milliseconds(1000)); while (true) { try { - previousOdomTransform_ = tfBuffer->lookupTransform(odom_frame_, base_footprint_frame_, rclcpp::Time(0), - rclcpp::Duration::from_nanoseconds(1e9 * 20.0)); + previousOdomTransform_ = + tfBuffer->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, rclcpp::Time(0), + rclcpp::Duration::from_nanoseconds(1e9 * 20.0)); break; } catch (const tf2::LookupException &ex) { RCLCPP_INFO(this->get_logger(), "Transforms not available, waiting for them... \n %s", ex.what()); @@ -26,37 +27,22 @@ Localization::Localization() } RCLCPP_INFO(this->get_logger(), "Transforms are available now"); - auto parameters = this->get_parameters(this->list_parameters({}, 10).names); - - config_ = std::make_shared(parameters); - - Localization::onSetParameters(parameters); - - param_callback_handle_ = this->add_on_set_parameters_callback(std::bind(&Localization::onSetParameters, this, _1)); -} - -rcl_interfaces::msg::SetParametersResult Localization::onSetParameters( - const std::vector ¶meters) { - config_->update_params(parameters); - - odom_frame_ = this->get_parameter("odom_frame").as_string(); - base_footprint_frame_ = this->get_parameter("base_footprint_frame").as_string(); - map_frame_ = this->get_parameter("map_frame").as_string(); - publishing_frame_ = this->get_parameter("publishing_frame").as_string(); - + // Init subscribers line_point_cloud_subscriber_ = this->create_subscription( - config_->line_pointcloud_topic, 1, std::bind(&Localization::LinePointcloudCallback, this, _1)); + config_.ros.line_pointcloud_topic, 1, std::bind(&Localization::LinePointcloudCallback, this, _1)); + goal_subscriber_ = this->create_subscription( - config_->goal_topic, 1, std::bind(&Localization::GoalPostsCallback, this, _1)); + config_.ros.goal_topic, 1, std::bind(&Localization::GoalPostsCallback, this, _1)); + fieldboundary_subscriber_ = this->create_subscription( - config_->fieldboundary_topic, 1, std::bind(&Localization::FieldboundaryCallback, this, _1)); + config_.ros.fieldboundary_topic, 1, std::bind(&Localization::FieldboundaryCallback, this, _1)); rviz_initial_pose_subscriber_ = this->create_subscription( "initialpose", 1, std::bind(&Localization::SetInitialPositionCallback, this, _1)); + // Init publishers pose_particles_publisher_ = - this->create_publisher(config_->particle_publishing_topic, 1); - + this->create_publisher(config_.ros.particle_publishing_topic, 1); pose_with_covariance_publisher_ = this->create_publisher("pose_with_covariance", 1); lines_publisher_ = this->create_publisher("lines", 1); @@ -67,92 +53,124 @@ rcl_interfaces::msg::SetParametersResult Localization::onSetParameters( field_publisher_ = this->create_publisher( "field/map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); - // Get field name - auto field = this->get_parameter("fieldname").as_string(); + // Set the measurement timestamps to 0 + line_pointcloud_relative_.header.stamp = rclcpp::Time(0); + goal_posts_relative_.header.stamp = rclcpp::Time(0); + fieldboundary_relative_.header.stamp = rclcpp::Time(0); + + // Update all things that are dependent on the parameters and + // might need to be updated during runtime later if a parameter is changed + updateParams(true); + + // Init the particles with the given distribution + RCLCPP_INFO(this->get_logger(), "Trying to initialize particle filter..."); + reset_filter(config_.misc.init_mode); + + // Init services that can be called from outside + reset_service_ = this->create_service( + "reset_localization", std::bind(&Localization::reset_filter_callback, this, _1, _2)); + pause_service_ = this->create_service( + "pause_localization", std::bind(&Localization::set_paused_callback, this, _1, _2)); + + // Start the timer that runs the filter + publishing_timer_ = + rclcpp::create_timer(this, this->get_clock(), rclcpp::Duration(0, uint32_t(1.0e9 / config_.particle_filter.rate)), + std::bind(&Localization::run_filter_one_step, this)); +} + +// TODO handle parameter reloading +void Localization::updateParams(bool force_reload) { + // Check if we don't need to update the parameters + if (!force_reload and !param_listener_.is_old(config_)) { + return; + } + + RCLCPP_INFO(this->get_logger(), "Updating parameters..."); + + // Update parameters + param_listener_.refresh_dynamic_parameters(); + config_ = param_listener_.get_params(); // Check if measurement type is used and load the correct map for that - if (config_->lines_factor) { - lines_.reset(new Map(field, "lines.png", -10.0)); // TODO real parameter - // Publish the map once - field_publisher_->publish(lines_->get_map_msg(map_frame_)); + if (config_.particle_filter.scoring.lines_factor) { + lines_.reset(new Map(config_.field.name, "lines.png", -10.0)); + // Publish the line map once + field_publisher_->publish(lines_->get_map_msg(config_.ros.map_frame)); } - if (config_->goals_factor) { - goals_.reset(new Map(field, "goals.png", -10.0)); + if (config_.particle_filter.scoring.goal_factor) { + goals_.reset(new Map(config_.field.name, "goals.png", -10.0)); } - if (config_->field_boundary_factor) { - field_boundary_.reset(new Map(field, "field_boundary.png", -10.0)); + if (config_.particle_filter.scoring.field_boundary_factor) { + field_boundary_.reset(new Map(config_.field.name, "field_boundary.png", -10.0)); } - line_pointcloud_relative_.header.stamp = rclcpp::Time(0); - goal_posts_relative_.header.stamp = rclcpp::Time(0); - fieldboundary_relative_.header.stamp = rclcpp::Time(0); - + // Init observation model robot_pose_observation_model_.reset(new RobotPoseObservationModel(lines_, goals_, field_boundary_, config_)); - robot_pose_observation_model_->set_min_weight(config_->min_weight); + robot_pose_observation_model_->set_min_weight(config_.particle_filter.weighting.min_weight); + // Init motion model + auto drift_config = config_.particle_filter.drift; Eigen::Matrix drift_cov; drift_cov << // Standard dev of applied drift related to // distance, rotation - config_->drift_distance_to_direction, - config_->drift_rotation_to_direction, config_->drift_distance_to_distance, config_->drift_rotation_to_distance, - config_->drift_distance_to_rotation, config_->drift_rotation_to_rotation; + drift_config.distance_to_direction, + drift_config.rotation_to_direction, drift_config.distance_to_distance, drift_config.rotation_to_distance, + drift_config.distance_to_rotation, drift_config.rotation_to_rotation; // Scale drift form drift per second to drift per filter iteration - drift_cov /= config_->publishing_frequency; + drift_cov /= config_.particle_filter.rate; - drift_cov.col(0) *= (1 / (config_->max_translation / config_->publishing_frequency)); - drift_cov.col(1) *= (1 / (config_->max_rotation / config_->publishing_frequency)); + // Scale drift + drift_cov.col(0) *= (1 / (config_.particle_filter.drift.max_translation / config_.particle_filter.rate)); + drift_cov.col(1) *= (1 / (config_.particle_filter.drift.max_rotation / config_.particle_filter.rate)); - robot_motion_model_.reset(new RobotMotionModel(random_number_generator_, config_->diffusion_x_std_dev, - config_->diffusion_y_std_dev, config_->diffusion_t_std_dev, - config_->starting_diffusion, drift_cov)); + // Create the motion model which updates the particles with the odometry data and adds noise to the different states + robot_motion_model_.reset(new RobotMotionModel(random_number_generator_, config_.particle_filter.diffusion.x_std_dev, + config_.particle_filter.diffusion.y_std_dev, + config_.particle_filter.diffusion.t_std_dev, + config_.particle_filter.diffusion.starting_multiplier, drift_cov)); + // Create standard particle probability distributions (e.g. for the initialization at the start of the game) robot_state_distribution_start_left_.reset(new RobotStateDistributionStartLeft( - random_number_generator_, std::make_pair(config_->field_x, config_->field_y))); - + random_number_generator_, std::make_pair(config_.field.size.x, config_.field.size.y))); robot_state_distribution_start_right_.reset(new RobotStateDistributionStartRight( - random_number_generator_, std::make_pair(config_->field_x, config_->field_y))); - - robot_state_distribution_left_half_.reset( - new RobotStateDistributionLeftHalf(random_number_generator_, std::make_pair(config_->field_x, config_->field_y))); - + random_number_generator_, std::make_pair(config_.field.size.x, config_.field.size.y))); + robot_state_distribution_left_half_.reset(new RobotStateDistributionLeftHalf( + random_number_generator_, std::make_pair(config_.field.size.x, config_.field.size.y))); robot_state_distribution_right_half_.reset(new RobotStateDistributionRightHalf( - random_number_generator_, std::make_pair(config_->field_x, config_->field_y))); - + random_number_generator_, std::make_pair(config_.field.size.x, config_.field.size.y))); robot_state_distribution_position_.reset( - new RobotStateDistributionPosition(random_number_generator_, config_->initial_robot_x, config_->initial_robot_y)); - + new RobotStateDistributionPosition(random_number_generator_, config_.field.initialization.single_pose.x, + config_.field.initialization.single_pose.y)); robot_state_distribution_pose_.reset(new RobotStateDistributionPose( - random_number_generator_, config_->initial_robot_x, config_->initial_robot_y, config_->initial_robot_t)); - - resampling_.reset(new pf::ImportanceResampling(true, config_->min_resampling_weight)); - - RCLCPP_INFO(this->get_logger(), "Trying to initialize particle filter..."); - reset_filter(config_->init_mode); - - if (first_configuration_) { - first_configuration_ = false; - reset_service_ = this->create_service( - "reset_localization", std::bind(&Localization::reset_filter_callback, this, _1, _2)); - pause_service_ = this->create_service( - "pause_localization", std::bind(&Localization::set_paused_callback, this, _1, _2)); + random_number_generator_, config_.field.initialization.single_pose.x, config_.field.initialization.single_pose.y, + config_.field.initialization.single_pose.t)); + + // Create the resampling strategy + resampling_.reset( + new pf::ImportanceResampling(true, config_.particle_filter.weighting.particle_reset_weight)); + + // Check if we need to create a new particle filter or if we can update the existing one (keeping the particle states) + if (!robot_pf_) { + // Create new particle filter + robot_pf_.reset(new particle_filter::ParticleFilter( + config_.particle_filter.particle_number, robot_pose_observation_model_, robot_motion_model_)); + } else { + // Update particle filter's components + robot_pf_->setResamplingStrategy(resampling_); + robot_pf_->setObservationModel(robot_pose_observation_model_); + robot_pf_->setMovementModel(robot_motion_model_); } - - publishing_timer_ = rclcpp::create_timer(this, this->get_clock(), - rclcpp::Duration(0, uint32_t(1.0e9 / config_->publishing_frequency)), - std::bind(&Localization::run_filter_one_step, this)); - - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - - return result; } void Localization::run_filter_one_step() { timer_callback_count_++; resampled_ = false; + // Check for new parameters and recreate necessary components if needed + updateParams(); + // Set the measurements in the observation model updateMeasurements(); @@ -161,11 +179,11 @@ void Localization::run_filter_one_step() { // Drops the diffusion noise back to normal if it was bumped by a reset/init. // Increasing the noise helps with the initial localization. - if (timer_callback_count_ > config_->starting_steps_with_higher_diffusion) { - robot_motion_model_->diffuse_multiplicator_ = config_->diffusion_multiplicator; + if (timer_callback_count_ > config_.particle_filter.diffusion.starting_steps_with_higher_diffusion) { + robot_motion_model_->diffuse_multiplier_ = config_.particle_filter.diffusion.multiplier; } - if ((config_->filter_only_with_motion and robot_moved) or (!config_->filter_only_with_motion)) { + if ((config_.misc.filter_only_with_motion and robot_moved) or (!config_.misc.filter_only_with_motion)) { robot_pf_->drift(linear_movement_, rotational_movement_); robot_pf_->diffuse(); } @@ -174,7 +192,7 @@ void Localization::run_filter_one_step() { robot_pf_->measure(); // Check if its resampling time! - if (timer_callback_count_ % config_->resampling_interval == 0) { + if (timer_callback_count_ % config_.particle_filter.resampling_interval == 0) { robot_pf_->resample(); resampled_ = true; } @@ -183,7 +201,7 @@ void Localization::run_filter_one_step() { // Publish covariance message publish_pose_with_covariance(); // Publish debug stuff - if (config_->debug_visualization) { + if (config_.ros.debug_visualization) { publish_debug(); } @@ -197,7 +215,7 @@ void Localization::GoalPostsCallback(const sv3dm::msg::GoalpostArray &msg) { goa void Localization::FieldboundaryCallback(const sv3dm::msg::FieldBoundary &msg) { fieldboundary_relative_ = msg; } void Localization::SetInitialPositionCallback(const gm::msg::PoseWithCovarianceStamped &msg) { // Transform the given pose to map frame - auto pose_in_map = tfBuffer->transform(msg, map_frame_, tf2::durationFromSec(1.0)); + auto pose_in_map = tfBuffer->transform(msg, config_.ros.map_frame, tf2::durationFromSec(1.0)); // Get yaw from quaternion double yaw = tf2::getYaw(pose_in_map.pose.pose.orientation); @@ -232,13 +250,13 @@ bool Localization::reset_filter_callback(const std::shared_ptrget_logger(), "reset filter"); - robot_pf_.reset(new particle_filter::ParticleFilter(config_->particle_number, + robot_pf_.reset(new particle_filter::ParticleFilter(config_.particle_filter.particle_number, robot_pose_observation_model_, robot_motion_model_)); timer_callback_count_ = 0; // Increasing the noise helps with the initial localization. - robot_motion_model_->diffuse_multiplicator_ = config_->starting_diffusion; + robot_motion_model_->diffuse_multiplier_ = config_.particle_filter.diffusion.starting_multiplier; robot_pf_->setResamplingStrategy(resampling_); if (distribution == 0) { @@ -255,7 +273,7 @@ void Localization::reset_filter(int distribution) { } void Localization::reset_filter(int distribution, double x, double y) { - robot_pf_.reset(new particle_filter::ParticleFilter(config_->particle_number, + robot_pf_.reset(new particle_filter::ParticleFilter(config_.particle_filter.particle_number, robot_pose_observation_model_, robot_motion_model_)); robot_pf_->setResamplingStrategy(resampling_); @@ -267,7 +285,7 @@ void Localization::reset_filter(int distribution, double x, double y) { } void Localization::reset_filter(int distribution, double x, double y, double angle) { - robot_pf_.reset(new particle_filter::ParticleFilter(config_->particle_number, + robot_pf_.reset(new particle_filter::ParticleFilter(config_.particle_filter.particle_number, robot_pose_observation_model_, robot_motion_model_)); robot_pf_->setResamplingStrategy(resampling_); @@ -280,13 +298,14 @@ 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_->lines_factor) { + if (line_pointcloud_relative_.header.stamp != last_stamp_lines_pc && config_.particle_filter.scoring.lines_factor) { robot_pose_observation_model_->set_measurement_lines_pc(line_pointcloud_relative_); } - if (config_->goals_factor && goal_posts_relative_.header.stamp != last_stamp_goals) { + if (config_.particle_filter.scoring.goal_factor && goal_posts_relative_.header.stamp != last_stamp_goals) { robot_pose_observation_model_->set_measurement_goalposts(goal_posts_relative_); } - if (config_->field_boundary_factor && fieldboundary_relative_.header.stamp != last_stamp_fb_points) { + if (config_.particle_filter.scoring.field_boundary_factor && + fieldboundary_relative_.header.stamp != last_stamp_fb_points) { robot_pose_observation_model_->set_measurement_field_boundary(fieldboundary_relative_); } @@ -304,7 +323,8 @@ void Localization::getMotion() { try { // Get current odometry transform - transformStampedNow = tfBuffer->lookupTransform(odom_frame_, base_footprint_frame_, rclcpp::Time(0)); + transformStampedNow = + tfBuffer->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, rclcpp::Time(0)); // Get linear movement from odometry transform and the transform of the previous filter step double global_diff_x, global_diff_y; @@ -326,8 +346,8 @@ void Localization::getMotion() { tf2::getYaw(transformStampedNow.transform.rotation) - tf2::getYaw(previousOdomTransform_.transform.rotation); // Check if robot moved - if (linear_movement_.x > config_->min_motion_linear or linear_movement_.y > config_->min_motion_linear or - rotational_movement_.z > config_->min_motion_angular) { + if (linear_movement_.x > config_.misc.min_motion_linear or linear_movement_.y > config_.misc.min_motion_linear or + rotational_movement_.z > config_.misc.min_motion_angular) { robot_moved = true; } @@ -341,7 +361,7 @@ void Localization::getMotion() { void Localization::publish_transforms() { // get estimate and covariance - auto estimate = robot_pf_->getBestXPercentEstimate(config_->percentage_best_particles); + auto estimate = robot_pf_->getBestXPercentEstimate(config_.misc.percentage_best_particles); // Convert to tf2 tf2::Transform filter_transform; @@ -353,8 +373,9 @@ void Localization::publish_transforms() { // Get the transform from the last measurement timestamp until now geometry_msgs::msg::TransformStamped odomDuringMeasurement, odomNow; try { - odomDuringMeasurement = tfBuffer->lookupTransform(odom_frame_, base_footprint_frame_, last_stamp_all_measurements); - odomNow = tfBuffer->lookupTransform(odom_frame_, base_footprint_frame_, rclcpp::Time(0)); + odomDuringMeasurement = tfBuffer->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, + last_stamp_all_measurements); + odomNow = tfBuffer->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, rclcpp::Time(0)); } catch (const tf2::TransformException &ex) { RCLCPP_WARN(this->get_logger(), "Could not acquire odom transforms: %s", ex.what()); } @@ -372,7 +393,7 @@ void Localization::publish_transforms() { estimate_.setXPos(filter_transform.getOrigin().x()); estimate_.setYPos(filter_transform.getOrigin().y()); estimate_.setTheta(tf2::getYaw(filter_transform.getRotation())); - estimate_cov_ = robot_pf_->getCovariance(config_->percentage_best_particles); + estimate_cov_ = robot_pf_->getCovariance(config_.misc.percentage_best_particles); ////////////////////// // publish transforms// @@ -382,8 +403,8 @@ void Localization::publish_transforms() { // Publish localization tf, not the odom offset geometry_msgs::msg::TransformStamped localization_transform; localization_transform.header.stamp = odomNow.header.stamp; - localization_transform.header.frame_id = map_frame_; - localization_transform.child_frame_id = publishing_frame_; + localization_transform.header.frame_id = config_.ros.map_frame; + localization_transform.child_frame_id = config_.ros.publishing_frame; localization_transform.transform.translation.x = estimate_.getXPos(); localization_transform.transform.translation.y = estimate_.getYPos(); localization_transform.transform.translation.z = 0.0; @@ -405,8 +426,8 @@ void Localization::publish_transforms() { geometry_msgs::msg::TransformStamped map_odom_transform; map_odom_transform.header.stamp = this->get_clock()->now(); - map_odom_transform.header.frame_id = map_frame_; - map_odom_transform.child_frame_id = odom_frame_; + map_odom_transform.header.frame_id = config_.ros.map_frame; + map_odom_transform.child_frame_id = config_.ros.odom_frame; // Calculate odom offset tf2::Transform odom_transform_tf, localization_transform_tf, map_tf; @@ -447,7 +468,7 @@ void Localization::publish_pose_with_covariance() { estimateMsg.pose.covariance[i] = estimate_cov_[i]; } - estimateMsg.header.frame_id = map_frame_; + estimateMsg.header.frame_id = config_.ros.map_frame; pose_with_covariance_publisher_->publish(estimateMsg); } @@ -468,21 +489,21 @@ void Localization::publish_particle_markers() { red.a = 1; pose_particles_publisher_->publish(robot_pf_->renderMarkerArray( - "pose_marker", map_frame_, rclcpp::Duration::from_nanoseconds(1e9), red, this->get_clock()->now())); + "pose_marker", config_.ros.map_frame, rclcpp::Duration::from_nanoseconds(1e9), red, this->get_clock()->now())); } void Localization::publish_ratings() { - if (config_->lines_factor) { + if (config_.particle_filter.scoring.lines_factor) { // Publish line ratings publish_debug_rating(robot_pose_observation_model_->get_measurement_lines(), 0.1, "line_ratings", lines_, line_ratings_publisher_); } - if (config_->goals_factor) { + if (config_.particle_filter.scoring.goal_factor) { // Publish goal ratings publish_debug_rating(robot_pose_observation_model_->get_measurement_goals(), 0.2, "goal_ratings", goals_, goal_ratings_publisher_); } - if (config_->field_boundary_factor) { + if (config_.particle_filter.scoring.field_boundary_factor) { // Publish field boundary ratings publish_debug_rating(robot_pose_observation_model_->get_measurement_field_boundary(), 0.2, "field_boundary_ratings", field_boundary_, fieldboundary_ratings_publisher_); @@ -492,10 +513,10 @@ void Localization::publish_ratings() { void Localization::publish_debug_rating(std::vector> measurements, double scale, const char name[], std::shared_ptr map, rclcpp::Publisher::SharedPtr &publisher) { - RobotState best_estimate = robot_pf_->getBestXPercentEstimate(config_->percentage_best_particles); + RobotState best_estimate = robot_pf_->getBestXPercentEstimate(config_.misc.percentage_best_particles); visualization_msgs::msg::Marker marker; - marker.header.frame_id = map_frame_; + marker.header.frame_id = config_.ros.map_frame; marker.header.stamp = this->get_clock()->now(); marker.ns = name; marker.action = visualization_msgs::msg::Marker::ADD; diff --git a/bitbots_localization/src/parameters.yml b/bitbots_localization/src/parameters.yml new file mode 100644 index 00000000..4476dcc4 --- /dev/null +++ b/bitbots_localization/src/parameters.yml @@ -0,0 +1,241 @@ +bitbots_localization: + misc: + init_mode: + type: int + description: "Initialization mode for the filters particle distribution" + validation: + bounds<>: [0, 4] + percentage_best_particles: + type: int + description: "Percentage of particles which are considered to be the best ones. These particles are used to calculate the pose estimate" + validation: + bounds<>: [0, 100] + min_motion_linear: + type: double + description: "Minimum linear motion which is considered to be a movement. This is relevant if we want to deactivate the filter if no movement is detected" + validation: + bounds<>: [0.0, 1.0] + min_motion_angular: + type: double + description: "Minimum angular motion which is considered to be a movement. This is relevant if we want to deactivate the filter if no movement is detected" + validation: + bounds<>: [0.0, 1.0] + filter_only_with_motion: + type: bool + description: "If true, the filter is only active if a movement is detected" + ros: + line_pointcloud_topic: + type: string + description: "Topic for the line pointcloud input messages" + read_only: true + goal_topic: + type: string + description: "Topic for the goal input messages" + read_only: true + fieldboundary_topic: + type: string + description: "Topic for the field boundary input messages" + read_only: true + particle_publishing_topic: + type: string + description: "Topic for the particle publishing messages" + read_only: true + debug_visualization: + type: bool + description: "Activate debug visualization" + odom_frame: + type: string + description: "The odometry frame name" + read_only: true + base_footprint_frame: + type: string + description: "The base footprint frame name" + read_only: true + map_frame: + type: string + description: "The map frame name" + read_only: true + publishing_frame: + type: string + description: "The frame which is used for publishing the pose estimate" + read_only: true + particle_filter: + particle_number: + type: int + description: "Number of particles" + read_only: true + validation: + bounds<>: [1, 10000] + rate: + type: int + description: "Frequency at which the filter is updated" + read_only: true + validation: + bounds<>: [1, 200] + resampling_interval: + type: int + description: "Number of steps after which resampling is performed" + validation: + gt<>: [0] + diffusion: + x_std_dev: + type: double + description: "Standard deviation for the diffusion in x direction" + validation: + gt_eq<>: [0.0] + y_std_dev: + type: double + description: "Standard deviation for the diffusion in y direction" + validation: + gt_eq<>: [0.0] + t_std_dev: + type: double + description: "Standard deviation for the diffusion in theta (rotation) direction" + validation: + gt_eq<>: [0.0] + multiplier: + type: double + description: "Scaling multiplier for the diffusion (hacky way to change the diffusion strength)" + validation: + gt_eq<>: [0.0] + starting_multiplier: + type: double + description: "Starting diffusion multiplier value. This can be used to increase the diffusion at after initialization, which can help with initial exploration and convergence" + validation: + gt_eq<>: [0.0] + starting_steps_with_higher_diffusion: + type: int + description: "Number of filter steps in which the value of the starting_multiplier is used." + validation: + gt_eq<>: [0] + drift: + distance_to_direction: + type: double + description: "Relationship between the moved distance and the noise which is added to the direction of movement (!= rotation as our robot is holonomic)" + validation: + gt_eq<>: [0.0] + rotation_to_direction: + type: double + description: "Relationship between the rotational movement of the robot and the noise which is added to its direction of movement estimates" + validation: + gt_eq<>: [0.0] + distance_to_distance: + type: double + description: "Relationship between the moved distance and the noise which is added to the distance estimates" + validation: + gt_eq<>: [0.0] + rotation_to_distance: + type: double + description: "Relationship between the rotational movement of the robot and the noise which is added to the distance estimates" + validation: + gt_eq<>: [0.0] + distance_to_rotation: + type: double + description: "Relationship between the moved distance and the noise which is added to the rotation estimates" + validation: + gt_eq<>: [0.0] + rotation_to_rotation: + type: double + description: "Relationship between the rotational movement of the robot and the noise which is added to the rotation estimates" + validation: + gt_eq<>: [0.0] + max_rotation: + type: double + description: "Maximum rotation which is expected for a single step" + validation: + gt_eq<>: [0.0] + max_translation: + type: double + description: "Maximum translation which is expected for a single step" + validation: + gt_eq<>: [0.0] + weighting: + min_weight: + type: double + description: "Minimum possible weight for a particle. Values below this value are clamped to this value" + validation: + bounds<>: [0.0, 1.0] + particle_reset_weight: + type: double + description: "Weight which is set for a particle after resampling" + validation: + bounds<>: [0.0, 1.0] + out_of_field_weight_decrease: + type: double + description: "Weight decrease for particles which are out of the field" + validation: + bounds<>: [0.0, 1.0] + out_of_field_range: + type: double + description: "Distance (m) from the field border in which particles are considered to be out of the field" + validation: + bounds<>: [-5.0, 5.0] + scoring: + lines_factor: + type: double + description: "Weighing how much the line information is considered for the scoring of a particle" + validation: + bounds<>: [0.0, 1.0] + goal_factor: + type: double + description: "Weighing how much the goal information is considered for the scoring of a particle" + validation: + bounds<>: [0.0, 1.0] + field_boundary_factor: + type: double + description: "Weighing how much the field boundary information is considered for the scoring of a particle" + validation: + bounds<>: [0.0, 1.0] + confidences: + line_element: + type: double + description: "Confidence we have in each data point of the line information. Meaning each projected line pixel" + validation: + bounds<>: [0.0, 1.0] + goal_element: + type: double + description: "Confidence we have in each data point of the goal information. Meaning each projected goal post" + validation: + bounds<>: [0.0, 1.0] + field_boundary_element: + type: double + description: "Confidence we have in each data point of the field boundary information. Meaning each projected field boundary segment" + validation: + bounds<>: [0.0, 1.0] + field: + name: + type: string + description: "Name of the field" + read_only: true + size: + x: + type: double + description: "Size of the field in x direction" + read_only: true + validation: + gt_eq<>: [0.0] + y: + type: double + description: "Size of the field in y direction" + read_only: true + validation: + gt_eq<>: [0.0] + padding: + type: double + description: "Padding (area outside of the lines) of the field in all directions" + read_only: true + validation: + gt_eq<>: [0.0] + initialization: + single_pose: + x: + type: double + description: "Initial x position of the robot" + y: + type: double + description: "Initial y position of the robot" + t: + type: double + description: "Initial rotation of the robot" + validation: + bounds<>: [-3.14159265359, 3.14159265359]