ros进阶——ros中快捷的函数调用

这里整理了几个非常好用的ros工具，可以用来快速实现强化学习的环境，包括对urdf，对gazebo中的控制器，对gazebo本身，对launch文件，对节点，对模型等等

ros_gazebo.py

可以设置gazebo中的各种目标参数，尤其是可以将gazebo的仿真速度加快

#!/usr/bin/env python

import rospy
import rospkg
import os
import subprocess
import time
import xacro
from rospy.service import ServiceException

from gazebo_msgs.srv import GetPhysicsProperties, SetPhysicsProperties, SetPhysicsPropertiesRequest
from gazebo_msgs.srv import DeleteModel, SpawnModel
from gazebo_msgs.srv import DeleteModelRequest, SpawnModelRequest
from gazebo_msgs.srv import GetModelState, SetModelState
from gazebo_msgs.srv import GetModelStateRequest, SetModelStateRequest
from gazebo_msgs.msg import ODEPhysics, ModelState
from geometry_msgs.msg import Vector3
from std_srvs.srv import Empty
from std_msgs.msg import Header
from geometry_msgs.msg import Pose, Point, Quaternion, Twist


def launch_Gazebo(paused=False, use_sim_time=True, gui=True, recording=False, debug=False, verbose=False, output='screen',
                  custom_world_path=None, custom_world_pkg=None, custom_world_name=None, respawn_gazebo=False,
                  pub_clock_frequency=100, server_required=False, gui_required=False, launch_new_term=True) -> bool:
    """
    使用 ROS 网络启动 Gazebo。

    :param paused: 如果为 True，则以暂停状态启动 gzserver。
    :type paused: bool

    :param use_sim_time: 如果为 True，则使用仿真时间。
    :type use_sim_time: bool

    :param gui: 如果为 True，则启动 Gazebo 的 GUI（gzclient）。
    :type gui: bool

    :param recording: 如果为 True，则记录 Gazebo 的数据。
    :type recording: bool

    :param debug: 如果为 True，则使用调试配置。
    :type debug: bool

    :param verbose: 如果为 True，则打印调试信息。
    :type verbose: bool

    :param output: 选择 Gazebo 的输出方式（screen 或 log）。
    :type output: str [screen, log]

    :param custom_world_path: 如果不为 None，则使用此世界文件路径。
    :type custom_world_path: str

    :param custom_world_pkg: 如果 custom_world_path 为 None，则从此包中使用世界文件，具体文件名由 custom_world_name 指定。
    :type custom_world_pkg: str

    :param custom_world_name: 如果 custom_world_path 为 None，则使用 custom_world_pkg 中的此世界文件。
    :type custom_world_name: str

    :param respawn_gazebo: 如果为 True，则在 Gazebo 被杀死时重新启动， 默认值为 False。
    :type respawn_gazebo: bool

    :param pub_clock_frequency: 时钟发布器的频率（以 Hz 为单位）。
    :type pub_clock_frequency: int

    :param server_required: 如果为 True，则启动文件将等待 gzserver 运行。
    :type server_required: bool

    :param gui_required: 如果为 True，则启动文件将等待 gzclient 运行。
    :type gui_required: bool

    :param launch_new_term: 在新终端（Xterm）中启动 Gazebo 节点。
    :type launch_new_term: bool

    :return: 如果启动成功，则返回 True，否则返回 False。
    :rtype: bool
    """

    rospack = rospkg.RosPack()
    try:
        rospack.get_path('gazebo_ros')
    except rospkg.common.ResourceNotFound:
        rospy.logerr("未找到 gazebo_ros 包")
        return False

    term_command = "roslaunch gazebo_ros empty_world.launch "

    # 初始化世界时暂停
    term_command += " paused:=" + str(paused)

    # 使用仿真时间
    term_command += " use_sim_time:=" + str(use_sim_time)

    # 启用/禁用 GUI
    term_command += " gui:=" + str(gui)

    # 启用/禁用录制
    term_command += " recording:=" + str(recording)

    # 启用/禁用调试
    term_command += " debug:=" + str(debug)

    # 启用/禁用详细信息
    term_command += " verbose:=" + str(verbose)

    # 选择输出类型（screen 或 log）
    term_command += " output:=" + str(output)

    # 选择世界文件
    if custom_world_path is not None:
        term_command += " world_name:=" + str(custom_world_path)
    elif custom_world_pkg is not None and custom_world_name is not None:
        try:
            world_pkg_path = rospack.get_path(custom_world_pkg)
        except rospkg.common.ResourceNotFound:
            rospy.logwarn("未找到包含世界文件的包")

        world_file_path = world_pkg_path + "/worlds/" + custom_world_name
        term_command += " world_name:=" + str(world_file_path)

    # 启用 Gazebo 的重新启动
    term_command += " respawn_gazebo:=" + str(respawn_gazebo)

    # 设置 gazebo_ros 发布频率
    term_command += " pub_clock_frequency:=" + str(pub_clock_frequency)

    # 设置是否需要 Gazebo 服务器
    term_command += " server_required:=" + str(server_required)

    # 设置是否需要 GUI
    term_command += " gui_required:=" + str(gui_required)

    print(term_command)

    # 执行命令
    if launch_new_term:
        term_command = "xterm -e ' " + term_command + "'"

    subprocess.Popen(term_command, shell=True)
    time.sleep(5.0)

    rospy.wait_for_service('/gazebo/pause_physics')

    return True


def close_Gazebo() -> bool:
    """
    Function to close gazebo if its running.

    :return: True if gazebo was closed, False otherwise.
    :rtype: bool
    """

    term_command = "rosnode kill /gazebo /gazebo_gui"
    subprocess.Popen("xterm -e ' " + term_command + "'", shell=True).wait()
    time.sleep(0.5)

    term_command = "killall -9 gzserver gzclient"
    subprocess.Popen("xterm -e ' " + term_command + "'", shell=True).wait()

    return True

def gazebo_set_max_update_rate(max_update_rate) -> bool:
    """
    Function to set the max update rate for gazebo in real time factor: 1 is real time, 10 is 10 times real time.

    :param max_update_rate: the max update rate for gazebo in real time factor.
    :type max_update_rate: float

    :return: True if the command was sent and False otherwise.
    :rtype: bool
    """

    rospy.wait_for_service("/gazebo/get_physics_properties")
    rospy.wait_for_service("/gazebo/set_physics_properties")
    client_get_physics = rospy.ServiceProxy("/gazebo/get_physics_properties", GetPhysicsProperties)
    orig_physics = client_get_physics()

    new_physics = SetPhysicsPropertiesRequest(  time_step=orig_physics.time_step,
                                                max_update_rate=((1.0/orig_physics.time_step)*max_update_rate),
                                                gravity=orig_physics.gravity,
                                                ode_config=orig_physics.ode_config)
    
    client_set_physics = rospy.ServiceProxy("/gazebo/set_physics_properties", SetPhysicsProperties)
    result_gaz_upd = client_set_physics(new_physics)
    rospy.logdebug("Gazebo physics was succesful: " + str(result_gaz_upd))

    return result_gaz_upd.success

def gazebo_get_max_update_rate() -> float:
    """
    Function to get the current max update rate.

    :return: the max update rate.
    :rtype: float
    """

    rospy.wait_for_service("/gazebo/get_physics_properties")
    client_get_physics = rospy.ServiceProxy("/gazebo/get_physics_properties", GetPhysicsProperties)
    physics = client_get_physics()

    return physics.max_update_rate

def gazebo_set_time_step(new_time_step) -> bool:
    """
    Function to set the time step for gazebo.

    :param new_time_step: the new time step.
    :type new_time_step: float

    :return: True if the command was sent and False otherwise.
    :rtype: bool
    """
    
    rospy.wait_for_service("/gazebo/get_physics_properties")
    rospy.wait_for_service("/gazebo/set_physics_properties")
    client_get_physics = rospy.ServiceProxy("/gazebo/get_physics_properties", GetPhysicsProperties)
    orig_physics = client_get_physics()

    max_update_rate = orig_physics.max_update_rate / (1.0/orig_physics.time_step)

    new_physics = SetPhysicsPropertiesRequest(  time_step=new_time_step,
                                                max_update_rate=((1.0/new_time_step)*max_update_rate),
                                                gravity=orig_physics.gravity,
                                                ode_config=orig_physics.ode_config)
    
    client_set_physics = rospy.ServiceProxy("/gazebo/set_physics_properties", SetPhysicsProperties)
    result_gaz_upd = client_set_physics(new_physics)
    rospy.logdebug("Gazebo physics was succesful: " + str(result_gaz_upd))

    return result_gaz_upd.success

def gazebo_get_time_step() -> float:
    """
    Function to get the current time step for gazebo.

    :return: the time step.
    :rtype: float
    """

    rospy.wait_for_service("/gazebo/get_physics_properties")
    client_get_physics = rospy.ServiceProxy("/gazebo/get_physics_properties", GetPhysicsProperties)
    physics = client_get_physics()

    return physics.time_step

def gazebo_set_gravity(x, y, z) -> bool:
    """
    Function to set the gravity for gazebo.

    :param x: the x component of the gravity vector.
    :type x: float

    :param y: the y component of the gravity vector.
    :type y: float

    :param z: the z component of the gravity vector.
    :type z: float

    :return: True if the command was sent and False otherwise.
    :rtype: bool
    """

    rospy.wait_for_service("/gazebo/get_physics_properties")
    rospy.wait_for_service("/gazebo/set_physics_properties")
    client_get_physics = rospy.ServiceProxy("/gazebo/get_physics_properties", GetPhysicsProperties)
    orig_physics = client_get_physics()

    gravity = Vector3()
    gravity.x = x
    gravity.y = y
    gravity.z = z

    new_physics = SetPhysicsPropertiesRequest(  time_step=orig_physics.time_step,
                                                max_update_rate=orig_physics.max_update_rate,
                                                gravity=gravity,
                                                ode_config=orig_physics.ode_config)
    
    client_set_physics = rospy.ServiceProxy("/gazebo/set_physics_properties", SetPhysicsProperties)
    result_gaz_upd = client_set_physics(new_physics)
    rospy.logdebug("Gazebo physics was succesful: " + str(result_gaz_upd))

    return result_gaz_upd.success

