PX4 仿真环境开发整理

博客地址：https://www.cnblogs.com/zylyehuo/

最终效果图

（一）PX4 仿真开发

搭建仿真环境

运行案例（C++）

（二）添加键盘控制

/home/yehuo/project_ws/change_mode.sh

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

import rospy

from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseStamped
from mavros_msgs.srv import *
from mavros_msgs.msg import State
import math
import sys, select, termios, tty

# 空格：降落
# 5  ：开启offboard模式
# 6  ：解锁，准备起飞
# 7  ：起飞

msg = """
Control Your Turtlebot!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
space key, k : force stop
anything else : stop smoothly
b : switch to OmniMode/CommonMode
CTRL-C to quit
"""
Omni = 0 #全向移动模式

#键值对应移动/转向方向
moveBindings = {
        'i':( 1, 0),
        'o':( 1,-1),
        'j':( 0, 1),
        'l':( 0,-1),
        'u':( 1, 1),
        ',':(-1, 0),
        '.':(-1, 1),
        'm':(-1,-1),
           }

#键值对应速度增量
speedBindings={
        'q':(1.1,1.1),
        'z':(0.9,0.9),
        'w':(1.1,1),
        'x':(0.9,1),
        'e':(1,  1.1),
        'c':(1,  0.9),
          }

#获取键值函数
def getKey():
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key


speed = 0.2 #默认移动速度 m/s
turn  = 1   #默认转向速度 rad/s
#以字符串格式返回当前速度
def vels(speed,turn):
    return "currently:\tspeed %s\tturn %s " % (speed,turn)

sita = 0.0  # 朝向
z = 0
w = 0
zf = 1
# 回调函数:订阅无人机位姿
def pose_cb(m):
    global sita
    global z
    global w
    global zf
    z = m.pose.orientation.z
    w = m.pose.orientation.w
    # 计算朝向在x轴的上方还是下方
    if z*w > 0:
        zf = 1
    else:
        zf = -1
    sita = 2*math.acos(w)*180/math.pi
    # rospy.loginfo('%.2f\r',sita)

current_state = State()
# 回调函数：订阅mavros状态
def state_cb(state):
    global current_state
    current_state = state

