分布式 ROS PX4 GAZEBO 多机仿真服务器-客户端模式

欣苦

已于 2023-03-22 17:51:24 修改

阅读量1k

点赞数 4

文章标签：分布式

于 2023-03-22 17:39:19 首次发布

本文链接：https://blog.csdn.net/weixin_43249526/article/details/129711879

版权

这是一个目录

最终目标

实现主从机器多机仿真，模拟真机部署。具体方案如下：
设定一台计算机为通信汇集节点，处理所有无人机位姿、移动控制等，该主机装配QGC,统一显示所有无人机状态。

注意：这种连接方式不适用于集群仿真，此处仅作为模拟真机部署，无人机处于不同环境仿真。原因：一个无人机对应一个虚拟机，非常浪费虚拟机资源。集群仿真采用PX4官方提供的同一机器下生成多无人机方式即可。
在这里插入图片描述

环境配置要求

当前配置：两个ubuntu20.04虚拟机都应安装完整的ROS及PX4开发工具，包括ROS，PX4，GAZEBO，MAVROS等。配置过程略。
简化部署过程：主控计算机中安装QGC作为可视化端，还当做无人机0号机器；无人机1则单独为一台虚拟机配置。
注意：无人机机间通过IP地址互联后，GAZEBO服务器只能启动一个。最好选择在主控计算机上启动，可以避免多余的配置。

具体实施方案

UAV0配置

修改相关文件：PX4-AUTOPILOT项目中launch文件夹下的multi_uav_mavros_sitl.launch文件。将多余的UAV1和UAV2注释，其余配置默认即可。

<?xml version="1.0"?>
<launch>
    <!-- MAVROS posix SITL environment launch script -->
    <!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
    <!-- vehicle model and world -->
    <arg name="est" default="ekf2"/>
    <arg name="vehicle" default="iris"/>
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
    <!-- gazebo configs -->
    <arg name="gui" default="true"/>
    <arg name="debug" default="false"/>
    <arg name="verbose" default="false"/>
    <arg name="paused" default="false"/>
    <!-- Gazebo sim -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="gui" value="$(arg gui)"/>
        <arg name="world_name" value="$(arg world)"/>
        <arg name="debug" value="$(arg debug)"/>
        <arg name="verbose" value="$(arg verbose)"/>
        <arg name="paused" value="$(arg paused)"/>
    </include>
    <!-- UAV0 -->
    <group ns="uav0">
        <!-- MAVROS and vehicle configs -->
        <arg name="ID" value="0"/>
        <!--
            fcu_url 如果设置为localhost则与本地PX4飞控连接
                    如果设置为对应PX4飞控所在IP,则与对应机器的PX4连接
         -->
        <arg name="fcu_url" default="udp://:14540@localhost:14580"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
            <arg name="x" value="0"/>
            <arg name="y" value="0"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="$(arg vehicle)"/>
            <arg name="mavlink_udp_port" value="14560"/>
            <arg name="mavlink_tcp_port" value="4560"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
        </include>
        <!-- MAVROS -->
        <include file="$(find mavros)/launch/px4.launch">
            <!-- fcu_url指定的飞控的连接方式 USB/CP2102/SITL -->
            <arg name="fcu_url" value="$(arg fcu_url)"/>
            <!-- gcs_url指定QGC所在主机的IP -->
            <arg name="gcs_url" value=""/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group>
    <!-- UAV1
    <group ns="uav1">

        <arg name="ID" value="1"/>
        <arg name="fcu_url" default="udp://:14541@localhost:14581"/>

        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
            <arg name="x" value="1"/>
            <arg name="y" value="0"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="$(arg vehicle)"/>
            <arg name="mavlink_udp_port" value="14561"/>
            <arg name="mavlink_tcp_port" value="4561"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
        </include>

        <include file="$(find mavros)/launch/px4.launch">
            <arg name="fcu_url" value="$(arg fcu_url)"/>
            <arg name="gcs_url" value=""/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group>

    <group ns="uav2">

        <arg name="ID" value="2"/>
        <arg name="fcu_url" default="udp://:14542@localhost:14582"/>

        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
            <arg name="x" value="0"/>
            <arg name="y" value="1"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="$(arg vehicle)"/>
            <arg name="mavlink_udp_port" value="14562"/>
            <arg name="mavlink_tcp_port" value="4562"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
        </include>

        <include file="$(find mavros)/launch/px4.launch">
            <arg name="fcu_url" value="$(arg fcu_url)"/>
            <arg name="gcs_url" value=""/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group> -->