def gazebo_get_gravity() -> Vector3:
    """
    Function to get the current gravity vector for gazebo.

    :return: the gravity vector.
    :rtype: Vector3
    """

    rospy.wait_for_service("/gazebo/get_physics_properties")
    client_get_physics = rospy.ServiceProxy("/gazebo/get_physics_properties", GetPhysicsProperties)
    physics = client_get_physics()

    return physics.gravity

def gazebo_set_ODE_physics( auto_disable_bodies, sor_pgs_precon_iters, sor_pgs_iters, sor_pgs_w, sor_pgs_rms_error_tol,
                            contact_surface_layer, contact_max_correcting_vel, cfm, erp, max_contacts) -> bool:
    """
    Function to set the ODE physics for gazebo.

    :return: True if the command was sent and False otherwise.
    """

    rospy.wait_for_service("/gazebo/get_physics_properties")
    rospy.wait_for_service("/gazebo/set_physics_properties")
    client_get_physics = rospy.ServiceProxy("/gazebo/get_physics_properties", GetPhysicsProperties)
    orig_physics = client_get_physics()

    ode_config = ODEPhysics()
    ode_config.auto_disable_bodies = auto_disable_bodies
    ode_config.sor_pgs_precon_iters = sor_pgs_precon_iters
    ode_config.sor_pgs_iters = sor_pgs_iters
    ode_config.sor_pgs_w = sor_pgs_w
    ode_config.sor_pgs_rms_error_tol = sor_pgs_rms_error_tol
    ode_config.contact_surface_layer = contact_surface_layer
    ode_config.contact_max_correcting_vel = contact_max_correcting_vel
    ode_config.cfm = cfm
    ode_config.erp = erp
    ode_config.max_contacts = max_contacts

    new_physics = SetPhysicsPropertiesRequest(  time_step=orig_physics.time_step,
                                                max_update_rate=orig_physics.max_update_rate,
                                                gravity=orig_physics.gravity,
                                                ode_config=orig_physics.ode_config)
    
    client_set_physics = rospy.ServiceProxy("/gazebo/set_physics_properties", SetPhysicsProperties)
    result_gaz_upd = client_set_physics(new_physics)
    rospy.logdebug("Gazebo physics was succesful: " + str(result_gaz_upd))

    return result_gaz_upd.success

def gazebo_get_ODE_physics() -> ODEPhysics:
    """
    Function to get the current ODE physics for gazebo.

    :return: the ODE physics.
    :rtype: ODEPhysics
    """

    rospy.wait_for_service("/gazebo/get_physics_properties")
    client_get_physics = rospy.ServiceProxy("/gazebo/get_physics_properties", GetPhysicsProperties)
    physics = client_get_physics()

    return physics.ode_config

def gazebo_set_default_properties() -> bool:
    """
    Function to set the default gazebo properties.

    :return: True if the command was sent and False otherwise.
    """

    counter_sucess = 0

    if gazebo_set_time_step(0.001):
        counter_sucess += 1
    if gazebo_set_max_update_rate(1.0):
        counter_sucess += 1
    if gazebo_set_gravity(0.0, 0.0, -9.81):
        counter_sucess += 1
    if gazebo_set_ODE_physics(False, 0, 50, 1.3, 0.0, 0.001, 0.0, 0.0, 0.2, 20):
        counter_sucess += 1

    if counter_sucess == 4:
        return True
    else:
        return False

def gazebo_reset_sim(retries=5) -> bool:
    """
    Function to reset the simulation, which reset models to original poses AND reset the simulation time.

    :param retries: The number of times to retry the service call.
    :type retries: int

    :return: True if the command was sent and False otherwise.
    :rtype: bool
    """

    rospy.wait_for_service("/gazebo/reset_simulation")
    client_srv = rospy.ServiceProxy("/gazebo/reset_simulation", Empty)

    for retry in range(retries):
        try:
            client_srv()
            return True
        except rospy.ServiceException as e:
            print ("/gazebo/reset_simulation service call failed")
        
    return False

def gazebo_reset_world(retries=5) -> bool:
    """
    Function to reset the world, which reset models to original poses WITHOUT resetting the simulation time.

    :param retries: The number of times to retry the service call.
    :type retries: int

    :return: True if the command was sent and False otherwise.
    :rtype: bool
    """

    rospy.wait_for_service("/gazebo/reset_world")
    client_srv = rospy.ServiceProxy("/gazebo/reset_world", Empty)

    for retry in range(retries):
        try:
            client_srv()
            return True
        except rospy.ServiceException as e:
            print ("/gazebo/reset_world service call failed")
        
    return False

def gazebo_pause_physics(retries=5) -> bool:
    """
    Function to pause the physics in the simulation.

    :param retries: The number of times to retry the service call.
    :type retries: int

    :return: True if the command was sent and False otherwise.
    :rtype: bool
    """

    rospy.wait_for_service("/gazebo/pause_physics")
    client_srv = rospy.ServiceProxy("/gazebo/pause_physics", Empty)

    for retry in range(retries):
        try:
            client_srv()
            return True
        except rospy.ServiceException as e:
            print ("/gazebo/pause_physics service call failed")
        
    return False

def gazebo_unpause_physics(retries=5) -> bool:
    """
    Function to unpause the physics in the simulation.

    :param retries: The number of times to retry the service call.
    :type retries: int

    :return: True if the command was sent and False otherwise.
    :rtype: bool
    """

    rospy.wait_for_service("/gazebo/unpause_physics")
    client_srv = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)
    
    for retry in range(retries):
        try:
            client_srv()
            return True
        except rospy.ServiceException as e:
            print ("/gazebo/pause_physics service call failed")
        
    return False

def gazebo_step_physics(steps=1) -> bool:
    """
    Function to step the physics in the simulation.

    :param steps: The number of times to step the simulation.
    :type steps: int

    :return: True if the command was sent and False otherwise.
    :rtype: bool
    """
    rospy.wait_for_service("/gazebo/pause_physics")

    term_command = "gz world -m " + str(steps)
    subprocess.Popen(term_command, shell=True).wait()
    return True

def gazebo_delete_model(model_name) -> bool:
    """
    Function to delete a model from the simulation.

    :param model_name: The name of the model to delete.
    :type model_name: str

    :return: True if the command was sent and False otherwise.
    :rtype: bool
    """

    rospy.wait_for_service("/gazebo/delete_model")
    client_srv = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel)

    srv_model = DeleteModelRequest(model_name=model_name)

    result = client_srv(srv_model)

    return result.success

def gazebo_spawn_urdf_path( model_path, model_name="robot1", robot_namespace="/", reference_frame="world",
                            pos_x=0.0, pos_y=0.0, pos_z=0.0, 
                            ori_x=0.0, ori_y=0.0, ori_z=0.0, ori_w=1.0) -> bool:
    """
    Function to spawn a model from a URDF file.

    :param model_path: The path to the URDF file.
    :type model_path: str

    :param model_name: Name of model to spawn
    :type model_name: str

    :param robot_namespace: change ROS namespace of gazebo-plugins.
    :type robot_namespace: str

    :param reference_frame: Name of the model/body where initial pose is defined. If left empty or specified as "world", gazebo world frame is used
    :type reference_frame: str

    :param pos_x: x position of model in model's reference frame
    :type pos_x: float
    :param pos_y: y position of model in model's reference frame
    :type pos_y: float
    :param pos_z: z position of model in model's reference frame
    :type pos_z: float

    :param ori_x: X part of Quaternion of model orientation in model's reference frame.
    :type ori_x: float
    :param ori_y: Y part of Quaternion of model orientation in model's reference frame.
    :type ori_y: float
    :param ori_z: Z part of Quaternion of model orientation in model's reference frame.
    :type ori_z: float
    :param ori_w: W part of Quaternion of model orientation in model's reference frame.
    :type ori_w: float

    :return: [True if the command was sent and False otherwise, Status message].
    :rtype: [bool, str]
    """
    
    rospy.wait_for_service("/gazebo/spawn_urdf_model")
    client_srv = rospy.ServiceProxy("/gazebo/spawn_urdf_model", SpawnModel)

    if os.path.exists(model_path) is False:
        print("Error: model path does not exist")
        return False

    model = xacro.process_file(model_path)
    encoding = {}
    model_string = model.toprettyxml(indent='  ', **encoding)

    srv_model = SpawnModelRequest(  model_name=model_name,
                                    model_xml=model_string,
                                    robot_namespace=robot_namespace,
                                    initial_pose=Pose(  position=Point(x=pos_x, y=pos_y, z=pos_z),
                                                        orientation=Quaternion(x=ori_x, y=ori_y, z=ori_z, w=ori_w)),
                                    reference_frame=reference_frame)
    
    try:
        result = client_srv(srv_model)
        return result.success, result.status_message
    except ServiceException:
        print("Error: Package used in model was not found, source the workspace in all terminals.")
        return False

