git commit -m "first commit"

simulators/third_party/dynamic_xml_config/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(dynamic_xml_config)
+find_package(catkin REQUIRED COMPONENTS
+    roscpp
+    rospy
+)
+
+catkin_package(
+    
+)
+
+catkin_install_python(PROGRAMS
+    plugins/obstacles_generate_ros.py
+    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

simulators/third_party/dynamic_xml_config/main_generate.py

@@ -0,0 +1,63 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+""" 
+******************************************************************************************
+*  Copyright (C) 2023 Yang Haodong, All Rights Reserved                                  *
+*                                                                                        *
+*  @brief    generate launch file dynamicly based on user configure.                     *
+*  @author   Haodong Yang                                                                *
+*  @version  1.0.3                                                                       *
+*  @date     2023.07.19                                                                  *
+*  @license  GNU General Public License (GPL)                                            *
+******************************************************************************************
+"""
+import xml.etree.ElementTree as ET
+from plugins import XMLGenerator, PedGenerator, RobotGenerator, ObstacleGenerator
+
+class MainGenerator(XMLGenerator):
+    def __init__(self, *plugins) -> None:
+        super().__init__()
+        self.main_path = self.root_path + "pedestrian_simulation/"
+        self.app_list = [app for app in plugins]
+    
+    def writeMainLaunch(self, path):
+        """
+        Create configure file of package `sim_env` dynamically.
+
+        Parameters
+        ----------
+        path: str
+            the path of file(.launch.xml) to write.
+        """
+        # root
+        launch = MainGenerator.createElement("launch")
+
+        # other applications
+        for app in self.app_list:
+            assert isinstance(app, XMLGenerator), "Expected type of app is XMLGenerator"
+            app_register = app.plugin()
+            for app_element in app_register:
+                launch.append(app_element)
+
+        # include
+        include = MainGenerator.createElement("include", props={"file": "$(find sim_env)/launch/config.launch"})
+        include.append(MainGenerator.createElement("arg", props={"name": "world", "value": "$(arg world_parameter)"}))
+        include.append(MainGenerator.createElement("arg", props={"name": "map", "value": self.user_cfg["map"]}))
+        include.append(MainGenerator.createElement("arg", props={"name": "robot_number", "value": str(len(self.user_cfg["robots_config"]))}))
+        include.append(MainGenerator.createElement("arg", props={"name": "rviz_file", "value": self.user_cfg["rviz_file"]}))
+
+        launch.append(include)
+        MainGenerator.indent(launch)
+
+        with open(path, "wb+") as f:
+            ET.ElementTree(launch).write(f, encoding="utf-8", xml_declaration=True)
+
+    def plugin(self):
+        pass
+
+# dynamic generator
+main_gen = MainGenerator(PedGenerator(),
+                         RobotGenerator(),
+                         ObstacleGenerator())
+main_gen.writeMainLaunch(main_gen.root_path + "sim_env/launch/main.launch")

simulators/third_party/dynamic_xml_config/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>dynamic_xml_config</name>
+  <version>0.2.0</version>
+  <description>
+    dynamic xml configure
+  </description>
+
+  <maintainer email="913982779@qq.com">Winter</maintainer>
+
+  <license>BSD</license>
+
+  <author email="913982779@qq.com">Winter</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+</package>

simulators/third_party/dynamic_xml_config/plugins/__init__.py

@@ -0,0 +1,6 @@
+from .xml_generate import XMLGenerator
+from .pedestrians_generate import PedGenerator
+from .robot_generate import RobotGenerator
+from .obstacles_generate import ObstacleGenerator
+
+__all__ = ["XMLGenerator", "PedGenerator", "RobotGenerator", "ObstacleGenerator"]

simulators/third_party/dynamic_xml_config/plugins/obstacles_generate.py

@@ -0,0 +1,191 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+""" 
+******************************************************************************************
+*  Copyright (C) 2023 Yang Haodong, All Rights Reserved                                  *
+*                                                                                        *
+*  @brief    generate launch file dynamicly based on user configure.                     *
+*  @author   Haodong Yang                                                                *
+*  @version  1.0.2                                                                       *
+*  @date     2023.03.15                                                                  *
+*  @license  GNU General Public License (GPL)                                            *
+******************************************************************************************
+"""
+import rospy, tf
+from gazebo_msgs.srv import SpawnModel
+from geometry_msgs.msg import Quaternion, Pose, Point
+import xml.etree.ElementTree as ET
+from .xml_generate import XMLGenerator
+
+
+class ObstacleGenerator(XMLGenerator):
+    def __init__(self) -> None:
+        super().__init__()
+        if "plugins" in self.user_cfg.keys() and "obstacles" in self.user_cfg["plugins"] \
+            and self.user_cfg["plugins"]["obstacles"]:
+            self.obs_cfg = ObstacleGenerator.yamlParser(self.root_path + "user_config/" + self.user_cfg["plugins"]["obstacles"])
+        else:
+            self.obs_cfg = None
+        
+        self.box_obs, self.cylinder_obs, self.sphere_obs = [], [], []
+    
+    def plugin(self):
+        """
+        Implement of obstacles application.
+        """
+        app_register = []
+        if not self.obs_cfg is None:
+            '''app register'''
+            # obstacles generation
+            obs_gen = ObstacleGenerator.createElement("node", props={"pkg": "dynamic_xml_config", "type": "obstacles_generate_ros.py",
+                "name": "obstacles_generate", "args": "user_config.yaml","output": "screen"})
+            app_register.append(obs_gen)
+        
+        return app_register
+
+    def spawn(self) -> None:
+        """
+        ROS service for spawning static obstacles in Gazebo world.
+        """
+        rospy.wait_for_service("gazebo/spawn_urdf_model")
+        proxy = rospy.ServiceProxy("gazebo/spawn_urdf_model", SpawnModel)
+
+        for obs in self.obs_cfg["obstacles"]:
+            pose = [float(i) for i in obs["pose"].split()]
+            x, y, z, r, p, yaw = pose
+            orient = tf.transformations.quaternion_from_euler(r, p, yaw)
+            orient = Quaternion(orient[0], orient[1], orient[2], orient[3])
+            params = obs["props"]
+            params["c"] = obs["color"]
+            urdf = self.constructURDF(obs["type"], **params)
+
+            if obs["type"] == "BOX":
+                z = z if z else obs["props"]["h"] / 2
+                pose = Pose(Point(x=x, y=y, z=z), orient)
+                proxy(obs["type"] + str(len(self.box_obs)), urdf, "", pose, "world")
+                self.box_obs.append(obs)
+            elif obs["type"] == "CYLINDER":
+                z = z if z else obs["props"]["h"] / 2
+                pose = Pose(Point(x=x, y=y, z=z), orient)
+                proxy(obs["type"] + str(len(self.cylinder_obs)), urdf, "", pose, "world")
+                self.cylinder_obs.append(obs)
+            elif obs["type"] == "SPHERE":
+                z = z if z else obs["props"]["r"]
+                pose = Pose(Point(x=x, y=y, z=z), orient)
+                proxy(obs["type"] + str(len(self.sphere_obs)), urdf, "", pose, "world")
+                self.sphere_obs.append(obs)
+
+    def constructURDF(self, model_type: str, **kwargs) -> str:
+        """
+        Construct URDF of specific type of obstacle.
+
+        Parameters
+        ----------
+        model_type: str
+            specific type of obstacle. Optional is `BOX`, `CYLINDER` and `SPHERE`
+        kwargs: dict
+            parameters of obstacle, such as mass, color, height, etc.
+
+        Return
+        ----------
+        urdf_str: str
+            URDF of obstacle
+        """
+        # parameters checking
+        if model_type.upper() == "BOX":
+            assert "m" in kwargs and \
+                "w" in kwargs and \
+                "d" in kwargs and \
+                "h" in kwargs and \
+                "c" in kwargs,    \
+                "Parameters of {} are `m`, `w`, `d`, `h`, `c` which mean mass, \
+                    width, depth, height and color, ".format(model_type)
+            ixx = (kwargs["m"] / 12.0) * (pow(kwargs["d"], 2) + pow(kwargs["h"], 2))
+            iyy = (kwargs["m"] / 12.0) * (pow(kwargs["w"], 2) + pow(kwargs["h"], 2))
+            izz = (kwargs["m"] / 12.0) * (pow(kwargs["w"], 2) + pow(kwargs["d"], 2))
+            geometry = ObstacleGenerator.createElement("geometry")
+            geometry.append(ObstacleGenerator.createElement(model_type.lower(),
+                props={"size": "{:f} {:f} {:f}".format(kwargs["w"], kwargs["d"], kwargs["h"])}))
+        elif model_type.upper() == "CYLINDER":
+            assert "m" in kwargs and \
+                "r" in kwargs and \
+                "h" in kwargs and \
+                "c" in kwargs,    \
+                "Parameters of {} are `m`, `r`, `h`, `c` which mean mass, \
+                    radius, height and color, ".format(model_type)
+            ixx = (kwargs["m"] / 12.0) * (3 * pow(kwargs["r"], 2) + pow(kwargs["h"], 2))
+            iyy = (kwargs["m"] / 12.0) * (3 * pow(kwargs["r"], 2) + pow(kwargs["h"], 2))
+            izz = (kwargs["m"] * pow(kwargs["r"], 2)) / 2.0
+            geometry = ObstacleGenerator.createElement("geometry")
+            geometry.append(ObstacleGenerator.createElement(model_type.lower(), 
+                props={"length": str(kwargs["h"]), "radius": str(kwargs["r"])}))
+        elif model_type.upper() == "SPHERE":
+            assert "m" in kwargs and \
+                "r" in kwargs and \
+                "c" in kwargs,    \
+                "Parameters of {} are `m`, `r`, `h`, `c` which mean mass, \
+                    radius and color, ".format(model_type)
+            ixx = (2 * kwargs["m"] * pow(kwargs["r"], 2)) / 5.0
+            iyy = (2 * kwargs["m"] * pow(kwargs["r"], 2)) / 5.0
+            izz = (2 * kwargs["m"] * pow(kwargs["r"], 2)) / 5.0
+            geometry = ObstacleGenerator.createElement("geometry")
+            geometry.append(ObstacleGenerator.createElement("sphere", props={"radius": str(kwargs["r"])}))
+        else:
+            raise NotImplementedError
+
+        # URDF generation dynamically
+        static_obs = ObstacleGenerator.createElement("robot", props={"name": model_type.lower()})
+        link = ObstacleGenerator.createElement("link", props={"name": model_type.lower() + "_link"})
+
+        inertial = ObstacleGenerator.createElement("inertial")
+        inertial.append(ObstacleGenerator.createElement("origin", props={"xyz": "0 0 0", "rpy": "0 0 0"}))
+        inertial.append(ObstacleGenerator.createElement("mass", props={"value": str(kwargs["m"])}))
+        inertial.append(ObstacleGenerator.createElement("inertia", props={"ixx": str(ixx), "ixy": "0.0", "ixz": "0.0",
+            "iyy": str(iyy), "iyz": "0.0", "izz": str(izz)}))
+
+        collision = ObstacleGenerator.createElement("collision")
+        collision.append(ObstacleGenerator.createElement("origin", props={"xyz": "0 0 0", "rpy": "0 0 0"}))
+        
+        collision.append(geometry)
+
+        visual = ObstacleGenerator.createElement("visual")
+        visual.append(ObstacleGenerator.createElement("origin", props={"xyz": "0 0 0", "rpy": "0 0 0"}))
+        visual.append(geometry)
+        r, g, b, a = ObstacleGenerator.color(kwargs["c"])
+        visual.append(ObstacleGenerator.createElement("color", props={"rgba": "{:f} {:f} {:f} {:f}".format(r, g, b, a)}))
+
+        link.append(inertial)
+        link.append(collision)
+        link.append(visual)
+
+        gazebo = ObstacleGenerator.createElement("gazebo", props={"reference": model_type.lower() + "_link"})
+        gazebo.append(ObstacleGenerator.createElement("kp", text=str(100000.0)))
+        gazebo.append(ObstacleGenerator.createElement("kd", text=str(100000.0)))
+        gazebo.append(ObstacleGenerator.createElement("mu1", text=str(10.0)))
+        gazebo.append(ObstacleGenerator.createElement("mu2", text=str(10.0)))
+        gazebo.append(ObstacleGenerator.createElement("material", text="Gazebo/" + kwargs["c"]))
+
+        static_obs.append(link)
+        static_obs.append(gazebo)
+
+        ObstacleGenerator.indent(static_obs)
+
+        return ET.tostring(static_obs).decode()
+
+    @staticmethod
+    def color(color_name):
+        if color_name == "Blue":
+            return (0, 0, 0.8, 1)
+        elif color_name == "Red":
+            return (0.8, 0, 0, 1)
+        elif color_name == "Green":
+            return (0, 0.8, 0, 1)
+        elif color_name == "Grey":
+            return (0.75, 0.75, 0.75, 1)
+        elif color_name == "White":
+            return (1.0, 1.0, 1.0, 1)
+        elif color_name == "Black":
+            return (0, 0, 0, 1)
+        else:
+            return (0.75, 0.75, 0.75, 1)

simulators/third_party/dynamic_xml_config/plugins/obstacles_generate_ros.py

@@ -0,0 +1,25 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+""" 
+******************************************************************************************
+*  Copyright (C) 2023 Yang Haodong, All Rights Reserved                                  *
+*                                                                                        *
+*  @brief    generate launch file dynamicly based on user configure.                     *
+*  @author   Haodong Yang                                                                *
+*  @version  1.0.2                                                                       *
+*  @date     2023.03.15                                                                  *
+*  @license  GNU General Public License (GPL)                                            *
+******************************************************************************************
+"""
+import rospy
+import sys, os
+sys.path.append(os.path.abspath(os.path.join(__file__, "../../")))
+
+from plugins import ObstacleGenerator
+
+if __name__ == "__main__":
+    obstacles = ObstacleGenerator()
+
+    rospy.init_node("spawn_obstacles")
+    obstacles.spawn()

simulators/third_party/dynamic_xml_config/plugins/pedestrians_generate.py

@@ -0,0 +1,185 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+""" 
+******************************************************************************************
+*  Copyright (C) 2023 Yang Haodong, All Rights Reserved                                  *
+*                                                                                        *
+*  @brief    generate launch file dynamicly based on user configure.                     *
+*  @author   Haodong Yang                                                                *
+*  @version  1.0.2                                                                       *
+*  @date     2023.03.15                                                                  *
+*  @license  GNU General Public License (GPL)                                            *
+******************************************************************************************
+"""
+import xml.etree.ElementTree as ET
+from .xml_generate import XMLGenerator
+
+
+class PedGenerator(XMLGenerator):
+    def __init__(self) -> None:
+        super().__init__()
+        if "plugins" in self.user_cfg.keys() and "pedestrians" in self.user_cfg["plugins"] \
+            and self.user_cfg["plugins"]["pedestrians"]:
+            self.ped_cfg = PedGenerator.yamlParser(self.root_path + "user_config/" + self.user_cfg["plugins"]["pedestrians"])
+        else:
+            self.ped_cfg = None
+    
+    def __str__(self) -> str:
+        return "Pedestrians Generator"
+
+    def plugin(self):
+        """
+        Implement of pedestrian application.
+        """
+        app_register = []
+        if not self.ped_cfg is None:
+            '''dynamic confiugre'''
+            ped_path = self.root_path + "sim_env/worlds/" + self.user_cfg["world"] + "_with_pedestrians.world"
+            self.writePedestrianWorld(ped_path)
+
+            '''app register'''
+            # world generation
+            ped_world = PedGenerator.createElement("arg", props={"name": "world_parameter", 
+                "value": self.user_cfg["world"] + "_with_pedestrians"})
+            app_register.append(ped_world)
+        else:
+            # world generation
+            ped_world = PedGenerator.createElement("arg", props={"name": "world_parameter", "value": self.user_cfg["world"]})
+            app_register.append(ped_world)
+        
+        return app_register
+
+    def writePedestrianWorld(self, path):
+        """
+        Create configure file to construct pedestrians environment.
+
+        Parameters
+        ----------
+        path: str
+            the path of file(.launch.xml) to write.
+        """
+
+        def createHuman(config, index):
+            """
+            Create human element of world file.
+
+            Parameters
+            ----------
+            config: dict
+                configure data structure.
+            index: int
+                human index
+
+            Return
+            ----------
+            human: ET.Element
+                human element ptr
+            """
+
+            def createCollision(name, scale, pose=None):
+                if pose:
+                    props = {"scale": " ".join("%s" % s for s in scale), "pose": " ".join("%s" % p for p in pose)}
+                else:
+                    props = {"scale": " ".join("%s" % s for s in scale)}
+                return PedGenerator.createElement("collision", text=name, props=props)
+
+            # configure
+            sfm = config["social_force"]
+            human = config["pedestrians"]["ped_property"][index]
+            upd_rate = config["pedestrians"]["update_rate"]
+
+            # root: actor
+            actor = PedGenerator.createElement("actor", props={"name": human["name"]})
+
+            # human initial pose
+            pose = PedGenerator.createElement("pose", text=human["pose"])
+
+            # human skin
+            skin = PedGenerator.createElement("skin")
+            skin.append(PedGenerator.createElement("filename", text="walk.dae"))
+            skin.append(PedGenerator.createElement("scale", text="1.0"))
+
+            animation = PedGenerator.createElement("animation", props={"name": "walking"})
+            animation.append(PedGenerator.createElement("filename", text="walk.dae"))
+            animation.append(PedGenerator.createElement("scale", text="1.0"))
+            animation.append(PedGenerator.createElement("interpolate_x", text="true"))
+
+            # plugin
+            if not index:
+                plugin_visual = PedGenerator.createElement("plugin", props={"name": "pedestrian_visual", "filename": "libPedestrianVisualPlugin.so"})
+                plugin_visual.append(PedGenerator.createElement("update_rate", text=str(upd_rate)))
+            else:
+                plugin_visual = None
+
+            plugin = PedGenerator.createElement("plugin", props={"name": human["name"] + "_plugin", "filename": "libPedestrianSFMPlugin.so"})
+            plugin.append(createCollision("LHipJoint_LeftUpLeg_collision", [0.01, 0.001, 0.001]))
+            plugin.append(createCollision("LeftUpLeg_LeftLeg_collision", [8.0, 8.0, 1.0]))
+            plugin.append(createCollision("LeftLeg_LeftFoot_collision", [10.0, 10.0, 1.5]))
+            plugin.append(createCollision("LeftFoot_LeftToeBase_collision", [4.0, 4.0, 1.5]))
+            plugin.append(createCollision("RHipJoint_RightUpLeg_collision", [0.01, 0.001, 0.001]))
+            plugin.append(createCollision("RightUpLeg_RightLeg_collision", [8.0, 8.0, 1.0]))
+            plugin.append(createCollision("RightLeg_RightFoot_collision", [10.0, 10.0, 1.5]))
+            plugin.append(createCollision("RightFoot_RightToeBase_collision", [4.0, 4.0, 1.5]))
+            plugin.append(createCollision("Spine_Spine1_collision", [0.01, 0.001, 0.001]))
+            plugin.append(createCollision("Neck_Neck1_collision", [0.01, 0.001, 0.001]))
+            plugin.append(createCollision("Neck1_Head_collision", [5.0, 5.0, 3.0]))
+            plugin.append(createCollision("LeftShoulder_LeftArm_collision", [0.01, 0.001, 0.001]))
+            plugin.append(createCollision("LeftArm_LeftForeArm_collision", [5.0, 5.0, 1.0]))
+            plugin.append(createCollision("LeftForeArm_LeftHand_collision", [5.0, 5.0, 1.0]))
+            plugin.append(createCollision("LeftFingerBase_LeftHandIndex1_collision", [4.0, 4.0, 3.0]))
+            plugin.append(createCollision("RightShoulder_RightArm_collision", [0.01, 0.001, 0.001]))
+            plugin.append(createCollision("RightArm_RightForeArm_collision", [5.0, 5.0, 1.0]))
+            plugin.append(createCollision("RightForeArm_RightHand_collision", [5.0, 5.0, 1.0]))
+            plugin.append(createCollision("RightFingerBase_RightHandIndex1_collision", [4.0, 4.0, 3.0]))
+            plugin.append(createCollision("LowerBack_Spine_collision", [12.0, 20.0, 5.0], [0.05, 0, 0, 0, -0.2, 0]))
+
+            plugin.append(PedGenerator.createElement("velocity", text=str(human["velocity"])))
+            plugin.append(PedGenerator.createElement("radius", text=str(human["radius"])))
+            plugin.append(PedGenerator.createElement("cycle", text=str(human["cycle"])))
+            plugin.append(PedGenerator.createElement("animation_factor", text=str(sfm["animation_factor"])))
+            plugin.append(PedGenerator.createElement("people_distance", text=str(sfm["people_distance"])))
+            plugin.append(PedGenerator.createElement("goal_weight", text=str(sfm["goal_weight"])))
+            plugin.append(PedGenerator.createElement("obstacle_weight", text=str(sfm["obstacle_weight"])))
+            plugin.append(PedGenerator.createElement("social_weight", text=str(sfm["social_weight"])))
+            plugin.append(PedGenerator.createElement("group_gaze_weight", text=str(sfm["group_gaze_weight"])))
+            plugin.append(PedGenerator.createElement("group_coh_weight", text=str(sfm["group_coh_weight"])))
+            plugin.append(PedGenerator.createElement("group_rep_weight", text=str(sfm["group_rep_weight"])))
+            
+            if "time_delay" in human.keys():
+                plugin.append(PedGenerator.createElement("time_delay", text=str(human["time_delay"])))
+
+            ignore_obstacles = PedGenerator.createElement("ignore_obstacles")
+            for model in human["ignore"].values():
+                ignore_obstacles.append(PedGenerator.createElement("model", text=model))
+
+            trajectory = PedGenerator.createElement("trajectory")
+            for goal in human["trajectory"].values():
+                trajectory.append(PedGenerator.createElement("goalpoint", text=goal))
+
+            plugin.append(ignore_obstacles)
+            plugin.append(trajectory)
+
+            actor.append(pose)
+            actor.append(skin)
+            actor.append(animation)
+            actor.append(plugin)
+
+            if not plugin_visual is None:
+                actor.append(plugin_visual)
+            
+            PedGenerator.indent(actor)
+
+            return actor
+
+        if not self.ped_cfg is None:
+            world_file = self.root_path + "sim_env/worlds/" + self.user_cfg["world"] + ".world"
+            tree = ET.parse(world_file)
+            world = tree.getroot().find("world")
+
+            human_num = len(self.ped_cfg["pedestrians"]["ped_property"])
+            for i in range(human_num):
+                world.append(createHuman(self.ped_cfg, i))
+
+            with open(path, "wb+") as f:
+                tree.write(f, encoding="utf-8", xml_declaration=True)

simulators/third_party/dynamic_xml_config/plugins/robot_generate.py

@@ -0,0 +1,124 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+""" 
+******************************************************************************************
+*  Copyright (C) 2023 Yang Haodong, All Rights Reserved                                  *
+*                                                                                        *
+*  @brief    generate launch file dynamicly based on user configure.                     *
+*  @author   Haodong Yang                                                                *
+*  @version  1.0.2                                                                       *
+*  @date     2023.07.19                                                                  *
+*  @license  GNU General Public License (GPL)                                            *
+******************************************************************************************
+"""
+import xml.etree.ElementTree as ET
+from .xml_generate import XMLGenerator
+
+
+class RobotGenerator(XMLGenerator):
+    def __init__(self) -> None:
+        super().__init__()
+    
+    def __str__(self) -> str:
+        return "Robots Generator"
+    
+    def plugin(self):
+        """
+        Implement of robots starting application.
+        """
+        app_register = []
+        self.writeRobotsXml(self.root_path + "sim_env/launch/include/robots/start_robots.launch.xml")
+        return app_register
+
+    def writeRobotsXml(self, path):
+        """
+        Create configure file to start robots dynamically.
+
+        Parameters
+        ----------
+        path: str
+            the path of file(.launch.xml) to write.
+        """
+
+        def getRobotArg(name: str, index: int = -1):
+            if index == -1:
+                e = RobotGenerator.createElement(
+                    "arg", props={"name": name, "value": "$(eval arg('robot' + str(arg('robot_number')) + '_" + name + "'))"}
+                )
+            else:
+                e = RobotGenerator.createElement(
+                    "arg",
+                    props={
+                        "name": "robot" + str(index + 1) + "_" + name,
+                        "value": self.user_cfg["robots_config"][index]["robot" + str(index + 1) + "_" + name],
+                    },
+                )
+            return e
+
+        # root
+        launch = RobotGenerator.createElement("launch")
+
+        # setting the number of robots
+        if "robots_config" in self.user_cfg.keys():
+            robots_num = len(self.user_cfg["robots_config"])
+        else:
+            robots_num = 0
+            raise ValueError("There is no robot!")
+
+        launch.append(RobotGenerator.createElement("arg", props={"name": "robot_number", "default": str(robots_num)}))
+
+        # setting the parameters of robots
+        for i in range(robots_num):
+            # robotic type
+            launch.append(getRobotArg("type", i))
+            # global planner
+            launch.append(getRobotArg("global_planner", i))
+            # local planner
+            launch.append(getRobotArg("local_planner", i))
+            # robotic pose
+            launch.append(getRobotArg("x_pos", i))
+            launch.append(getRobotArg("y_pos", i))
+            launch.append(getRobotArg("z_pos", i))
+            launch.append(getRobotArg("yaw", i))
+
+        # create starting node
+        include = RobotGenerator.createElement("include", props={"file": "$(find sim_env)/launch/app/environment_single.launch.xml"})
+        include.append(
+            RobotGenerator.createElement("arg", props={"name": "robot", "value": "$(eval arg('robot' + str(arg('robot_number')) + '_type'))"})
+        )
+        # planner
+        include.append(getRobotArg("global_planner"))
+        include.append(getRobotArg("local_planner"))
+        # namespace
+        include.append(RobotGenerator.createElement("arg", props={"name": "robot_namespace", "value": "robot$(arg robot_number)"}))
+        if robots_num > 1:
+            include.append(RobotGenerator.createElement("arg", props={"name": "start_ns", "value": "true"}))
+        else:
+            include.append(RobotGenerator.createElement("arg", props={"name": "start_ns", "value": "false"}))
+        # pose
+        include.append(
+            RobotGenerator.createElement("arg", props={"name": "robot_x", "value": "$(eval arg('robot' + str(arg('robot_number')) + '_x_pos'))"})
+        )
+        include.append(
+            RobotGenerator.createElement("arg", props={"name": "robot_y", "value": "$(eval arg('robot' + str(arg('robot_number')) + '_y_pos'))"})
+        )
+        include.append(
+            RobotGenerator.createElement("arg", props={"name": "robot_z", "value": "$(eval arg('robot' + str(arg('robot_number')) + '_z_pos'))"})
+        )
+        include.append(
+            RobotGenerator.createElement("arg", props={"name": "robot_yaw", "value": "$(eval arg('robot' + str(arg('robot_number')) + '_yaw'))"})
+        )
+
+        # recursive start
+        cycle = RobotGenerator.createElement(
+            "include", props={"file": "$(find sim_env)/launch/include/robots/start_robots.launch.xml", "if": "$(eval arg('robot_number') > 1)"}
+        )
+        cycle.append(RobotGenerator.createElement("arg", props={"name": "robot_number", "value": "$(eval arg('robot_number') - 1)"}))
+
+        launch.append(include)
+        launch.append(cycle)
+        RobotGenerator.indent(launch)
+
+        with open(path, "wb+") as f:
+            ET.ElementTree(launch).write(f, encoding="utf-8", xml_declaration=True)

simulators/third_party/dynamic_xml_config/plugins/xml_generate.py

@@ -0,0 +1,88 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+""" 
+******************************************************************************************
+*  Copyright (C) 2023 Yang Haodong, All Rights Reserved                                  *
+*                                                                                        *
+*  @brief    generate launch file dynamicly based on user configure.                     *
+*  @author   Haodong Yang                                                                *
+*  @version  1.0.2                                                                       *
+*  @date     2023.04.23                                                                  *
+*  @license  GNU General Public License (GPL)                                            *
+******************************************************************************************
+"""
+import yaml
+import xml.etree.ElementTree as ET
+import sys, os
+
+from abc import abstractmethod, ABC
+
+class XMLGenerator(ABC):
+    def __init__(self) -> None:
+        # get the root path of package(src layer)
+        self.root_path = os.path.split(os.path.realpath(__file__))[0] + "/../../../"
+        # user configure
+        self.user_cfg = XMLGenerator.yamlParser(self.root_path + "user_config/" + sys.argv[1])
+
+    @abstractmethod
+    def plugin(self):
+        """
+        [interface] Implement of specific application.
+
+        Return
+        ----------
+        app_register: list of ET.Element to register
+        """
+        pass
+
+    @staticmethod
+    def yamlParser(path: str) -> dict:
+        """
+        Parser user configure file(.yaml).
+
+        Parameters
+        ----------
+        path: str
+            the path of file(.yaml).
+
+        Return
+        ----------
+        data: dict
+            user configuer tree
+        """
+        with open(path, "r") as f:
+            data = yaml.load(f, Loader=yaml.FullLoader)
+        return data
+
+    @staticmethod
+    def indent(elem: ET.Element, level: int = 0) -> None:
+        """
+        Format the generated xml document.
+
+        Parameters
+        ----------
+        elem: ET.Element
+        level: int
+            indent level.
+        """
+        i = "\n" + level * "\t"
+        if len(elem):
+            if not elem.text or not elem.text.strip():
+                elem.text = i + "\t"
+            if not elem.tail or not elem.tail.strip():
+                elem.tail = i
+            for elem in elem:
+                XMLGenerator.indent(elem, level + 1)
+            if not elem.tail or not elem.tail.strip():
+                elem.tail = i
+        else:
+            if level and (not elem.tail or not elem.tail.strip()):
+                elem.tail = i
+
+    @staticmethod
+    def createElement(name: str, text: str = None, props: dict = {}) -> ET.Element:
+        e = ET.Element(name, attrib=props)
+        if not text is None:
+            e.text = text
+        return e