Use generate_parameter_library and cleanup parameters

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})

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
+
+
+
+

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
+

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
+

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

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