def gazebo_spawn_urdf_pkg(  pkg_name, file_name, file_folder="/urdf", model_name="robot1", robot_namespace="/", reference_frame="world",
                            pos_x=0.0, pos_y=0.0, pos_z=0.0, 
                            ori_x=0.0, ori_y=0.0, ori_z=0.0, ori_w=1.0) -> bool:
    """
    Function to spawn a model from a URDF file.

    :param pkg_name: Name of the package to import the URDF file from.
    :type pkg_name: str

    :param file_name: Name of the URDF file to import.
    :type file_name: str

    :param file_folder: Folder where the URDF file is located. Defaults to "/urdf".
    :type file_folder: str

    :param model_name: Name of model to spawn
    :type model_name: str

    :param robot_namespace: change ROS namespace of gazebo-plugins.
    :type robot_namespace: str

    :param reference_frame: Name of the model/body where initial pose is defined. If left empty or specified as "world", gazebo world frame is used
    :type reference_frame: str

    :param pos_x: x position of model in model's reference frame
    :type pos_x: float
    :param pos_y: y position of model in model's reference frame
    :type pos_y: float
    :param pos_z: z position of model in model's reference frame
    :type pos_z: float

    :param ori_x: X part of Quaternion of model orientation in model's reference frame.
    :type ori_x: float
    :param ori_y: Y part of Quaternion of model orientation in model's reference frame.
    :type ori_y: float
    :param ori_z: Z part of Quaternion of model orientation in model's reference frame.
    :type ori_z: float
    :param ori_w: W part of Quaternion of model orientation in model's reference frame.
    :type ori_w: float

    :return: [True if the command was sent and False otherwise, Status message].
    :rtype: [bool, str]
    """
    
    rospy.wait_for_service("/gazebo/spawn_urdf_model")
    client_srv = rospy.ServiceProxy("/gazebo/spawn_urdf_model", SpawnModel)

    rospack = rospkg.RosPack()
    try:
        pkg_path = rospack.get_path(pkg_name)
        rospy.logdebug("Package FOUND...")
    except rospkg.common.ResourceNotFound:
        rospy.logerr("Package NOT FOUND")
        return False

    file_path = pkg_path + file_folder + "/" + file_name

    if os.path.exists(file_path) is False:
        print("Error: model path does not exist")
        return False

    model = xacro.process_file(file_path)
    encoding = {}
    model_string = model.toprettyxml(indent='  ', **encoding)

    srv_model = SpawnModelRequest(  model_name=model_name,
                                    model_xml=model_string,
                                    robot_namespace=robot_namespace,
                                    initial_pose=Pose(  position=Point(x=pos_x, y=pos_y, z=pos_z),
                                                        orientation=Quaternion(x=ori_x, y=ori_y, z=ori_z, w=ori_w)),
                                    reference_frame=reference_frame)
    
    try:
        result = client_srv(srv_model)
        return result.success, result.status_message
    except ServiceException:
        print("Error: Package used in model was not found, source the workspace in all terminals.")
        return False

def gazebo_spawn_urdf_string(   model_string, model_name="robot1", robot_namespace="/", reference_frame="world",
                                pos_x=0.0, pos_y=0.0, pos_z=0.0, 
                                ori_x=0.0, ori_y=0.0, ori_z=0.0, ori_w=1.0) -> bool:
    """
    Function to spawn a model from a URDF file.

    :param model_string: URDF string to import.
    :type model_string: str

    :param model_name: Name of model to spawn
    :type model_name: str

    :param robot_namespace: change ROS namespace of gazebo-plugins.
    :type robot_namespace: str

    :param reference_frame: Name of the model/body where initial pose is defined. If left empty or specified as "world", gazebo world frame is used.
    :type reference_frame: str

    :param pos_x: x position of model in model's reference frame
    :type pos_x: float
    :param pos_y: y position of model in model's reference frame
    :type pos_y: float
    :param pos_z: z position of model in model's reference frame
    :type pos_z: float

    :param ori_x: X part of Quaternion of model orientation in model's reference frame.
    :type ori_x: float
    :param ori_y: Y part of Quaternion of model orientation in model's reference frame.
    :type ori_y: float
    :param ori_z: Z part of Quaternion of model orientation in model's reference frame.
    :type ori_z: float
    :param ori_w: W part of Quaternion of model orientation in model's reference frame.
    :type ori_w: float

    :return: [True if the command was sent and False otherwise, Status message].
    :rtype: [bool, str]
    """
    
    rospy.wait_for_service("/gazebo/spawn_urdf_model")
    client_srv = rospy.ServiceProxy("/gazebo/spawn_urdf_model", SpawnModel)

    srv_model = SpawnModelRequest(  model_name=model_name,
                                    model_xml=model_string,
                                    robot_namespace=robot_namespace,
                                    initial_pose=Pose(  position=Point(x=pos_x, y=pos_y, z=pos_z),
                                                        orientation=Quaternion(x=ori_x, y=ori_y, z=ori_z, w=ori_w)),
                                    reference_frame=reference_frame)
    
    try:
        result = client_srv(srv_model)
        return result.success, result.status_message
    except ServiceException:
        print("Error: Package used in model was not found, source the workspace in all terminals.")
        return False

def gazebo_spawn_urdf_param(    param_name, model_name="robot1", robot_namespace="/", reference_frame="world",
                                pos_x=0.0, pos_y=0.0, pos_z=0.0, 
                                ori_x=0.0, ori_y=0.0, ori_z=0.0, ori_w=1.0) -> bool:
    """
    Function to spawn a model from a URDF in the param server.

    :param param_name: Name of model to spawn
    :type param_name: str

    :param model_name: Name of model to spawn
    :type model_name: str

    :param robot_namespace: change ROS namespace of gazebo-plugins.
    :type robot_namespace: str

    :param reference_frame: Name of the model/body where initial pose is defined. If left empty or specified as "world", gazebo world frame is used.
    :type reference_frame: str

    :param pos_x: x position of model in model's reference frame
    :type pos_x: float
    :param pos_y: y position of model in model's reference frame
    :type pos_y: float
    :param pos_z: z position of model in model's reference frame
    :type pos_z: float

    :param ori_x: X part of Quaternion of model orientation in model's reference frame.
    :type ori_x: float
    :param ori_y: Y part of Quaternion of model orientation in model's reference frame.
    :type ori_y: float
    :param ori_z: Z part of Quaternion of model orientation in model's reference frame.
    :type ori_z: float
    :param ori_w: W part of Quaternion of model orientation in model's reference frame.
    :type ori_w: float

    :return: [True if the command was sent and False otherwise, Status message].
    :rtype: [bool, str]
    """

    rospy.wait_for_service("/gazebo/spawn_urdf_model")
    client_srv = rospy.ServiceProxy("/gazebo/spawn_urdf_model", SpawnModel)

    final_param_name = robot_namespace + "/" + param_name

    if rospy.has_param(final_param_name) is False:
        print("Error: Parameter " + final_param_name +" does not exist")
        return False

    model_string = rospy.get_param(final_param_name)

    srv_model = SpawnModelRequest(  model_name=model_name,
                                    model_xml=model_string,
                                    robot_namespace=robot_namespace,
                                    initial_pose=Pose(  position=Point(x=pos_x, y=pos_y, z=pos_z),
                                                        orientation=Quaternion(x=ori_x, y=ori_y, z=ori_z, w=ori_w)),
                                    reference_frame=reference_frame)
    
    try:
        result = client_srv(srv_model)
        return result.success , result.status_message
    except ServiceException:
        print("Error: Package used in model was not found, source the workspace in all terminals.")
        return False

def gazebo_spawn_sdf_path( model_path, model_name="robot1", robot_namespace="/", reference_frame="world",
                            pos_x=0.0, pos_y=0.0, pos_z=0.0, 
                            ori_x=0.0, ori_y=0.0, ori_z=0.0, ori_w=1.0) -> bool:
    """
    Function to spawn a model from a SDF file.

    :param model_path: The path to the SDF file.
    :type model_path: str

    :param model_name: Name of model to spawn
    :type model_name: str

    :param robot_namespace: change ROS namespace of gazebo-plugins.
    :type robot_namespace: str

    :param reference_frame: Name of the model/body where initial pose is defined. If left empty or specified as "world", gazebo world frame is used
    :type reference_frame: str

    :param pos_x: x position of model in model's reference frame
    :type pos_x: float
    :param pos_y: y position of model in model's reference frame
    :type pos_y: float
    :param pos_z: z position of model in model's reference frame
    :type pos_z: float

    :param ori_x: X part of Quaternion of model orientation in model's reference frame.
    :type ori_x: float
    :param ori_y: Y part of Quaternion of model orientation in model's reference frame.
    :type ori_y: float
    :param ori_z: Z part of Quaternion of model orientation in model's reference frame.
    :type ori_z: float
    :param ori_w: W part of Quaternion of model orientation in model's reference frame.
    :type ori_w: float

    :return: True if the command was sent and False otherwise.
    :rtype: bool
    """
    
    rospy.wait_for_service("/gazebo/spawn_sdf_model")
    client_srv = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel)

    if os.path.exists(model_path) is False:
        print("Error: model path does not exist")
        return False

    model = xacro.process_file(model_path)
    encoding = {}
    model_string = model.toprettyxml(indent='  ', **encoding)

    srv_model = SpawnModelRequest(  model_name=model_name,
                                    model_xml=model_string,
                                    robot_namespace=robot_namespace,
                                    initial_pose=Pose(  position=Point(x=pos_x, y=pos_y, z=pos_z),
                                                        orientation=Quaternion(x=ori_x, y=ori_y, z=ori_z, w=ori_w)),
                                    reference_frame=reference_frame)
    
    try:
        result = client_srv(srv_model)
        return result.success
    except ServiceException:
        print("Error: Package used in model was not found, source the workspace in all terminals.")
        return False

