update multi_floor_manager to configure ublox_converter parameters by…

… map_config file

Signed-off-by: Masayuki Murata <[email protected]>

mf_localization/script/multi_floor_manager.py

@@ -95,6 +95,8 @@
 import std_msgs.msg
 from cabot_msgs.srv import LookupTransform
 
+from ublox_converter import UbloxConverterNode
+
 
 def json2anchor(jobj):
     return geoutil.Anchor(lat=jobj["lat"],
@@ -2083,6 +2085,7 @@ def transform(self, pose_stamped, target, timeout=None):
     broadcaster = tf2_ros.TransformBroadcaster(node)
     tfBuffer = BufferProxy(node)
 
+    # multi floor manager
     multi_floor_manager = MultiFloorManager(node)
     # load node parameters
     configuration_directory_raw = node.declare_parameter("configuration_directory", '').value
@@ -2150,6 +2153,18 @@ def transform(self, pose_stamped, target, timeout=None):
 
     # current_publisher = CurrentPublisher(node, verbose=verbose)
 
+    # ublox converter
+    ublox_converter_config = {
+                                "min_cno": node.declare_parameter("ublox_converter.min_cno", 30).value,
+                                "min_elev": node.declare_parameter("ublox_converter.min_elev", 15).value,
+                                "num_sv_threshold_low": node.declare_parameter("ublox_converter.num_sv_threshold_low", 5).value,
+                                "num_sv_threshold_high": node.declare_parameter("ublox_converter.num_sv_threshold_high", 10).value,
+                            }
+    map_ublox_converter_config = map_config.get("ublox_converter", {})
+    ublox_converter_config.update(map_ublox_converter_config)  # update if defined in map_config
+    ublox_converter = UbloxConverterNode.from_dict(node, ublox_converter_config)
+    logger.info(F"ublox_converter = {ublox_converter}")
+
     # configuration file check
     configuration_directory = configuration_directory_raw  # resource_utils.get_filename(configuration_directory_raw)
     temporary_directory = os.path.join(configuration_directory, temporary_directory_name)

mf_localization/script/ublox_converter.py

@@ -55,8 +55,12 @@ def convert_count_to_status(self, count):
         else:
             return MFNavSAT.STATUS_ACTIVE
 
+    def __repr__(self):
+        return f"{self.__class__.__name__}({vars(self)})"
+
 
 class UbloxConverterNode:
+
     def __init__(self, node, min_cno, min_elev, num_sv_threshold_low, num_sv_threshold_high):
         self.node = node
         self.ublox_converter = UbloxConverter(min_cno, min_elev, num_sv_threshold_low, num_sv_threshold_high)
@@ -65,6 +69,15 @@ def __init__(self, node, min_cno, min_elev, num_sv_threshold_low, num_sv_thresho
         self.status_pub = self.node.create_publisher(Int8, "sv_status", 10)
         self.mf_navsat_pub = self.node.create_publisher(MFNavSAT, "mf_navsat", 10)
 
+    @classmethod
+    def from_dict(cls, node, data):
+        min_cno = data.get("min_cno", 30)
+        min_elev = data.get("min_elev", 15)
+        num_sv_threshold_low = data.get("num_sv_threshold_low", 5)
+        num_sv_threshold_high = data.get("num_sv_threshold_high", 10)
+        node.get_logger().info(f"data = {data}")
+        return cls(node, min_cno, min_elev, num_sv_threshold_low, num_sv_threshold_high)
+
     def navsat_callback(self, msg: NavSAT):
         num_sv = msg.num_svs
 
@@ -85,6 +98,9 @@ def navsat_callback(self, msg: NavSAT):
         mf_navsat_msg.sv_status = sv_status
         self.mf_navsat_pub.publish(mf_navsat_msg)
 
+    def __repr__(self):
+        return repr(self.ublox_converter)
+
 
 def main():
     rclpy.init()

mf_localization_mapping/launch/demo_multi_2d_VLP16_rss_localization.launch.py

@@ -110,6 +110,10 @@ def configure_ros2_bag_play(context, node):
             'current_mode:=current_mode_temp',
             'localize_status:=localize_status_temp',
             'map:=map_temp',
+            # ublox_converter
+            'mf_navsat:=mf_navsat_temp',
+            'ublox_converter/num_active_sv:=ublox_converter/num_active_sv_temp',
+            'ublox_converter/sv_status:=ublox_converter/sv_status_temp',
         ])
         if convert_points.perform(context) == 'true':
             cmd.append([points2, ':=', points2_temp])
@@ -219,18 +223,18 @@ def configure_ros2_bag_play(context, node):
             name="lookup_transform_service_node",
         ),
 
-        # run ublox_converter
-        Node(
-            package='mf_localization',
-            executable='ublox_converter.py',
-            name='ublox_converter',
-            parameters=[PathJoinSubstitution([mf_localization_dir, 'configuration_files', 'ublox/ublox_converter.yaml'])],
-            remappings=[
-                ('navsat', 'ublox/navsat'),
-                ('num_active_sv', 'ublox_converter/num_active_sv'),
-                ('sv_status', 'ublox_converter/sv_status')
-            ],
-        ),
+        # [deprecated] run ublox_converter
+        # Node(
+        #     package='mf_localization',
+        #     executable='ublox_converter.py',
+        #     name='ublox_converter',
+        #     parameters=[PathJoinSubstitution([mf_localization_dir, 'configuration_files', 'ublox/ublox_converter.yaml'])],
+        #     remappings=[
+        #         ('navsat', 'ublox/navsat'),
+        #         ('num_active_sv', 'ublox_converter/num_active_sv'),
+        #         ('sv_status', 'ublox_converter/sv_status')
+        #     ],
+        # ),
 
         # run multi_floor_manager
         GroupAction([
@@ -275,6 +279,10 @@ def configure_ros2_bag_play(context, node):
                     ('pressure', pressure_topic),
                     ('gnss_fix', gnss_fix),
                     ('gnss_fix_velocity', gnss_fix_velocity),
+                    # ublox_converter
+                    ('navsat', 'ublox/navsat'),
+                    ('num_active_sv', 'ublox_converter/num_active_sv'),
+                    ('sv_status', 'ublox_converter/sv_status')
                 ],
                 output="both",
                 arguments=['--ros-args', '--log-level', PythonExpression(['"multi_floor_manager:=" + "', log_level, '"'])]