#主函数
if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin) #获取键值初始化，读取终端相关属性
    
    rospy.init_node('turtlebot_teleop') #创建ROS节点
    pub = rospy.Publisher('mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=5) #创建速度话题发布者
    # 订阅无人机位姿
    rospy.Subscriber('mavros/local_position/pose',PoseStamped, pose_cb)

    # 订阅mavros状态
    rospy.Subscriber('mavros/state',State,state_cb)

    # 定义起飞降落服务客户端（起飞，降落）
    setModeServer = rospy.ServiceProxy('mavros/set_mode',SetMode)

    armServer = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)

    x      = 0   #前进后退方向
    y      = 0   #左右移动方向
    z      = 0   #上下移动方向
    th     = 0   #转向/横向移动方向
    count  = 0   #键值不再范围计数
    target_speed = 0 #前进后退目标速度
    target_z_speed = 0 #上下运动目标速度
    target_turn  = 0 #转向目标速度
    control_speed = 0 #前进后退实际控制速度
    control_z_speed = 0 #上下运动实际控制速度
    control_turn  = 0 #转向实际控制速度
    try:
        print(msg) #打印控制说明
        print(vels(speed,turn)) #打印当前速度
        while(1):
            key = getKey() #获取键值

            # if key:
            #     print('key = ',key)
            
            #判断键值是否在移动/转向方向键值内
            # if key in moveBindings.keys():
            #     x  = moveBindings[key][0]
            #     th = moveBindings[key][1]
            #     count = 0

            if key == 'i':   #前进
                count = 0
                x = 1
                z = 0
            elif key == ',': #后退 
                count = 0
                x = -1
                z = 0
            elif key == 'j': #往左转
                count = 0
                th = 1
                z = 0
            elif key == 'l': #往右转
                count = 0
                th = -1
                z = 0
            elif key == 'r': #上升
                count = 0
                z = 1
            elif key == 'f': #下降
                count = 0
                z = -1
            #判断键值是否在速度增量键值内
            elif key in speedBindings.keys():
                speed = speed * speedBindings[key][0]
                turn  = turn  * speedBindings[key][1]
                count = 0
                print(vels(speed,turn)) #速度发生变化，打印出来

            #空键值/'k',相关变量置0
            elif key == 'k' :
                x  = 0
                y  = 0
                z  = 0
                th = 0
                control_speed = 0
                control_z_speed = 0
                control_turn  = 0

            # 降落
            elif key == ' ':
                print("Vehicle Land")
                setModeServer(custom_mode='AUTO.LAND')
            # 开启offboard模式
            elif key == '5':
                if current_state.mode != "OFFBOARD" :
                    setModeServer(custom_mode='OFFBOARD')
                    print("Offboard enabled")
            # 解锁，准备起飞
            elif key == '6':
                armServer(True) 
                print("Vehicle armed")
            # 起飞
            elif key == '7':
                print("Vehicle Takeoff")
                setModeServer(custom_mode='AUTO.TAKEOFF')

            #长期识别到不明键值，相关变量置0
            else:
                count = count + 1
                if count > 4:
                    x  = 0
                    y  = 0
                    z  = 0
                    th = 0
                if (key == '\x03'):
                    break

            #根据速度与方向计算目标速度
            target_speed = speed * x
            target_z_speed = speed * z
            target_turn  = turn * th

            #x方向平滑控制，计算前进后退实际控制速度
            if target_speed > control_speed:
                control_speed = min( target_speed, control_speed + 0.1 )
            elif target_speed < control_speed:
                control_speed = max( target_speed, control_speed - 0.1 )
            else:
                control_speed = target_speed
            
            #z方向平滑控制，实际控制速度
            if target_z_speed > control_z_speed:
                control_z_speed = min( target_z_speed, control_z_speed + 0.1 )
            elif target_z_speed < control_z_speed:
                control_z_speed = max( target_z_speed, control_z_speed - 0.1 )
            else:
                control_z_speed = target_z_speed

            #平滑控制，计算转向实际控制速度
            if target_turn > control_turn:
                control_turn = min( target_turn, control_turn + 0.5 )
            elif target_turn < control_turn:
                control_turn = max( target_turn, control_turn - 0.5 )
            else:
                control_turn = target_turn
         
            # 计算出y方向的sin值
            y_sita = math.sin(sita/180*math.pi)
            # 如果小于0，则改为正数
            if y_sita < 0:
                y_sita = -y_sita
            # 乘以y分量的正负（通过四元数z*w获得，z*w>0,y分量在x轴上方）
            y_sita = y_sita * zf

            twist = Twist()  #创建ROS速度话题变量
            twist.linear.x = control_speed * math.cos(sita/180*math.pi)
            twist.linear.y = control_speed * y_sita  # 朝向速度乘以y轴sin值
            twist.linear.z = control_z_speed
            twist.angular.x = 0
            twist.angular.y = 0
            twist.angular.z = control_turn

            pub.publish(twist) #ROS发布速度话题

    #运行出现问题则程序终止并打印相关错误信息
    except Exception as e:
        print(e)

    #程序结束前发布速度为0的速度话题
    finally:
        twist = Twist()
        twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn
        pub.publish(twist)

    #程序结束前设置终端相关属性
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

（三）添加相机

/home/yehuo/下载/PX4_Firmware/launch/mavros_posix_sitl.launch