def gazebo_spawn_sdf_pkg(  pkg_name, file_name, file_folder="/sdf", model_name="robot1", robot_namespace="/", reference_frame="world",
                            pos_x=0.0, pos_y=0.0, pos_z=0.0, 
                            ori_x=0.0, ori_y=0.0, ori_z=0.0, ori_w=1.0) -> bool:
    """
    Function to spawn a model from a SDF file.

    :param pkg_name: Name of the package to import the SDF file from.
    :type pkg_name: str

    :param file_name: Name of the SDF file to import.
    :type file_name: str

    :param file_folder: Folder where the SDF file is located. Default is "/sdf".
    :type file_folder: str

    :param model_name: Name of model to spawn
    :type model_name: str

    :param robot_namespace: change ROS namespace of gazebo-plugins.
    :type robot_namespace: str

    :param reference_frame: Name of the model/body where initial pose is defined. If left empty or specified as "world", gazebo world frame is used
    :type reference_frame: str

    :param pos_x: x position of model in model's reference frame
    :type pos_x: float
    :param pos_y: y position of model in model's reference frame
    :type pos_y: float
    :param pos_z: z position of model in model's reference frame
    :type pos_z: float

    :param ori_x: X part of Quaternion of model orientation in model's reference frame.
    :type ori_x: float
    :param ori_y: Y part of Quaternion of model orientation in model's reference frame.
    :type ori_y: float
    :param ori_z: Z part of Quaternion of model orientation in model's reference frame.
    :type ori_z: float
    :param ori_w: W part of Quaternion of model orientation in model's reference frame.
    :type ori_w: float

    :return: True if the command was sent and False otherwise.
    :rtype: bool
    """
    
    rospy.wait_for_service("/gazebo/spawn_sdf_model")
    client_srv = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel)

    rospack = rospkg.RosPack()
    try:
        pkg_path = rospack.get_path(pkg_name)
        rospy.logdebug("Package FOUND...")
    except rospkg.common.ResourceNotFound:
        rospy.logerr("Package NOT FOUND")
        return False

    file_path = pkg_path + file_folder + "/" + file_name

    if os.path.exists(file_path) is False:
        print("Error: model path does not exist")
        return False

    model = xacro.process_file(file_path)
    encoding = {}
    model_string = model.toprettyxml(indent='  ', **encoding)

    srv_model = SpawnModelRequest(  model_name=model_name,
                                    model_xml=model_string,
                                    robot_namespace=robot_namespace,
                                    initial_pose=Pose(  position=Point(x=pos_x, y=pos_y, z=pos_z),
                                                        orientation=Quaternion(x=ori_x, y=ori_y, z=ori_z, w=ori_w)),
                                    reference_frame=reference_frame)
    
    try:
        result = client_srv(srv_model)
        return result.success
    except ServiceException:
        print("Error: Package used in model was not found, source the workspace in all terminals.")
        return False

def gazebo_spawn_sdf_string(   model_string, model_name="robot1", robot_namespace="/", reference_frame="world",
                                pos_x=0.0, pos_y=0.0, pos_z=0.0, 
                                ori_x=0.0, ori_y=0.0, ori_z=0.0, ori_w=1.0) -> bool:
    """
    Function to spawn a model from a SDF file.

    :param model_string: SDF string to import.
    :type model_string: str

    :param model_name: Name of model to spawn
    :type model_name: str

    :param robot_namespace: change ROS namespace of gazebo-plugins.
    :type robot_namespace: str

    :param reference_frame: Name of the model/body where initial pose is defined. If left empty or specified as "world", gazebo world frame is used.
    :type reference_frame: str

    :param pos_x: x position of model in model's reference frame
    :type pos_x: float
    :param pos_y: y position of model in model's reference frame
    :type pos_y: float
    :param pos_z: z position of model in model's reference frame
    :type pos_z: float

    :param ori_x: X part of Quaternion of model orientation in model's reference frame.
    :type ori_x: float
    :param ori_y: Y part of Quaternion of model orientation in model's reference frame.
    :type ori_y: float
    :param ori_z: Z part of Quaternion of model orientation in model's reference frame.
    :type ori_z: float
    :param ori_w: W part of Quaternion of model orientation in model's reference frame.
    :type ori_w: float

    :return: True if the command was sent and False otherwise.
    :rtype: bool
    """
    
    rospy.wait_for_service("/gazebo/spawn_sdf_model")
    client_srv = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel)

    srv_model = SpawnModelRequest(  model_name=model_name,
                                    model_xml=model_string,
                                    robot_namespace=robot_namespace,
                                    initial_pose=Pose(  position=Point(x=pos_x, y=pos_y, z=pos_z),
                                                        orientation=Quaternion(x=ori_x, y=ori_y, z=ori_z, w=ori_w)),
                                    reference_frame=reference_frame)
    
    try:
        result = client_srv(srv_model)
        return result.success
    except ServiceException:
        print("Error: Package used in model was not found, source the workspace in all terminals.")
        return False

def gazebo_spawn_sdf_param(    param_name, model_name="robot1", robot_namespace="/", reference_frame="world",
                                pos_x=0.0, pos_y=0.0, pos_z=0.0, 
                                ori_x=0.0, ori_y=0.0, ori_z=0.0, ori_w=1.0) -> bool:
    """
    Function to spawn a model from a SDF in the param server.

    :param param_name: Name of model to spawn
    :type param_name: str

    :param model_name: Name of model to spawn
    :type model_name: str

    :param robot_namespace: change ROS namespace of gazebo-plugins.
    :type robot_namespace: str

    :param reference_frame: Name of the model/body where initial pose is defined. If left empty or specified as "world", gazebo world frame is used.
    :type reference_frame: str

    :param pos_x: x position of model in model's reference frame
    :type pos_x: float
    :param pos_y: y position of model in model's reference frame
    :type pos_y: float
    :param pos_z: z position of model in model's reference frame
    :type pos_z: float

    :param ori_x: X part of Quaternion of model orientation in model's reference frame.
    :type ori_x: float
    :param ori_y: Y part of Quaternion of model orientation in model's reference frame.
    :type ori_y: float
    :param ori_z: Z part of Quaternion of model orientation in model's reference frame.
    :type ori_z: float
    :param ori_w: W part of Quaternion of model orientation in model's reference frame.
    :type ori_w: float

    :return: True if the command was sent and False otherwise.
    :rtype: bool
    """

    rospy.wait_for_service("/gazebo/spawn_sdf_model")
    client_srv = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel)

    final_param_name = robot_namespace + "/" + param_name

    if rospy.has_param(final_param_name) is False:
        print("Error: Parameter " + final_param_name +" does not exist")
        return False

    model_string = rospy.get_param(final_param_name)

    srv_model = SpawnModelRequest(  model_name=model_name,
                                    model_xml=model_string,
                                    robot_namespace=robot_namespace,
                                    initial_pose=Pose(  position=Point(x=pos_x, y=pos_y, z=pos_z),
                                                        orientation=Quaternion(x=ori_x, y=ori_y, z=ori_z, w=ori_w)),
                                    reference_frame=reference_frame)
    
    try:
        result = client_srv(srv_model)
        return result.success
    except ServiceException:
        print("Error: Package used in model was not found, source the workspace in all terminals.")
        return False

def gazebo_get_model_state(model_name, relative_entity_name="world"):
    """
    Function to get the state of a model.

    :param model_name: Name of model to get the state of.
    :type model_name: str

    :param relative_entity_name: Return pose and twist relative to this entity (an entity can be a model, body, or geom).
    :type relative_entity_name: str

    :return: The header of the message, the pose of the model, the twist of the model, and success.
    """

    rospy.wait_for_service("/gazebo/get_model_state")
    client_srv = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState)

    srv_msg = GetModelStateRequest(model_name=model_name, relative_entity_name=relative_entity_name)

    try:
        result = client_srv(srv_msg)
        return result.header, result.pose, result.twist, result.success
    except ServiceException:
        print("Error processing the request")
        return Header(), Pose(), Twist(), False

def gazebo_set_model_state(model_name, ref_frame="world", pos_x=0.0, pos_y=0.0, pos_z=0.0, ori_x=0.0, ori_y=0.0, ori_z=0.0, ori_w=0.0,
                            lin_vel_x=0.0, lin_vel_y=0.0, lin_vel_z=0.0, ang_vel_x=0.0, ang_vel_y=0.0, ang_vel_z=0.0, sleep_time=0.05) -> bool:
    """
    Function to set the model name in gazebo.

    :param model_name: Name of model to set.
    :type model_name: str

    :param ref_frame: Reference frame of model.
    :type ref_frame: str

    :param pos_x: x position of model in model's reference frame
    :type pos_x: float
    :param pos_y: y position of model in model's reference frame
    :type pos_y: float
    :param pos_z: z position of model in model's reference frame
    :type pos_z: float

    :param ori_x: X part of Quaternion of model orientation in model's reference frame.
    :type ori_x: float
    :param ori_y: Y part of Quaternion of model orientation in model's reference frame.
    :type ori_y: float
    :param ori_z: Z part of Quaternion of model orientation in model's reference frame.
    :type ori_z: float
    :param ori_w: W part of Quaternion of model orientation in model's reference frame.
    :type ori_w: float

    :param lin_vel_x: X part of linear velocity of model in model's reference frame.
    :type lin_vel_x: float
    :param lin_vel_y: Y part of linear velocity of model in model's reference frame.
    :type lin_vel_y: float
    :param lin_vel_z: Z part of linear velocity of model in model's reference frame.
    :type lin_vel_z: float

    :param ang_vel_x: X part of angular velocity of model in model's reference frame.
    :type ang_vel_x: float
    :param ang_vel_y: Y part of angular velocity of model in model's reference frame.
    :type ang_vel_y: float
    :param ang_vel_z: Z part of angular velocity of model in model's reference frame.
    :type ang_vel_z: float

    :param sleep_time: Time to sleep bewteen sending request and getting response.
    :type sleep_time: float

    :return: True if the command was sent and False otherwise.
    :rtype: bool
    """

    rospy.wait_for_service("/gazebo/set_model_state")
    client_srv = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState)

    msg_pose = Pose(  position=Point(x=pos_x, y=pos_y, z=pos_z), orientation=Quaternion(x=ori_x, y=ori_y, z=ori_z, w=ori_w))
    msg_twist = Twist(  linear=Vector3(x=lin_vel_x, y=lin_vel_y, z=lin_vel_z), angular=Vector3(x=ang_vel_x, y=ang_vel_y, z=ang_vel_z))

    srv_msg = ModelState(model_name=model_name, pose=msg_pose, twist=msg_twist, reference_frame=ref_frame)

    srv_request = SetModelStateRequest(model_state=srv_msg)

    try:
        result = client_srv(srv_request)
        time.sleep(sleep_time)
        return result.success
    except ServiceException:
        print("Error processing the request")
        return False

