双无人机巡航

场景简介

• 本实验通过两架无人机起飞后按照既定路线在地图上方巡航并最终降落

实验室资源方式简介

进入实操前，请确保阿里云账号满足以下条件：

• 个人账号资源

  • 使用您个人的云资源进行操作，资源归属于个人。

  • 平台仅提供手册参考，不会对资源做任何操作。

• 确保已完成云工开物300元代金券领取。

• 已通过实名认证且账户余额≥100元。

本次实验将在您的账号下开通实操所需计算型实例规格族c7a，费用约为：3元/时（具体金额取决于实验完成的时间），需要您通过阿里云云工开物学生专属300元抵扣金兑换本次实操的云资源。

如果您调整了资源规格、使用时长，或执行了本方案以外的操作，可能导致费用发生变化，请以控制台显示的实际价格和最终账单为准。

领取专属权益及创建实验资源

在开始实验之前，请先点击右侧屏幕的“进入实操”再进行后续操作

本次实验需要您通过领取阿里云云工开物学生专属300元抵扣券兑换本次实操的云资源，如未领取请先点击领取。（若已领取请跳过）

实验步骤

• 1、服务部署

  • 点击链接，进入部署页面

  • 按下图设置【服务实例名称】、【地域】、【实例密码】

    • 服务实例名称：test（可自定义命名）

    • 地域：华东2（上海）

    • 实例密码：Sjtu@520

      说明

      输入实例密码时请注意大小写，请记住您设置的实例名称及对应密码，后续实验过程会用到。

      

  • 完成填写后点击【下一步：确认订单】

    

  • 核对实例信息及价格预览，无误请点击【立即创建】

    

    重要

    领取300元优惠券后，资源应为0元/小时，且会提示【您当前账户的余额充足】！若提示余额不足等，请检查是否正确领取优惠券

  • 创建成功，点击【去列表查看】

    

  • 查看实例，并等待部署

    

    点击左侧的图标展开目录，选择目录中的【云服务器ECS】

    

  • 云服务器ECS—实例—远程连接

    

  • 下拉展开更多登录方式，选择【通过VNC远程连接】

    

  • 点击ecs-user，并输入之前设置的实例密码：Sjtu@520，回车

    

  • 进入镜像后，点击左侧第2个图标，打开文件夹后点击catkin_ws文件夹

    

  • 进入catkin_ws文件夹，在空白处点击鼠标右键，选择Open in Terminal

    

    