</launch>
<!-- to add more UAVs (up to 10):
Increase the id
Change the name space
Set the FCU to default="udp://:14540+id@localhost:14550+id"
Set the malink_udp_port to 14560+id) -->

下一步以PX4的官方教程为参考实现offboard控制无人机移动。
注意：官方的教程仅适用于mavros_posix_sitl.launch为启动文件的单无人机控制。多无人模式下，需要将对应话题更改为对应ID无人机的话题。如下：

#! /usr/bin/env python3
"""
 * File: offb_node.py
 * Stack and tested in Gazebo Classic 9 SITL
"""

import rospy
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest

current_state = State()

def state_cb(msg):
    global current_state
    current_state = msg


if __name__ == "__main__":
    rospy.init_node("offb_node_py")
	# 修改： /mavros/state -> /uav0/mavros/state
    state_sub = rospy.Subscriber("/uav0/mavros/state", State, callback = state_cb)

    local_pos_pub = rospy.Publisher("/uav0/mavros/setpoint_position/local", PoseStamped, queue_size=10)

    rospy.wait_for_service("/uav0/mavros/cmd/arming")
    arming_client = rospy.ServiceProxy("/uav0/mavros/cmd/arming", CommandBool)    

    rospy.wait_for_service("/uav/mavros/set_mode")
    set_mode_client = rospy.ServiceProxy("/uav/mavros/set_mode", SetMode)


    # Setpoint publishing MUST be faster than 2Hz
    rate = rospy.Rate(20)

    # Wait for Flight Controller connection
    while(not rospy.is_shutdown() and not current_state.connected):
        rate.sleep()

    pose = PoseStamped()

    pose.pose.position.x = 0
    pose.pose.position.y = 0
    pose.pose.position.z = 2

    # Send a few setpoints before starting
    for i in range(100):   
        if(rospy.is_shutdown()):
            break

        local_pos_pub.publish(pose)
        rate.sleep()

    offb_set_mode = SetModeRequest()
    offb_set_mode.custom_mode = 'OFFBOARD'

    arm_cmd = CommandBoolRequest()
    arm_cmd.value = True

    last_req = rospy.Time.now()

    while(not rospy.is_shutdown()):
        if(current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
            if(set_mode_client.call(offb_set_mode).mode_sent == True):
                rospy.loginfo("OFFBOARD enabled")

            last_req = rospy.Time.now()
        else:
            if(not current_state.armed and (rospy.Time.now() - last_req) > rospy.Duration(5.0)):
                if(arming_client.call(arm_cmd).success == True):
                    rospy.loginfo("Vehicle armed")

                last_req = rospy.Time.now()

        local_pos_pub.publish(pose)

        rate.sleep()

至此，已经完成对UVA0无人机代码的相关修改。
下面对ROSIP地址互联文件进行修改：
在~/.bashrc文件中增加相关IP配置，如下：

export ROS_MASTER_URI=http://主机IP:11311
export ROS_HOSTNAME=主机IP

注意：如果主控计算机与UAV0无人机是分开的，主控计算机同上配置，而UAV0无人机配置如下：

export ROS_MASTER_URI=http://主机IP:11311
export ROS_HOSTNAME=从机IP
# 下面这句话必须写，不写从机与PX4连接失败
export PX4_SIM_HOST_ADDR=主机IP

我们采用主控与UAV0无人机在同一台设备，所以采用上一种配置方案。到此，主控安装完QGC后，配置完成。
/etc/host文件中添加同一局域网下机器的IP地址可填，可不填。只要确保虚拟机都在同一局域网下即可。
注意：虚拟机请将网络连接更改为桥接模式。

UAV1配置

在第二台虚拟机中，首先完成~/.bashrc文件配置。
UAV1所在虚拟器作为从机，文件配置选择上述两种方式的后一种即可。

下一步修改第二台PX4项目的multi_uav_mavros_sitl.launch文件，注释GAZEBO仿真服务器，注释UAV0及UAV2，如下：

<?xml version="1.0"?>
<launch>
    <!-- MAVROS posix SITL environment launch script -->
    <!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
    <!-- vehicle model and world -->
    <arg name="est" default="ekf2"/>
    <arg name="vehicle" default="iris"/>
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
    <!-- gazebo configs -->
    <arg name="gui" default="true"/>
    <arg name="debug" default="false"/>
    <arg name="verbose" default="false"/>
    <arg name="paused" default="false"/>
    <!-- Gazebo sim -->
    <!-- <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="gui" value="$(arg gui)"/>
        <arg name="world_name" value="$(arg world)"/>
        <arg name="debug" value="$(arg debug)"/>
        <arg name="verbose" value="$(arg verbose)"/>
        <arg name="paused" value="$(arg paused)"/>
    </include> -->
    <!-- UAV0 -->
    <!-- <group ns="uav0">

        <arg name="ID" value="0"/>
        <arg name="fcu_url" default="udp://:14540@localhost:14580"/>

        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
            <arg name="x" value="0"/>
            <arg name="y" value="0"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="$(arg vehicle)"/>
            <arg name="mavlink_udp_port" value="14560"/>
            <arg name="mavlink_tcp_port" value="4560"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
        </include>

        <include file="$(find mavros)/launch/px4.launch">

            <arg name="fcu_url" value="$(arg fcu_url)"/>

            <arg name="gcs_url" value=""/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group> -->

    <group ns="uav1">

        <arg name="ID" value="1"/>
        <!--
            fcu_url 如果设置为localhost则与本地PX4飞控连接
                    如果设置为对应PX4飞控所在IP,则与对应机器的PX4连接
         -->
        <arg name="fcu_url" default="udp://:14541@localhost:14581"/>
        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
            <arg name="x" value="1"/>
            <arg name="y" value="0"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="$(arg vehicle)"/>
            <arg name="mavlink_udp_port" value="14561"/>
            <arg name="mavlink_tcp_port" value="4561"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
        </include>

        <include file="$(find mavros)/launch/px4.launch">
            <arg name="fcu_url" value="$(arg fcu_url)"/>
            <arg name="gcs_url" value="udp://@QGC所在机器IP地址"/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group>

    <!-- <group ns="uav2">

        <arg name="ID" value="2"/>
        <arg name="fcu_url" default="udp://:14542@localhost:14582"/>

        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
            <arg name="x" value="0"/>
            <arg name="y" value="1"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="$(arg vehicle)"/>
            <arg name="mavlink_udp_port" value="14562"/>
            <arg name="mavlink_tcp_port" value="4562"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
            <arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
            <arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
        </include>

        <include file="$(find mavros)/launch/px4.launch">
            <arg name="fcu_url" value="$(arg fcu_url)"/>
            <arg name="gcs_url" value=""/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group> -->
</launch>
<!-- to add more UAVs (up to 10):
Increase the id
Change the name space
Set the FCU to default="udp://:14540+id@localhost:14550+id"
Set the malink_udp_port to 14560+id) -->