加速gazebo仿真环境的使用方式：

#!/usr/bin/env python
# coding: utf-8

import rospy
from ros_gazebo import gazebo_set_max_update_rate

def main():
    # 初始化 ROS 节点
    rospy.init_node('speed_up_gazebo', anonymous=True)

    # 设置最大更新速率
    max_update_rate = 5

    try:
        # 调用函数设置最大更新速率
        success = gazebo_set_max_update_rate(max_update_rate)
        if success:
            rospy.loginfo(f"成功将 Gazebo 的最大更新速率设置为 {max_update_rate}")
        else:
            rospy.logerr("设置 Gazebo 最大更新速率失败")
    except rospy.ServiceException as e:
        rospy.logerr(f"服务调用失败: {e}")

if __name__ == '__main__':
    main()

可以看到加速了5倍：

也可以通过它来用xterm启动自己的仿真环境，示例：

#!/usr/bin/env python
# coding: utf-8

import rospy
import ros_gazebo
import ros_node
from ros_gazebo import gazebo_set_max_update_rate


def main():
    ros_node.ros_kill_all_processes()

    ros_gazebo.launch_Gazebo(paused=True, gui=True)

    # 初始化 ROS 节点
    rospy.init_node('speed_up_gazebo', anonymous=True)

if __name__ == '__main__':
    main()
    

ros_urdf.py:

可以用来加载我们的模型的urdf

#!/bin/python3

import rospy
import rospkg
import os
import xacro

def urdf_load_from_pkg(pkg_name, model_name, param_name, folder="/urdf", ns=None, args_xacro=None) -> bool:
    """
    从 ROS 包中加载 URDF 文件到参数服务器。

    :param pkg_name: ROS 包的名称。
    :type pkg_name: str

    :param model_name: 模型文件的名称。
    :type model_name: str

    :param param_name: 参数的名称。
    :type param_name: str

    :param folder: 模型所在的文件夹。默认为 "/urdf"
    :type folder: str

    :param ns: 参数的命名空间。
    :type ns: str

    :param args_xacro: xacro 参数列表，例如：['arg1:=True','arg2:=10.0']
    :type args_xacro: str 列表

    :return: 如果 URDF 文件已加载，则返回 True，否则返回 False。
    :rtype: bool
    """

    rospack = rospkg.RosPack()
    try:
        pkg_path = rospack.get_path(pkg_name)
        rospy.logdebug("找到包...")
    except rospkg.common.ResourceNotFound:
        rospy.logerr("未找到包")
        return False

    file_path = pkg_path + folder + "/" + model_name

    if os.path.exists(file_path) is False:
        print("错误：模型路径不存在")
        return False

    list_args = [file_path] 
    if args_xacro is not None:
        list_args = list_args + args_xacro

    opts, input_file_name = xacro.process_args(list_args)
    model = xacro.process_file(input_file_name, **vars(opts))
    encoding = {}
    model_string = model.toprettyxml(indent='  ', **encoding)
    
    if ns is not None and ns != "/":
        final_param_name = ns + "/" + param_name
    else:
        final_param_name = param_name

    rospy.set_param(final_param_name, model_string)
    return True

def urdf_load_from_path(model_path, param_name, ns=None, args_xacro=None) -> bool:
    """
    从文件路径加载 URDF 文件到参数服务器。

    :param model_path: 模型文件的路径。
    :type model_path: str

    :param param_name: 参数的名称。
    :type param_name: str

    :param ns: 参数的命名空间。
    :type ns: str

    :param args_xacro: xacro 参数列表，例如：['arg1:=True','arg2:=10.0']
    :type args_xacro: str 列表

    :return: 如果 URDF 文件已加载，则返回 True，否则返回 False。
    :rtype: bool
    """

    if os.path.exists(model_path) is False:
        print("错误：模型路径不存在")
        return False

    list_args = [model_path] 
    if args_xacro is not None:
        list_args = list_args + args_xacro

    opts, input_file_name = xacro.process_args(list_args)
    model = xacro.process_file(input_file_name, **vars(opts))
    encoding = {}
    model_string = model.toprettyxml(indent='  ', **encoding)

    if ns is not None and ns != "/":
        final_param_name = ns + "/" + param_name
    else:
        final_param_name = param_name

    rospy.set_param(final_param_name, model_string)
    return True

def urdf_parse_from_pkg(pkg_name, model_name, folder="/urdf", args_xacro=None) -> str:
    """
    从 ROS 包中解析 URDF 文件并返回 URDF 字符串。

    :param pkg_name: ROS 包的名称。
    :type pkg_name: str

    :param model_name: 模型文件的名称。
    :type model_name: str

    :param folder: 模型所在的文件夹。默认为 "/urdf"
    :type folder: str

    :param args_xacro: xacro 参数列表，例如：['arg1:=True','arg2:=10.0']
    :type args_xacro: str 列表

    :return: URDF 字符串，如果未找到包或文件，则返回 None。
    :rtype: str
    """
    rospack = rospkg.RosPack()
    try:
        pkg_path = rospack.get_path(pkg_name)
        rospy.logdebug("找到包...")
    except rospkg.common.ResourceNotFound:
        rospy.logerr("未找到包")
        return None

    file_path = pkg_path + folder + "/" + model_name

    if os.path.exists(file_path) is False:
        print("错误：模型路径不存在")
        return None

    list_args = [file_path] 
    if args_xacro is not None:
        list_args = list_args + args_xacro

    opts, input_file_name = xacro.process_args(list_args)
    model = xacro.process_file(input_file_name, **vars(opts))
    encoding = {}
    model_string = model.toprettyxml(indent='  ', **encoding)

    return model_string

def urdf_parse_from_path(model_path, args_xacro=None) -> str:
    """
    从文件路径解析 URDF 文件并返回 URDF 字符串。

    :param model_path: 模型文件的路径。
    :type model_path: str

    :param args_xacro: xacro 参数列表，例如：['arg1:=True','arg2:=10.0']
    :type args_xacro: str 列表

    :return: URDF 字符串，如果未找到文件，则返回 None。
    :rtype: str
    """

    if os.path.exists(model_path) is False:
        print("错误：模型路径不存在")
        return None

    list_args = [model_path] 
    if args_xacro is not None:
        list_args = list_args + args_xacro

    opts, input_file_name = xacro.process_args(list_args)
    model = xacro.process_file(input_file_name, **vars(opts))
    encoding = {}
    model_string = model.toprettyxml(indent='  ', **encoding)

    return model_string

例如结合ros_gazebo:

    # 生成箱子
    UnitBox = ros_urdf.urdf_parse_from_pkg("kobuki_maze_rl", "model.sdf", folder="/worlds/Box")
    # 放置到某个位置
    ros_gazebo.gazebo_spawn_sdf_string(UnitBox, model_name="box1", pos_x= -2.5, pos_y=-5.0)

ros_param.py:

参数文件的快捷加载，可以通过功能包或者路径的形式去加载配置yaml文件

#!/bin/python3

import rospy
import rospkg
import os
import rosparam

def ros_load_yaml_from_pkg(pkg_name, file_name, ns='/') -> bool:
    """
    从包中获取 YAML 文件并将其加载到 ROS 参数服务器中。

    :param pkg_name: 包的名称。
    :type  pkg_name: str

    :param file_name: 文件的名称。
    :type  file_name: str

    :param ns: 要加载参数的命名空间。
    :type  ns: str

    :return: 如果文件已加载，则返回 True，否则返回 False。
    :rtype: bool
    """

    rospack = rospkg.RosPack()
    try:
        pkg_path = rospack.get_path(pkg_name)
        rospy.logdebug("找到包...")
    except rospkg.common.ResourceNotFound:
        rospy.logerr("未找到包")
        return False

    file_path = pkg_path + "/config/" + file_name
    if os.path.exists(pkg_path + "/config/" + file_name) is False:
        print("配置文件 " + file_name + " 在 " + file_path + " 中不存在")
        return False

    paramlist = rosparam.load_file(file_path)
    
    for params, namespace in paramlist:
        rosparam.upload_params(ns, params)

    return True

def ros_load_yaml_from_path(file_path, ns='/') -> bool:
    """
    从路径中获取 YAML 文件并将其加载到 ROS 参数服务器中。

    :param file_path: 文件的路径。
    :type  file_path: str

    :param ns: 要加载参数的命名空间。
    :type  ns: str

    :return: 如果文件已加载，则返回 True，否则返回 False。
    :rtype: bool
    """

    if os.path.exists(file_path) is False:
        print("配置文件 " + file_path + " 不存在")
        return False

    paramlist = rosparam.load_file(file_path)
    
    for params, namespace in paramlist:
        rosparam.upload_params(ns, params)

    return True

ros_node.py:

#!/bin/python3

import subprocess
import time

import rospkg
import rospy

"""
与 ROS 节点处理相关的函数。
"""