• 2、代码部分

  生成gazebo仿真环境需要调用spawn.launch

  <launch>
  
    <!-- 定义启动文件中的参数 -->
    <arg name="paused" default="false"/> <!-- 是否暂停仿真，默认为false -->
    <arg name="use_sim_time" default="true"/> <!-- 是否使用仿真时间，默认为true -->
    <arg name="gui" default="true"/> <!-- 是否启动GUI，默认为true -->
    <arg name="headless" default="false"/> <!-- 是否无头模式，默认为false -->
    <arg name="debug" default="false"/> <!-- 是否调试模式，默认为false -->
    <arg name="verbose" default="false"/> <!-- 是否详细输出，默认为false -->
    <arg name="model" value="$(find site_model)/urdf/car.urdf"/> <!-- 机器人的URDF模型路径 -->
  
    <!-- Start gazebo and load the world -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" >
      <arg name="paused" value="$(arg paused)"/>
      <arg name="use_sim_time" value="$(arg use_sim_time)"/>
      <arg name="gui" value="$(arg gui)"/>
      <arg name="headless" value="$(arg headless)"/>
      <arg name="debug" value="$(arg debug)"/>
      <arg name="verbose" value="$(arg verbose)"/>
    </include>
  
    <!-- Spawn the site -->
    <node
      name="tf_footprint_base"
      pkg="tf"
      type="static_transform_publisher"
      args="0 0 0 0 0 0 base_link base_footprint 40" />
    <arg name="gpu" default="false"/>
    <arg name="organize_cloud" default="false"/>
    <param
      name="robot_description"
      command="$(find xacro)/xacro --inorder '$(find site_model)/urdf/site_model.urdf.xacro' organize_cloud:=$(arg organize_cloud) gpu:=$(arg gpu)" />
    <node
      pkg="gazebo_ros"
      type="spawn_model"
      name="spawn_model"
      args="-urdf -param /robot_description -model site_model"/>
  
    <node
      pkg="robot_state_publisher"
      type="robot_state_publisher"
      name="robot_state_publisher">
      <param
        name="publish_frequency"
        type="double"
        value="30.0" />
    </node>
    <node
      name="fake_joint_calibration"
      pkg="rostopic"
      type="rostopic"
      args="pub /calibrated std_msgs/Bool true"/>
  
      <!-- 从yaml文件加载联合控制器的参数 -->
  <rosparam file="$(find pkg)/config/ctrl.yaml" command="load"/>
  
  <!-- 加载机器人模型描述参数 -->
  <group ns="car1">
  <param name="robot_description" textfile="$(find pkg)/urdf/car1.urdf"/>
  <node name= "robot_state_publisher" pkg= "robot_state_publisher" type= "robot_state_publisher">
  <param name="publish_frequency" type="double" value="20.0" />
  </node>
      <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
            args="-urdf -model car1 -param robot_description -x -1.5 -y -3 -z 0.05"/> 
  <node name="controller_manager" pkg="controller_manager" type="spawner" 
        respawn="true" output="screen" ns="/car1" 
        args="left_rear_wheel_velocity_controller       right_rear_wheel_velocity_controller
              left_front_wheel_velocity_controller      right_front_wheel_velocity_controller
              left_steering_hinge_position_controller   right_steering_hinge_position_controller
              joint_state_controller"/>
  </group>
  
    <group ns="car2">
      <param name="robot_description" textfile="$(find pkg)/urdf/car2.urdf"/>
      <node name= "robot_state_publisher" pkg= "robot_state_publisher" type= "robot_state_publisher">
        <param name="publish_frequency" type="double" value="20.0" />
      </node>
        <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
              args="-urdf -model car2 -param robot_description -x -1.5 -y -4 -z 0.05"/> 
      <node name="controller_manager" pkg="controller_manager" type="spawner" 
            respawn="true" output="screen" ns="/car2" 
            args="left_rear_wheel_velocity_controller       right_rear_wheel_velocity_controller
                  left_front_wheel_velocity_controller      right_front_wheel_velocity_controller
                  left_steering_hinge_position_controller   right_steering_hinge_position_controller
                  joint_state_controller"/>
      </group>
  
    <node pkg="site_model" type="ServoCommands.py" name="ServoCommands" output="screen"/>
  
  </launch>
  

  

  在gazebo仿真环境中生成两架无人机需要调用uav2.launch

  <?xml version="1.0"?>
  <launch>
      <!-- vehicle model and world -->
      <arg name="vehicle" default="iris"/>
      <arg name="my_model" default="iris_fpv_cam"/>
      <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
      
      <env name="PX4_SIM_MODEL" value="gazebo-classic_$(arg vehicle)" />
  
      <group ns="uav0">
          <!-- MAVROS and vehicle configs -->
          <arg name="ID" value="0"/>
          <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
          <!-- PX4 SITL and vehicle spawn -->
          <include file="$(find px4)/launch/single_vehicle_spawn.launch">
              <!-- arg name="x" value="1.825"/ -->
              <arg name="x" value="2.327"/>
              <arg name="y" value="3.303"/>
              <arg name="z" value="0.12"/>
              <arg name="R" value="0.00"/>
              <arg name="P" value="0.00"/>
              <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">
              <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">
          <!-- MAVROS and vehicle configs -->
          <arg name="ID" value="1"/>
          <arg name="fcu_url" default="udp://:14541@localhost:14558"/>
          <!-- PX4 SITL and vehicle spawn -->
          <include file="$(find px4)/launch/single_vehicle_spawn.launch">
              <!-- arg name="x" value="-1.63"/ -->
              <arg name="x" value="-2.394"/>
              <arg name="y" value="-5.411"/>
              <arg name="z" value="0.12"/>
              <arg name="R" value="0.00"/>
              <arg name="P" value="0.00"/>
              <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>
          <!-- MAVROS -->
          <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>
  
  

  

  场景生成完毕后，分别开启两架无人机的巡航程序iris0_cruise.py和iris1_cruise.py，以iris0_cruise.py为例。主函数中定义一系列参数，订阅无人机有关的ros消息和服务，随后控制无人机起飞，当无人机成功起飞后控制无人机的巡航。

  #主函数
  if __name__=="__main__":
      global target_position, target_count, refresh
      target_position = [[-4.8,-2.25],[0,-4.5],[-4.8,-6.75],[0,-9]] #目标点路径
      target_count = 0
      refresh = False
      settings = termios.tcgetattr(sys.stdin) #获取键值初始化，读取终端相关属性
      rospy.init_node('uav0_cruise') #创建ROS节点
      
      pub = rospy.Publisher('uav0/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=5) #创建速度话题发布者
      
      current_position = None
      init_position = None
      target_hight = 1.5 #目标高度
      start_follow = False #第一次达到目标高度后开始
      x      = 0   #前进后退方向
      y      = 0   #左右移动方向
      z      = 0   #上下移动方向
      th     = 0   #转向/横向移动方向
      target_x_speed = 0 #前进后退目标速度
      target_y_speed = 0 #左右平移目标速度
      target_z_speed = 0 #上下运动目标速度
      target_turn  = 0 #转向目标速度
      control_x_speed = 0 #前进后退实际控制速度
      control_y_speed = 0 #左右平移实际控制速度
      control_z_speed = 0 #上下运动实际控制速度
      control_turn  = 0 #转向实际控制速度
      
      speed = 1 #默认移动速度 m/s
      turn  = 1   #默认转向速度 rad/s
      
      sita = 0.0  # 朝向
      z = 0
      w = 0
      zf = 1
      # 订阅uav0位姿信息
      rospy.Subscriber('uav0/mavros/local_position/pose',PoseStamped, pose_cb)
  
      # 订阅mavros状态
      current_state = State()
      rospy.Subscriber('uav0/mavros/state',State,state_cb)
  
      # 定义起飞降落服务客户端（起飞，降落）
      setModeServer = rospy.ServiceProxy('uav0/mavros/set_mode',SetMode)
      armServer = rospy.ServiceProxy('uav0/mavros/cmd/arming', CommandBool)
  
      # uav0起飞
      takeoff()
      while True:
          iris0_callback()
      rospy.spin()
      
      #程序结束前设置终端相关属性
      termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
  

  控制无人机起飞需要按照顺序给服务发送命令。

  def takeoff():
      global current_state
      x = -1
      y = 0
      while(1):
          key = getKey() #获取键值,去除后无法起飞？
          if x == 0:
              # 开启offboard模式
              setModeServer(custom_mode='OFFBOARD')
              print("UAV0 Offboard enabled")
              x += 1
          elif x == 1:
              # 解锁，准备起飞
              armServer(True) 
              print("UAV0 Vehicle armed")
              x += 1
              time.sleep(2)
          elif x == 2:
              # 起飞
              print("UAV0 Vehicle Takeoff")
              setModeServer(custom_mode='AUTO.TAKEOFF')
              x += 1
              time.sleep(10)
          elif x == 3:
              # 获得控制权
              setModeServer(custom_mode='OFFBOARD')
              x += 1
              print("UAV0 Offboard enabled")
          elif x == 4:
              break
                  
  
          twist = Twist()  #创建ROS速度话题变量
          pub.publish(twist) #ROS发布速度话题,去除后无法起飞？
          y += 1
          if y == 10:
              x = 0
  

  根据订阅的无人机位姿信息，在起飞阶段start_follow为False时，无人机在上升阶段保持初始坐标。在巡航阶段当到达目标点附近时切换到下一个目标点，当所有目标点都完成巡航时无人机降落。（无人机的起飞和降落点都不能位于仿真环境中site_model模型上，否则会出现错误。）

  def iris0_callback():
      global x, y, z, th, target_x_speed, target_y_speed, target_z_speed, target_turn, control_x_speed, control_y_speed, control_z_speed, control_turn, speed, turn, current_position, init_position, target_hight, start_follow, target_position, target_count, refresh
      #获取到新的无人机位置数据时才会进行下一步
      if not refresh:
          return
          
      
      print("target_position:", target_position[target_count][0], target_position[target_count][1])
      print("current_position:", current_position.x, current_position.y)
      #开始跟随目标点
      if start_follow:
          x = (target_position[target_count][0] - current_position.x) * 0.1
          y = (target_position[target_count][1] - current_position.y) * 0.1
  
      
      # 升高到一定高度时              
      if (target_hight - current_position.z) < 0.2:
          start_follow = True   
      # 无人机保持指定高度飞行
      if (current_position.z - target_hight) > 0.1:
          #print("下降")
          z = -0.1
      elif (current_position.z - target_hight) < -0.1:
          #print("升高")
          z = 0.1
      else:
          z = 0
      
      #print("x:", init_position.x, current_position.x)
      #print("y:", init_position.y, current_position.y)
      #未开始跟随目标点时保持无人机位置在当前位置
      if not start_follow:
          x = (init_position.x - current_position.x) * 0.1
          y = (init_position.y - current_position.y) * 0.1
      refresh = False
      
      th = 0
      #判断是否在目标点附近
      if abs(target_position[target_count][0] - current_position.x) <= 0.25 and abs(target_position[target_count][1] - current_position.y) <= 0.25:
          target_count += 1 #切换到下一个目标点
      #当所有目标点巡航完毕时
      if target_count >= len(target_position):
          for i in range(100):
              setModeServer(custom_mode='AUTO.LAND') #降落
          time.sleep(25)
          for i in range(100):
              armServer(False)
          time.sleep(5)
          rospy.signal_shutdown("~owari~")
          exit(0)
      #根据速度与方向计算目标速度
      target_x_speed = speed * x
      target_y_speed = speed * y
      target_z_speed = speed * z
      target_turn = turn * th
  
  
      #x方向平滑控制，计算前进后退实际控制速度
      if target_x_speed > control_x_speed:
          control_x_speed = min( target_x_speed, control_x_speed + 0.1 )
      elif target_x_speed < control_x_speed:
          control_x_speed = max( target_x_speed, control_x_speed - 0.1 )
      else:
          control_x_speed = target_x_speed
                  
                  
      #y方向平滑控制，计算前进后退实际控制速度
      if target_y_speed > control_y_speed:
          control_y_speed = min( target_y_speed, control_y_speed + 0.1 )
      elif target_y_speed < control_y_speed:
          control_y_speed = max( target_y_speed, control_y_speed - 0.1 )
      else:
          control_y_speed = target_y_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_sin = math.sin(sita/180*math.pi)
      # 如果小于0，则改为正数
      if y_sin < 0:
          y_sin = -y_sin
      # 乘以y分量的正负（通过四元数z*w获得，z*w>0,y分量在x轴上方）
      y_sin = y_sin * zf
  
      twist = Twist()  #创建ROS速度话题变量
      
      #正反朝向前进时
      if x != 0 and y == 0:
          twist.linear.x = control_x_speed * math.cos(sita/180*math.pi)
          twist.linear.y = control_x_speed * y_sin  # 朝向速度乘以y轴sin值
      #左右平移时
      if y != 0 and x == 0:
          twist.linear.x = control_y_speed * -y_sin
          twist.linear.y = control_y_speed * math.cos(sita/180*math.pi)
      #斜向移动？
      if x != 0 and y != 0:
          twist.linear.x = control_x_speed * math.cos(sita/180*math.pi)
          twist.linear.y = control_y_speed * math.cos(sita/180*math.pi)
                  
      twist.linear.z = control_z_speed
      twist.angular.x = 0
      twist.angular.y = 0
      twist.angular.z = control_turn
  
      pub.publish(twist) #ROS发布速度话题
  
  # 回调函数:订阅无人机位姿
  def pose_cb(m):
      global sita, z, w, zf, current_position, init_position, refresh
      #起飞时初始位置
      if init_position is None:
          init_position = m.pose.position
          #print("init_position:", init_position)
      current_position = m.pose.position #当前位置
      #print("current_position:", current_position)
      refresh = True
      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)
  

• 3、实验操作

  在~/catkin_ws文件夹中新建终端，输出以下命令开启程序。

  source devel/setup.bash
  roslaunch site_model spawn.launch
  

  

  在~/catkin_ws文件夹中新建终端，输出以下命令开启程序。

  roslaunch px4 uav2.launch

  

  在~/catkin_ws文件夹中新建两个终端，分别输出以下命令开启程序。

  source devel/setup.bash
  rosrun site_model iris0_cruise.py
  

  source devel/setup.bash
  rosrun site_model iris1_cruise.py
  

  

清理资源

• 计算巢—服务实例—复制服务实例ID，点击【删除】

  

• 在弹窗粘贴服务实例ID，并进行勾选，点击【确定删除】

  

• 完成安全验证后，即可成功释放实例。

  

• 回到云服务器ECS——实例，检查是否成功释放资源

  

关闭实验

• 在完成实验后，点击 结束实操

  

• 点击 取消 回到实验页面，点击 确定 跳转实验评分

  

• 请为本次实验评分，并给出您的建议，点击 确认，结束本次实验