<?xml version="1.0"?>
<launch>
    <!-- MAVROS posix SITL environment launch script -->
    <!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle -->
    <!-- vehicle pose -->
    <arg name="x" default="0"/>
    <arg name="y" default="0"/>
    <arg name="z" default="0"/>
    <arg name="R" default="0"/>
    <arg name="P" default="0"/>
    <arg name="Y" default="0"/>
    <!-- vehicle model and world -->
    <arg name="est" default="ekf2"/>
    <arg name="vehicle" default="iris"/>
    <arg name="my_model" default="iris_depth_camera"/>
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
    <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
    <!-- gazebo configs -->
    <arg name="gui" default="true"/>
    <arg name="debug" default="false"/>
    <arg name="verbose" default="false"/>
    <arg name="paused" default="false"/>
    <arg name="respawn_gazebo" default="false"/>
    <!-- MAVROS configs -->
    <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
    <arg name="respawn_mavros" default="false"/>
    <!-- PX4 configs -->
    <arg name="interactive" default="true"/>
    <!-- PX4 SITL and Gazebo -->
    <include file="$(find px4)/launch/posix_sitl.launch">
        <arg name="x" value="$(arg x)"/>
        <arg name="y" value="$(arg y)"/>
        <arg name="z" value="$(arg z)"/>
        <arg name="R" value="$(arg R)"/>
        <arg name="P" value="$(arg P)"/>
        <arg name="Y" value="$(arg Y)"/>
        <arg name="world" value="$(arg world)"/>
        <arg name="vehicle" value="$(arg vehicle)"/>
        <!-- <arg name="sdf" value="$(arg sdf)"/> -->
        <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg my_model)/$(arg my_model).sdf"/>
        <arg name="gui" value="$(arg gui)"/>
        <arg name="interactive" value="$(arg interactive)"/>
        <arg name="debug" value="$(arg debug)"/>
        <arg name="verbose" value="$(arg verbose)"/>
        <arg name="paused" value="$(arg paused)"/>
        <arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
    </include>
    <!-- MAVROS -->
    <include file="$(find mavros)/launch/px4.launch">
        <!-- GCS link is provided by SITL -->
        <arg name="gcs_url" value=""/>
        <arg name="fcu_url" value="$(arg fcu_url)"/>
        <arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
    </include>
</launch>

（四）基于ar_track_alvar的二维码识别

/home/yehuo/project_ws/src/install/ar_track_alvar/share/ar_track_alvar/launch/ar_track_camera.launch

<launch>
	<node pkg="tf" type="static_transform_publisher" name="world_to_cam" args="0 0 0.5 0 1.57 0 world camera_link 10" />

	<arg name="marker_size" default="5" />
	<arg name="max_new_marker_error" default="0.08" />
	<arg name="max_track_error" default="0.2" />
	<arg name="cam_image_topic" default="/camera/rgb/image_raw" />
	<arg name="cam_info_topic" default="/camera/rgb/camera_info" />
	<arg name="output_frame" default="/camera_link" />

	<node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen">
		<param name="marker_size"           type="double" value="$(arg marker_size)" />
		<param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
		<param name="max_track_error"       type="double" value="$(arg max_track_error)" />
		<param name="output_frame"          type="string" value="$(arg output_frame)" />

		<remap from="camera_image"  to="$(arg cam_image_topic)" />
		<remap from="camera_info"   to="$(arg cam_info_topic)" />
	</node>

	<!--rviz view /-->
	<node pkg="rviz" type="rviz" name="rviz" args="-d /home/yehuo/project_ws/src/ar_track_alvar/ar_track_alvar/config/ar_track_camera.rviz"/>
</launch>

/home/yehuo/project_ws/src/ar_track_alvar/ar_track_alvar/config/ar_track_camera.rviz