def ros_node_from_pkg(
    pkg_name,
    node_name,
    launch_master=False,
    launch_master_term=True,
    launch_new_term=True,
    name=None,
    ns="/",
    output="log",
) -> bool:
    """
    从包中启动 ROS 节点的函数。

    :param pkg_name: 要从中启动节点的包的名称。
    :type pkg_name: str

    :param node_name: 要启动的节点的名称。
    :type node_name: str

    :param launch_master: 如果 ROSMASTER 没有运行，则启动它。
    :type launch_master: bool

    :param launch_master_term: 如果启动 ROSMASTER，则在外部终端中启动。
    :type launch_master_term: bool

    :param launch_new_term: 是否在新终端（Xterm）中启动进程。
    :type launch_new_term: bool

    :param name: 要启动的节点的名称。
    :type name: str

    :param ns: 要启动的节点的命名空间。
    :type ns: str

    :param output: 输出目标，可以是 log、screen 或 None。
    :type output: str

    :return: 如果节点已启动，则返回 True，否则返回 False。
    :rtype: bool
    """

    rospack = rospkg.RosPack()
    try:
        rospack.get_path(pkg_name)
        rospy.logdebug("找到包...")
    except rospkg.common.ResourceNotFound:
        rospy.logerr("未找到包")
        return False

    if launch_master:
        print("启动 Master")
        try:
            rospy.get_master().getPid()
        except ConnectionRefusedError:
            print("Master 未运行")
            if launch_master_term:
                subprocess.Popen("xterm -e 'roscore' ", shell=True)
            else:
                subprocess.Popen("roscore", shell=True)
            time.sleep(5.0)
        else:
            print("Master 正在运行")

    try:
        rospy.get_master().getPid()
    except ConnectionRefusedError:
        print("Master 未运行")
        return False
    else:
        print("Master 正在运行")

    term_command = "rosrun " + pkg_name + " " + node_name

    if name is not None:
        term_command += " __name:=" + str(name)

    term_command += " __ns:=" + str(ns)
    term_command += " __log:=" + str(output)

    if launch_new_term:
        term_command = "xterm -e ' " + term_command + "'"

    subprocess.Popen(term_command, shell=True)
    time.sleep(5.0)

    return True


def ros_kill_node(node_name) -> bool:
    """
    杀死 ROS 节点的函数。

    :param node_name: 要杀死的节点的名称。
    :type node_name: str

    :return: 如果节点被杀死，则返回 True，否则返回 False。
    :rtype: bool
    """

    term_command = "rosnode kill " + node_name
    subprocess.Popen("xterm -e ' " + term_command + "'", shell=True).wait()
    return True


def ros_kill_all_nodes() -> bool:
    """
    杀死所有正在运行的 ROS 节点的函数。

    :return: 如果所有节点被杀死，则返回 True，否则返回 False。
    :rtype: bool
    """

    term_command = "rosnode kill -a"
    subprocess.Popen("xterm -e ' " + term_command + "'", shell=True).wait()
    return True


def ros_kill_master() -> bool:
    """
    杀死 ROS master 的函数。

    :return: 如果 master 被杀死，则返回 True，否则返回 False。
    :rtype: bool
    """

    try:
        rospy.get_master().getPid()
    except ConnectionRefusedError:
        print("Master 未运行")
        return True
    else:
        print("Master 正在运行")
        term_command = "rosnode kill -a"
        subprocess.Popen("xterm -e ' " + term_command + "'", shell=True).wait()
        time.sleep(0.5)
        term_command = "killall -9 rosout roslaunch rosmaster nodelet"
        subprocess.Popen("xterm -e ' " + term_command + "'", shell=True).wait()

        return True


def ros_kill_all_processes() -> bool:
    """
    函数用于杀死所有正在运行的与ROS相关的进程。

    :return: 如果所有进程都被杀死，则返回True，否则返回False。
    :rtype: bool
    """

    # 定义要终止的进程命令
    term_command = "killall -9 rosout roslaunch rosmaster gzserver nodelet robot_state_publisher gzclient xterm"
    # 使用subprocess.Popen执行命令，并等待命令执行完成
    subprocess.Popen("xterm -e ' " + term_command + "'", shell=True).wait()
    # 返回True表示所有进程已被杀死
    return True

ros_launch.py:

#!/bin/python3

import rospy
import rospkg
import os
import subprocess
import time

def ros_launch_from_pkg(pkg_name, launch_file, args=None, launch_new_term=True) -> bool:
    """
    从包中执行 roslaunch 文件的函数。
    :param pkg_name: 包含 launch 文件的包的名称。
    :type pkg_name: str
    
    :param launch_file: launch 文件的名称。
    :type launch_file: str
    
    :param args: 要传递给 launch 文件的参数。
    :type args: str 列表

    :param launch_new_term: 是否在新终端（Xterm）中启动进程。
    :type launch_new_term: bool

    :return: 如果 launch 文件已执行，则返回 True。
    :rtype: bool
    """

    rospack = rospkg.RosPack()
    try:
        pkg_path = rospack.get_path(pkg_name)
        rospy.logdebug("找到包...")
    except rospkg.common.ResourceNotFound:
        rospy.logerr("未找到包")
        return False

    file_path = pkg_path + "/launch/" + launch_file
    if os.path.exists(pkg_path + "/launch/" + launch_file) is False:
        print("launch 文件 " + launch_file + " 在 " + file_path + " 中不存在")
        return False

    term_command = "roslaunch " + pkg_name + " " + launch_file

    if args is not None:
        for arg in args:
            term_command += " " + arg

    if launch_new_term:
        term_command = "xterm -e ' " + term_command + "'"
    
    subprocess.Popen(term_command, shell=True)
    time.sleep(5.0)

    return True

def ros_launch_from_path(launch_file_path, args=None, launch_new_term=True) -> bool:
    """
    从路径中执行 roslaunch 文件的函数。
    :param launch_file_path: launch 文件的路径。
    :type launch_file_path: str

    :param args: 要传递给 launch 文件的参数。
    :type args: str 列表

    :param launch_new_term: 是否在新终端（Xterm）中启动进程。
    :type launch_new_term: bool

    :return: 如果 launch 文件已执行，则返回 True。
    :rtype: bool
    """

    if os.path.exists(launch_file_path) is False:
        print("launch 文件 " + launch_file_path + " 不存在")
        return False

    term_command = "roslaunch " + launch_file_path

    if args is not None:
        for arg in args:
            term_command += " " + arg

    if launch_new_term:
        term_command = "xterm -e ' " + term_command + "'"
    
    subprocess.Popen(term_command, shell=True)
    time.sleep(5.0)

    return True

def ros_kill_launch_process() -> bool:
    """
    杀死所有 roslaunch 进程的函数。

    :return: 如果 roslaunch 进程被杀死，则返回 True。
    :rtype: bool
    """
    term_command = "killall -9 roslaunch"
    subprocess.Popen("xterm -e ' " + term_command + "'", shell=True).wait()
    return True

加载模型

ros_spawn.py:

#!/bin/python3

import rospy
import time
from frobs_rl.common import ros_gazebo
from frobs_rl.common import ros_controllers
from frobs_rl.common import ros_node
from frobs_rl.common import ros_params
from frobs_rl.common import ros_urdf

def init_robot_state_pub(namespace="/", max_pub_freq=None, launch_new_term=False) -> bool:
    """
    初始化机器人状态发布器的函数。

    :param namespace: 机器人的命名空间。
    :type namespace: str

    :param max_pub_freq: 发布器的最大发布频率。
    :type max_pub_freq: float

    :param launch_new_term: 是否在新终端（Xterm）中启动进程。
    :type launch_new_term: bool

    :return: 如果发布器已初始化，则返回 True。
    :rtype: bool
    """

    if max_pub_freq is not None:
        if namespace != "/":
            rospy.set_param(namespace + "/rob_st_pub/publish_frequency", max_pub_freq)
        else:
            rospy.set_param("/rob_st_pub/publish_frequency", max_pub_freq)
            
    return ros_node.ros_node_from_pkg("robot_state_publisher", "robot_state_publisher", launch_new_term=launch_new_term, name="rob_st_pub", ns=namespace)