这里的fcu_url设置为localhost是保证与第二机器的PX4连接，如果想和别的PX4机器连接，则设置为对应IP，此处应保证端口不重复。
gcs_url设置为QGC所在机器IP地址。如果上述的UAV0和主控不在同一台计算机，则也应该修改为主控IP。

下一步修改对应offboard模式代码，将上述部分修改为：/UAV0 -> /UAV1 即可。
注意：在设置无人机的移动点部分：

	pose = PoseStamped()

    pose.pose.position.x = 0
    pose.pose.position.y = 0
    pose.pose.position.z = 2

最好设置与UVA0无人机不同的值，避免发生碰撞。

执行

offboard代码完全遵循PX4官方教程，以下代码同PX4。
1、UAV0所在虚拟机首先启动QGC
2、执行代码：roslaunch offboard_py start_offb.launch
3、切换至UAV1所在虚拟机，执代码：roslaunch offboard_py start_offb.launch
至此，可以在GAZEBO中看到两架无人机起飞后悬停。
在QGC自动连接两架无人机，并控制模式切换等操作。

写在最后，这种方式仅作为模拟真机部署，集群编队不作用途！！！
这种方式可能实现机间通讯更直观，方便后期无人机内部通讯、信息共享等。
有缘再会。。。。。。

别忘了这是python代码，得要执行权限哦！！！

欣苦

关注

4
点赞
踩
12

收藏

觉得还不错? 一键收藏
1
评论
分布式 ROS PX4 GAZEBO 多机仿真服务器-客户端模式

修改相关文件：PX4-AUTOPILOT项目中launch文件夹下的multi_uav_mavros_sitl.launch文件。注意：官方的教程仅适用于mavros_posix_sitl.launch为启动文件的单无人机控制。当前配置：两个ubuntu20.04虚拟机都应安装完整的ROS及PX4开发工具，包括ROS，PX4，GAZEBO，MAVROS等。这里的fcu_url设置为localhost是保证与第二机器的PX4连接，如果想和别的PX4机器连接，则设置为对应IP，此处应保证端口不重复。
复制链接

扫一扫