Panels:
  - Class: rviz/Displays
    Help Height: 0
    Name: Displays
    Property Tree Widget:
      Expanded: ~
      Splitter Ratio: 0.5
    Tree Height: 298
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
    Expanded:
      - /2D Pose Estimate1
      - /2D Nav Goal1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.588679016
  - Class: rviz/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
  - Class: rviz/Time
    Experimental: false
    Name: Time
    SyncMode: 0
    SyncSource: Camera
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.0299999993
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Class: rviz/TF
      Enabled: true
      Frame Timeout: 15
      Frames:
        All Enabled: true
        ar_marker_0:
          Value: true
        ar_marker_1:
          Value: true
        ar_marker_192:
          Value: true
        ar_marker_2:
          Value: true
        ar_marker_4:
          Value: true
        ar_marker_5:
          Value: true
        ar_marker_7:
          Value: true
        ar_marker_8:
          Value: true
        usb_cam:
          Value: true
        world:
          Value: true
      Marker Scale: 0.300000012
      Name: TF
      Show Arrows: true
      Show Axes: true
      Show Names: true
      Tree:
        world:
          usb_cam:
            ar_marker_0:
              {}
            ar_marker_1:
              {}
            ar_marker_192:
              {}
            ar_marker_2:
              {}
            ar_marker_4:
              {}
            ar_marker_5:
              {}
            ar_marker_7:
              {}
            ar_marker_8:
              {}
      Update Interval: 0
      Value: true
    - Class: rviz/Marker
      Enabled: true
      Marker Topic: /visualization_marker
      Name: Marker
      Namespaces:
        basic_shapes: true
      Queue Size: 100
      Value: true
    - Class: rviz/Camera
      Enabled: true
      Image Rendering: background and overlay
      Image Topic: /camera/rgb/image_raw
      Name: Camera
      Overlay Alpha: 0.5
      Queue Size: 2
      Transport Hint: raw
      Unreliable: false
      Value: true
      Visibility:
        Grid: true
        Marker: true
        TF: true
        Value: true
      Zoom Factor: 1
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Fixed Frame: world
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz/Interact
      Hide Inactive Objects: true
    - Class: rviz/MoveCamera
    - Class: rviz/Select
    - Class: rviz/FocusCamera
    - Class: rviz/Measure
    - Class: rviz/SetInitialPose
      Topic: /initialpose
    - Class: rviz/SetGoal
      Topic: /move_base_simple/goal
    - Class: rviz/PublishPoint
      Single click: true
      Topic: /clicked_point
  Value: true
  Views:
    Current:
      Class: rviz/Orbit
      Distance: 1.3831408
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.0599999987
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
        X: 0.158190101
        Y: -0.0226284917
        Z: 0.342923284
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.0500000007
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.00999999978
      Pitch: 0.484797359
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz)
      Yaw: 2.52039075
    Saved: ~
Window Geometry:
  Camera:
    collapsed: false
  Displays:
    collapsed: false
  Height: 846
  Hide Left Dock: false
  Hide Right Dock: true
  QMainWindow State: 000000ff00000000fd000000040000000000000231000002c4fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000016b000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000199000001530000001600ffffff000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005b60000003efc0100000002fb0000000800540069006d00650100000000000005b60000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000037f000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: true
  Width: 1462
  X: 268
  Y: 154

Rviz 界面截图

（五）在gazebo中贴图二维码

my_ground_plane.material

material MyGroundPlane/Image
        {
          technique
          {
            pass
            {
              ambient 1 1 1 1.000000
              diffuse 1 1 1 1.000000
              specular 0.03 0.03 0.03 1.000000 
 
              texture_unit
              {
                texture MarkerData_0.png
              }
            }
          }
        }

model.sdf

<?xml version="1.0" encoding="UTF-8"?>
<sdf version="1.4">
   <model name="my_ground_plane">
      <static>true</static>
      <link name="link">
         <collision name="collision">
            <geometry>
               <plane>
                  <normal>0 0 1</normal>
                  <size>1 1</size>
               </plane>
            </geometry>
            <surface>
               <friction>
                  <ode>
                     <mu>1</mu>
                     <mu2>0.5</mu2>
                  </ode>
               </friction>
            </surface>
         </collision>
         <visual name="visual">
            <cast_shadows>false</cast_shadows>
            <geometry>
               <plane>
                  <normal>0 0 1</normal>
                  <size>1 1</size>
               </plane>
            </geometry>
            <material>
               <script>
                  <uri>model://my_ground_plane/materials/scripts</uri>
                  <uri>model://my_ground_plane/materials/textures/</uri>
                  <name>MyGroundPlane/Image</name>
               </script>
            </material>
         </visual>
      </link>
   </model>
</sdf>

model.config

<?xml version="1.0" encoding="UTF-8"?>
<model>
   <name>My Ground Plane</name>
   <version>1.0</version>
   <sdf version="1.4">model.sdf</sdf>
   <description>My textured ground plane.</description>
</model>

效果图

（六）终端运行指令罗列

~$ roscore

~$ rosrun usb_cam usb_cam_node

~$ roslaunch px4 mavros_posix_sitl.launch

~/project_ws$ ./change_mode.sh

~$ roslaunch ar_track_alvar ar_track_camera.launch

（七）问题汇总

posted @ 2025-03-21 10:18  zylyehuo  阅读(308)  评论(0)    收藏  举报