def spawn_model_in_gazebo(  pkg_name, model_urdf_file, 
                            controllers_file, controllers_list=[],
                            model_urdf_folder="/urdf", ns="/", args_xacro=None, max_pub_freq=None, rob_st_term=False,
                            gazebo_name="robot1", gaz_ref_frame="world", 
                            pos_x=0.0, pos_y=0.0, pos_z=0.0, ori_w=0.0, ori_x=0.0, ori_y=0.0, ori_z=0.0):
    """
    在 Gazebo 中生成模型的函数。

    :param pkg_name: 模型所在的包名。
    :type pkg_name: str

    :param model_urdf_file: 模型的 URDF 文件名。
    :type model_urdf_file: str

    :param controllers_file: 控制器文件的名称。如果为 None，则不会加载任何控制器。
    :type controllers_file: str

    :param controllers_list: 要加载的控制器列表。
    :type controllers_list: list

    :param model_urdf_folder: 模型 URDF 文件所在的文件夹。默认为 "/urdf"。
    :type model_urdf_folder: str

    :param ns: 模型的命名空间。默认为 "/"。
    :type ns: str

    :param args_xacro: 要传递给 xacro 的参数。
    :type args_xacro: list

    :param max_pub_freq: 机器人状态发布器的最大发布频率。
    :type max_pub_freq: float

    :param rob_st_term: 是否在新终端（Xterm）中启动机器人状态发布器。
    :type rob_st_term: bool

    :param gazebo_name: Gazebo 模型的名称。
    :type gazebo_name: str

    :param gaz_ref_frame: Gazebo 模型的参考框架。
    :type gaz_ref_frame: str

    :param pos_x: Gazebo 模型的 X 位置。
    :param pos_y: Gazebo 模型的 Y 位置。
    :param pos_z: Gazebo 模型的 Z 位置。
    :type pos_x: float
    :type pos_y: float
    :type pos_z: float

    :param ori_w: Gazebo 模型的 W 方向。
    :param ori_x: Gazebo 模型的 X 方向。
    :param ori_y: Gazebo 模型的 Y 方向。
    :param ori_z: Gazebo 模型的 Z 方向。
    :type ori_w: float
    :type ori_x: float
    :type ori_y: float
    :type ori_z: float

    :return: 如果模型已生成，则返回 True。
    :rtype: bool
    """

    # 将模型 URDF 加载到参数服务器中
    if ros_urdf.urdf_load_from_pkg(pkg_name, model_urdf_file, "robot_description", folder=model_urdf_folder, ns=ns, args_xacro=args_xacro):
        rospy.logwarn("URDF 文件加载成功")
    else:
        rospy.logwarn("加载 URDF 文件时出错")
        return False
    
    time.sleep(0.1)

    # 初始化机器人状态发布器
    if init_robot_state_pub(namespace=ns, max_pub_freq=max_pub_freq, launch_new_term=rob_st_term):
        rospy.logwarn("机器人状态发布器初始化成功")
    else:
        rospy.logwarn("初始化机器人状态发布器时出错")
        return False

    time.sleep(0.1)

    # 在 Gazebo 中生成模型
    result_spawn, message = ros_gazebo.gazebo_spawn_urdf_param("robot_description", model_name=gazebo_name, robot_namespace=ns, reference_frame=gaz_ref_frame,
                                        pos_x=pos_x, pos_y=pos_y, pos_z=pos_z, ori_w=ori_w, ori_x=ori_x, ori_y=ori_y, ori_z=ori_z,)
    if result_spawn:
        rospy.logwarn("模型生成成功")
    else:
        rospy.logwarn("生成模型时出错")
        rospy.logwarn(message)
        return False

    time.sleep(0.1)

    if controllers_file is not None:
        # 从 YAML 文件中将机器人控制器加载到参数服务器中
        if ros_params.ros_load_yaml_from_pkg(pkg_name, controllers_file, ns=ns):
            rospy.logwarn("机器人控制器加载成功")
        else:
            rospy.logwarn("加载机器人控制器时出错")
            return False

        time.sleep(0.1)

        # 生成控制器
        if ros_controllers.spawn_controllers_srv(controllers_list, ns=ns):
            rospy.logwarn("控制器生成成功")
        else:
            rospy.logwarn("生成控制器时出错")
            return False
    
    return True

ros_controller.py

管理ros中的控制器

#!/usr/bin/env python

import rospy
from controller_manager_msgs.srv import *

def load_controller_srv(controller_name, ns=None, max_retries=5) -> bool:
    """
    在命名空间中加载控制器的服务函数。

    :param controller_name: 要加载的控制器名称
    :type controller_name: 字符串

    :param ns: 命名空间
    :type ns: 字符串

    :param max_retries: 加载控制器的最大重试次数。
    :type max_retries: 整数

    :return: 如果控制器加载成功，则返回 True。
    :rtype: 布尔值
    """

    if ns is not None:
        srv_name = ns + '/controller_manager/load_controller'
    else:
        srv_name = '/controller_manager/load_controller'

    rospy.wait_for_service(srv_name)
    client_srv = rospy.ServiceProxy(srv_name, LoadController)
    
    try: 
        for ii in range(max_retries):
            srv_request = LoadControllerRequest(name=controller_name)
            resp1 = client_srv(srv_request)
            if resp1.ok:
                return True
            
        if resp1.ok is False:
            print("控制器 " + controller_name + " 无法加载。")
        
        return resp1.ok
    
    except rospy.ServiceException as exc:
        print("服务未处理请求: " + str(exc))
        return False

def load_controller_list_srv(controller_list,ns=None, max_retries=5) -> None:
    """
    在命名空间中加载控制器列表的服务函数。

    :param controller_list: 要加载的控制器列表
    :type controller_list: 字符串列表

    :param ns: 命名空间
    :type ns: 字符串

    :param max_retries: 加载控制器的最大重试次数。
    :type max_retries: 整数
    """

    for controller in controller_list:
        load_controller_srv(controller, ns=ns, max_retries=max_retries)

def unload_controller_srv(controller_name,ns=None, max_retries=5) -> bool:
    """
    在命名空间中卸载控制器的服务函数。

    :param controller_name: 要卸载的控制器名称
    :type controller_name: 字符串

    :param ns: 命名空间
    :type ns: 字符串

    :param max_retries: 卸载控制器的最大重试次数。
    :type max_retries: 整数

    :return: 如果控制器卸载成功，则返回 True。
    :rtype: 布尔值
    """

    if ns is not None:
        srv_name = ns + '/controller_manager/unload_controller'
    else:
        srv_name = '/controller_manager/unload_controller'

    rospy.wait_for_service(srv_name)
    client_srv = rospy.ServiceProxy(srv_name, UnloadController)
    
    try: 
        for ii in range(max_retries):
            srv_request = UnloadControllerRequest(name=controller_name)
            resp1 = client_srv(srv_request)
            if resp1.ok:
                return True

        if resp1.ok is False:
            print("控制器 " + controller_name + " 无法卸载。")    
        
        return resp1.ok

    except rospy.ServiceException as exc:
        print("服务未处理请求: " + str(exc))
        return False

def unload_controller_list_srv(controller_list,ns=None, max_retries=5) -> None:
    """
    在命名空间中卸载控制器列表的服务函数。

    :param controller_list: 要卸载的控制器列表
    :type controller_list: 字符串列表

    :param ns: 命名空间
    :type ns: 字符串

    :param max_retries: 卸载控制器的最大重试次数。
    :type max_retries: 整数
    """
    for controller in controller_list:
        unload_controller_srv(controller, ns=ns, max_retries=max_retries)

def switch_controllers_srv( start_controllers, stop_controllers, ns=None, 
                            strictness=1, start_asap=False, timeout=3.0, max_retries=5) -> bool:
    """
    在命名空间中切换控制器的服务函数。

    :param start_controllers: 要启动的控制器列表
    :type start_controllers: 字符串列表

    :param stop_controllers: 要停止的控制器列表
    :type stop_controllers: 字符串列表

    :param ns: 命名空间
    :type ns: 字符串

    :param strictness: 控制器管理器的严格性：BEST_EFFORT 或 STRICT (1 和 2，分别)。
    :type strictness: 整数

    :param start_asap: 一旦硬件依赖项准备就绪，立即启动控制器，否则等待所有接口准备就绪。
    :type start_asap: 布尔值

    :param timeout: 在中止挂起的控制器之前等待的超时时间（以秒为单位）。零表示无限。
    :type timeout: 浮点数

    :param max_retries: 切换控制器的最大重试次数。
    :type max_retries: 整数

    :return: 如果操作成功，则返回 True。
    :rtype: 布尔值
    """

    if ns is not None:
        srv_name = ns + '/controller_manager/switch_controller'
    else:
        srv_name = '/controller_manager/switch_controller'

    rospy.wait_for_service(srv_name)
    client_srv = rospy.ServiceProxy(srv_name, SwitchController)
    
    try: 
        for ii in range(max_retries):
            srv_request = SwitchControllerRequest(  start_controllers=start_controllers,stop_controllers=stop_controllers,
                                                    strictness=strictness,start_asap=start_asap,timeout=timeout)
            resp1 = client_srv(srv_request)
            if resp1.ok:
                return True
        
        if resp1.ok is False:
            print("控制器无法切换。")

        return resp1.ok
    except rospy.ServiceException as exc:
        print("服务未处理请求: " + str(exc))
        return False

def start_controllers_srv(start_controllers, ns=None, strictness=1, start_asap=False, timeout=3.0) -> bool:
    """
    在命名空间中启动控制器的服务函数。

    :param start_controllers: 要启动的控制器列表
    :type start_controllers: 字符串列表

    :param ns: 命名空间
    :type ns: 字符串

    :param strictness: 控制器管理器的严格性：BEST_EFFORT 或 STRICT (1 和 2，分别)。
    :type strictness: 整数

    :param start_asap: 一旦硬件依赖项准备就绪，立即启动控制器，否则等待所有接口准备就绪。
    :type start_asap: 布尔值

    :param timeout: 在中止挂起的控制器之前等待的超时时间（以秒为单位）。零表示无限。
    :type timeout: 浮点数

    :return: 如果操作成功，则返回 True。
    :rtype: 布尔值
    """

    return switch_controllers_srv(start_controllers, [], ns=ns, strictness=strictness, start_asap=start_asap, timeout=timeout)

def stop_controllers_srv(stop_controllers, ns=None, strictness=1, start_asap=False, timeout=3.0) -> bool:
    """
    在命名空间中停止控制器的服务函数。

    :param stop_controllers: 要停止的控制器列表
    :type stop_controllers: 字符串列表

    :param ns: 命名空间
    :type ns: 字符串

    :param strictness: 控制器管理器的严格性：BEST_EFFORT 或 STRICT (1 和 2，分别)。
    :type strictness: 整数

    :param start_asap: 一旦硬件依赖项准备就绪，立即启动控制器，否则等待所有接口准备就绪。
    :type start_asap: 布尔值

    :param timeout: 在中止挂起的控制器之前等待的超时时间（以秒为单位）。零表示无限。
    :type timeout: 浮点数

    :return: 如果操作成功，则返回 True。
    :rtype: 布尔值
    """

    return switch_controllers_srv([], stop_controllers, ns=ns, strictness=strictness, start_asap=start_asap, timeout=timeout)

def reset_controllers_srv(reset_controllers, max_retries=10, ns=None, strictness=1, start_asap=False, timeout=3.0) -> bool:
    """
    在命名空间中重置控制器的服务函数。

    :param reset_controllers: 要重置的控制器列表
    :type reset_controllers: 字符串列表

    :param max_retries: 在放弃之前重置控制器的最大重试次数。
    :type max_retries: 整数

    :param ns: 命名空间
    :type ns: 字符串

    :param strictness: 控制器管理器的严格性：BEST_EFFORT 或 STRICT (1 和 2，分别)。
    :type strictness: 整数

    :param start_asap: 一旦硬件依赖项准备就绪，立即启动控制器，否则等待所有接口准备就绪。
    :type start_asap: 布尔值

    :param timeout: 在中止挂起的控制器之前等待的超时时间（以秒为单位）。零表示无限。
    :type timeout: 浮点数

    :return: 如果操作成功，则返回 True。
    :rtype: 布尔值
    """

    done_switch_off = False
    for ii in range(max_retries):
        done_switch_off = stop_controllers_srv(reset_controllers, ns=ns, strictness=strictness, start_asap=start_asap, timeout=timeout)
        if done_switch_off:
            break
    
    if not done_switch_off:
        return False

    done_switch_on = False
    for ii in range(max_retries):
        done_switch_on = start_controllers_srv(reset_controllers, ns=ns, strictness=strictness, start_asap=start_asap, timeout=timeout)
        if done_switch_on:
            break

    if not done_switch_on:
        return False
    
    return True

def spawn_controllers_srv(spawn_controllers, ns=None, strictness=1, start_asap=False, timeout=3.0) -> bool:
    """
    在命名空间中生成控制器的服务函数。

    :param spawn_controllers: 要生成的控制器列表
    :type spawn_controllers: 字符串列表

    :param ns: 命名空间
    :type ns: 字符串

    :param strictness: 控制器管理器的严格性：BEST_EFFORT 或 STRICT (1 和 2，分别)。
    :type strictness: 整数

    :param start_asap: 一旦硬件依赖项准备就绪，立即启动控制器，否则等待所有接口准备就绪。
    :type start_asap: 布尔值

    :param timeout: 在中止挂起的控制器之前等待的超时时间（以秒为单位）。零表示无限。
    :type timeout: 浮点数

    :return: 如果操作成功，则返回 True。
    :rtype: 布尔值
    """

    load_controller_list_srv(spawn_controllers, ns=ns)

    return start_controllers_srv(spawn_controllers, ns=ns, strictness=strictness, start_asap=start_asap, timeout=timeout)

def kill_controllers_srv(kill_controllers, ns=None, strictness=1, start_asap=False, timeout=3.0) -> bool:
    """
    在命名空间中杀死控制器的服务函数。

    :param kill_controllers: 要杀死的控制器列表
    :type kill_controllers: 字符串列表

    :param ns: 命名空间
    :type ns: 字符串

    :param strictness: 控制器管理器的严格性：BEST_EFFORT 或 STRICT (1 和 2，分别)。
    :type strictness: 整数

    :param start_asap: 一旦硬件依赖项准备就绪，立即启动控制器，否则等待所有接口准备就绪。
    :type start_asap: 布尔值

    :param timeout: 在中止挂起的控制器之前等待的超时时间（以秒为单位）。零表示无限。
    :type timeout: 浮点数

    :return: 如果操作成功，则返回 True。
    :rtype: 布尔值
    """

    res = stop_controllers_srv(kill_controllers, ns=ns, strictness=strictness, start_asap=start_asap, timeout=timeout)

    if res:
        unload_controller_list_srv(kill_controllers, ns=ns)
        return True
    else:
        return False

使用示例：

#!/usr/bin/env python

import rospy
import time

# 导入你的函数模块（假设上述代码保存为`controller_manager_utils.py`）
from controller_manager_utils import (
    load_controller_srv,
    unload_controller_srv,
    start_controllers_srv,
    stop_controllers_srv,
    reset_controllers_srv,
    spawn_controllers_srv,
    kill_controllers_srv
)

def main():
    rospy.init_node("controller_manager_test", anonymous=True)

    # 示例控制器名称
    controller_name = "joint_state_controller"
    controller_list = ["joint_state_controller", "effort_controller"]

    # 1. 加载控制器
    rospy.loginfo("加载控制器: {}".format(controller_name))
    if load_controller_srv(controller_name):
        rospy.loginfo("控制器加载成功")
    else:
        rospy.logerr("控制器加载失败")

    # 2. 启动控制器
    rospy.loginfo("启动控制器: {}".format(controller_name))
    if start_controllers_srv([controller_name]):
        rospy.loginfo("控制器启动成功")
    else:
        rospy.logerr("控制器启动失败")

    # 等待一段时间
    time.sleep(2)

    # 3. 停止控制器
    rospy.loginfo("停止控制器: {}".format(controller_name))
    if stop_controllers_srv([controller_name]):
        rospy.loginfo("控制器停止成功")
    else:
        rospy.logerr("控制器停止失败")

    # 4. 卸载控制器
    rospy.loginfo("卸载控制器: {}".format(controller_name))
    if unload_controller_srv(controller_name):
        rospy.loginfo("控制器卸载成功")
    else:
        rospy.logerr("控制器卸载失败")

    # 5. 测试控制器列表操作
    rospy.loginfo("加载控制器列表: {}".format(controller_list))
    load_controller_list_srv(controller_list)

    rospy.loginfo("启动控制器列表: {}".format(controller_list))
    if start_controllers_srv(controller_list):
        rospy.loginfo("控制器列表启动成功")
    else:
        rospy.logerr("控制器列表启动失败")

    time.sleep(2)

    rospy.loginfo("停止控制器列表: {}".format(controller_list))
    if stop_controllers_srv(controller_list):
        rospy.loginfo("控制器列表停止成功")
    else:
        rospy.logerr("控制器列表停止失败")

    rospy.loginfo("卸载控制器列表: {}".format(controller_list))
    unload_controller_list_srv(controller_list)

    # 6. 测试重置控制器
    rospy.loginfo("测试重置控制器: {}".format(controller_name))
    if reset_controllers_srv([controller_name]):
        rospy.loginfo("控制器重置成功")
    else:
        rospy.logerr("控制器重置失败")

    # 7. 测试生成和杀死控制器
    rospy.loginfo("测试生成控制器: {}".format(controller_name))
    if spawn_controllers_srv([controller_name]):
        rospy.loginfo("控制器生成成功")
    else:
        rospy.logerr("控制器生成失败")

    time.sleep(2)

    rospy.loginfo("测试杀死控制器: {}".format(controller_name))
    if kill_controllers_srv([controller_name]):
        rospy.loginfo("控制器杀死成功")
    else:
        rospy.logerr("控制器杀死失败")

if __name__ == "__main__":
    try:
        main()
    except rospy.ROSInterruptException:
        pass

但是在实际使用的过程中发现控制器启动失败了，所以我还是使用node，通过spawer来加载和启动控制器。

        # # 启动 controller_manager 的 spawner 节点
        node_name = "cartpole_controller_node"
        pkg_name = "controller_manager"
        node_type = "spawner"
        controllers = "joint_state_controller stand_cart_position_controller"
        ros_node.ros_node_from_pkg(pkg_name, node_type, launch_new_term=False, name=node_name, ns=ns, args=controllers)

由于node之前没有考虑有args传入，所以也需要对应修改一下：

def ros_node_from_pkg(
    pkg_name,
    node_name,
    launch_master=False,
    launch_master_term=True,
    launch_new_term=True,
    name=None,
    ns="/",
    output="log",
    args=None  # 新增参数
) -> bool:
    """
    从包中启动 ROS 节点的函数。

    :param pkg_name: 要从中启动节点的包的名称。
    :type pkg_name: str

    :param node_name: 要启动的节点的名称。
    :type node_name: str

    :param launch_master: 如果 ROSMASTER 没有运行，则启动它。
    :type launch_master: bool

    :param launch_master_term: 如果启动 ROSMASTER，则在外部终端中启动。
    :type launch_master_term: bool

    :param launch_new_term: 是否在新终端（Xterm）中启动进程。
    :type launch_new_term: bool

    :param name: 要启动的节点的名称。
    :type name: str

    :param ns: 要启动的节点的命名空间。
    :type ns: str

    :param output: 输出目标，可以是 log、screen 或 None。
    :type output: str

    :param args: 要传递给节点的额外参数。
    :type args: str

    :return: 如果节点已启动，则返回 True，否则返回 False。
    :rtype: bool
    """

    rospack = rospkg.RosPack()
    try:
        rospack.get_path(pkg_name)
        rospy.logdebug("找到包...")
    except rospkg.common.ResourceNotFound:
        rospy.logerr("未找到包")
        return False

    if launch_master:
        print("启动 Master")
        try:
            rospy.get_master().getPid()
        except ConnectionRefusedError:
            print("Master 未运行")
            if launch_master_term:
                subprocess.Popen("xterm -e 'roscore' ", shell=True)
            else:
                subprocess.Popen("roscore", shell=True)
            time.sleep(5.0)
        else:
            print("Master 正在运行")

    try:
        rospy.get_master().getPid()
    except ConnectionRefusedError:
        print("Master 未运行")
        return False
    else:
        print("Master 正在运行")

    term_command = "rosrun " + pkg_name + " " + node_name

    if name is not None:
        term_command += " __name:=" + str(name)

    term_command += " __ns:=" + str(ns)
    term_command += " __log:=" + str(output)

    if args is not None:
        term_command += " " + args  # 将额外的参数添加到命令中

    if launch_new_term:
        term_command = "xterm -e ' " + term_command + "'"

    subprocess.Popen(term_command, shell=True)
    time.sleep(5.0)

    return True