AirSim学习（5）AirSim的C++接口、AirSim与ROS的联合仿真

文章内容

AirSim学习笔记汇总
AirSim的C++接口
AirSim和ROS的联合仿真
- 添加`airsim_ros_pkgs`到工作空间
- 利用C++ API添加角速度推力控制接口
小结

AirSim学习笔记汇总

AirSim的C++接口

在AirSim学习（3）AirSim的PythonAPI基本操作——VehicleClient类和
AirSim学习（4）AirSim的PythonAPI基本操作——MultirotorClient类两篇博客中，AirSim的Python接口已经有详细的讲解，但是在实际应用中，为了追求程序运行效率，开发者往往使用C++编写程序，尤其在ROS开发中更是如此。所幸AirSim提供了C++的接口供我们使用。AirSim无人机仿真的C++接口在AirSim/AirLib/src/vehicles/multirotor/api下，函数名称与Python完全一致，输入输出类型略有区别。
在这里插入图片描述
以角速度推力控制接口为例，输入参数分别为横滚角速度float roll_rate，俯仰角速度float pitch_rate，偏航角速度float yaw_rate，油门float throttle，持续时间float duration，载具名称const std::string& vehicle_name：

MultirotorRpcLibClient* MultirotorRpcLibClient::moveByAngleRatesThrottleAsync(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration, const std::string& vehicle_name)
{
    
    
    pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("moveByAngleRatesThrottle", roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name);
    return this;
}

对比PythonClient中给出的Python API中对应的角速度推力控制接口，可以发现二者几乎完全一致，其余接口读者也可以自行类比，笔者在此就不一一列举

def moveByAngleThrottleAsync(self, pitch, roll, throttle, yaw_rate, duration, vehicle_name = ''):
    logging.warning("moveByAngleThrottleAsync API is deprecated, use moveByRollPitchYawrateThrottleAsync() API instead")
    return self.client.call_async('moveByRollPitchYawrateThrottle', roll, -pitch, -yaw_rate, throttle, duration, vehicle_name)

AirSim和ROS的联合仿真

AirSim提供了与ROS联合仿真的示例程序，位于AirSim/ros/src。其中提供了两个功能包，airsim_ros_pkgs包含AirSim C++ API的 ROS 包装器，而airsim_tutorial_pkgs提供了一些具体的应用。下面的内容将展示如何将airsim_ros_pkgs添加到我们自己的工作空间中，并且使用C++ API实现一些原先没有的功能。

添加`airsim_ros_pkgs`到工作空间

➜  AirSimLearning git:(main) cd src           
➜  src git:(main) ls
airsim_ros_pkgs  CMakeLists.txt

修改CMakeLists.txt的AIRSIM_ROOT为自己的AirSim仓库路径，例如我这里修改为"/home/star/Documents/AirSim"

cmake_minimum_required(VERSION 3.10.0)
project(airsim_ros_pkgs)

# set this to path to AirSim root folder if you want your catkin workspace in a custom directory
set(AIRSIM_ROOT "/home/star/Documents/AirSim")

add_subdirectory("${AIRSIM_ROOT}/cmake/rpclib_wrapper" rpclib_wrapper)
add_subdirectory("${AIRSIM_ROOT}/cmake/AirLib" AirLib)
add_subdirectory("${AIRSIM_ROOT}/cmake/MavLinkCom" MavLinkCom)

确保你的AirSim已经按照AirSim学习（1）安装Unreal Engine和AirSim中成功编译。随后在工作空间执行如下命令编译

catkin_make

即可编译成功。
本文使用的AirSim配置文件在官方示例front_stereo_and_center_mono.json中做了一定修改，复制并替换~/Documents/AirSim/settings.json（记得备份原有的settings.json）：

{
    
    
  "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/main/docs/settings.md",
  "SettingsVersion": 1.2,
  "SimMode": "Multirotor",
  "ViewMode": "SpringArmChase",
  "ClockSpeed": 1.0,
  "Vehicles": {
    
    
    "drone_1": {
    
    
      "VehicleType": "SimpleFlight",
      "DefaultVehicleState": "Armed",
      "EnableCollisionPassthrogh": false,
      "EnableCollisions": true,
      "AllowAPIAlways": true,
      "RC": {
    
    
        "RemoteControlID": 0,
        "AllowAPIWhenDisconnected": false
      },
      "Sensors": {
    
    
        "Imu": {
    
    
          "SensorType": 2,
          "Enabled": true
        },
        "LidarCustom": {
    
    
          "SensorType": 6,
          "Enabled": true,
          "NumberOfChannels": 32,
          "PointsPerSecond": 10000,
          "X": 0,
          "Y": 0,
          "Z": -1,
          "VerticalFOVUpper": 45,
          "VerticalFOVLower": -45,
          "HorizontalFOVStart": -45,
          "HorizontalFOVEnd": 45,
          "DrawDebugPoints": true
        },
        "Gps": {
    
    
          "SensorType": 3,
          "Enabled": true,
          "EphTimeConstant": 0.9,
          "EpvTimeConstant": 0.9,
          "EphInitial": 25,
          "EpvInitial": 25,
          "EphFinal": 0.1,
          "EpvFinal": 0.1,
          "EphMin3d": 3,
          "EphMin2d": 4,
          "UpdateLatency": 0.2,
          "UpdateFrequency": 50,
          "StartupDelay": 1
        },
        "Magnetometer": {
    
    
          "SensorType": 4,
          "Enabled": true,
          "NoiseSigma": 0.005,
          "ScaleFactor": 1,
          "NoiseBias": 0,
          "UpdateLatency": 0,
          "UpdateFrequency": 50,
          "StartupDelay": 0
        }
      },
      "Cameras": {
    
    
        "front_center_custom": {
    
    
          "CaptureSettings": [
            {
    
    
              "PublishToRos": 1,
              "ImageType": 0,
              "Width": 640,
              "Height": 480,
              "FOV_Degrees": 27,
              "DepthOfFieldFstop": 2.8,
              "DepthOfFieldFocalDistance": 200.0,
              "DepthOfFieldFocalRegion": 200.0,
              "TargetGamma": 1.5
            }
          ],
          "X": 0.50,
          "Y": 0,
          "Z": 0.10,
          "Pitch": 0,
          "Roll": 0,
          "Yaw": 0
        },
        "front_left_custom": {
    
    
          "CaptureSettings": [
            {
    
    
              "PublishToRos": 1,
              "ImageType": 0,
              "Width": 672,
              "Height": 376,
              "FOV_Degrees": 90,
              "TargetGamma": 1.5
            },
            {
    
    
              "PublishToRos": 1,
              "ImageType": 1,
              "Width": 672,
              "Height": 376,
              "FOV_Degrees": 90,
              "TargetGamma": 1.5
            }
          ],
          "X": 0.50,
          "Y": -0.06,
          "Z": 0.10,
          "Pitch": 0.0,
          "Roll": 0.0,
          "Yaw": 0.0
        },
        "front_right_custom": {
    
    
          "CaptureSettings": [
            {
    
    
              "PublishToRos": 1,
              "ImageType": 0,
              "Width": 672,
              "Height": 376,
              "FOV_Degrees": 90,
              "TargetGamma": 1.5
            }
          ],
          "X": 0.50,
          "Y": 0.06,
          "Z": 0.10,
          "Pitch": 0.0,
          "Roll": 0.0,
          "Yaw": 0.0
        }
      },
      "X": 2,
      "Y": 0,
      "Z": 0,
      "Pitch": 0,
      "Roll": 0,
      "Yaw": 0
    }
  },
  "SubWindows": [
    {
    
    
      "WindowID": 0,
      "ImageType": 0,
      "CameraName": "front_left_custom",
      "Visible": true
    },
    {
    
    
      "WindowID": 1,
      "ImageType": 0,
      "CameraName": "front_center_custom",
      "Visible": false
    },
    {
    
    
      "WindowID": 2,
      "ImageType": 0,
      "CameraName": "front_right_custom",
      "Visible": true
    }
  ]
}

启动UE工程，然后运行如下命令启动ROS节点。

source devel/setup.zsh
roslaunch airsim_ros_pkgs airsim_node.launch

在另一个终端中切换到当前工作空间，执行如下命令

source devel/setup.zsh
rostopic list

即可看到如下输出：
在这里插入图片描述
右侧的这些话题就是AirSim发布的，我们可以利用这些话题对无人机进行控制，例如发布速度控制指令，效果如下：

AirSim ROS联合仿真——速度控制接口

利用C++ API添加角速度推力控制接口

在无人机控制当中，我们常常用到无人机的角速度-推力控制模式，这种模式更适合无人机大机动飞行，但是在原有的airsim_ros_pkgs中没有提供这一接口，可以使用AirSim的C++ API实现这一功能
上述角速度推力控制API需要三轴角速度、推力、持续时间、载具名称，因此首先需要添加能够满足这种需求的消息类型，将如下的AngleRateThrottle.msg添加到airsim_ros_pkgs的msg目录下并且对应地修改CMakeLists.txt

float64 rollRate
float64 pitchRate
float64 yawRate
float64 throttle

在airsim_ros_wrapper.h中做如下修改

#include <airsim_ros_pkgs/AngleRateThrottle.h>
...
struct AngleRateThrCmd
{
    
    
    std::string vehicle_name;
    double rollRate;
    double pitchRate;
    double yawRate;
    double throttle;
};
...
 class MultiRotorROS : public VehicleROS
    {
    
    
    public:
        ...
        ros::Subscriber angleRateThr_cmd_sub;
		...
        bool has_angleRateThr_cmd;
        AngleRateThrCmd angleRateThr_cmd;
    };
...
	void angleRateThr_cmd_cb(const airsim_ros_pkgs::AngleRateThrottle::ConstPtr& msg, const std::string& vehicle_name);
...
	private:
	...
	/// ROS params
    double control_cmd_duration_;
    ...

笔者为了通用起见，将原有的速度指令持续时间double vel_cmd_duration_;修改为double control_cmd_duration_;
然后在airsim_ros_wrapper.cpp中做如下修改。其中angleRateThr_cmd_cb回调函数在ros::Subscriber angleRateThr_cmd_sub接收到消息时更新角速度推力控制命令，并且将标志位has_angleRateThr_cmd设为true；update_commands函数在标志位has_angleRateThr_cmd为true且has_vel_cmd为false时调用C++ API moveByAngleRatesThrottleAsync输入接收到的角速度推力控制指令。

void AirsimROSWrapper::initialize_ros()
{
    
    

    ...
    control_cmd_duration_ = 0.05; // todo rosparam
    ...
}
void AirsimROSWrapper::create_ros_pubs_from_settings_json()
{
    
    
...
			drone->vel_cmd_world_frame_sub = nh_private_.subscribe<airsim_ros_pkgs::VelCmd>(
                curr_vehicle_name + "/vel_cmd_world_frame",
                1,
                boost::bind(&AirsimROSWrapper::vel_cmd_world_frame_cb, this, _1, vehicle_ros->vehicle_name));

            // Angle rate throttle control cmd subscriber
            drone->angleRateThr_cmd_sub = nh_private_.subscribe<airsim_ros_pkgs::AngleRateThrottle>(
                curr_vehicle_name + "/AngleRateThrottleCmd",
                1,
                boost::bind(&AirsimROSWrapper::angleRateThr_cmd_cb, this, _1, vehicle_ros->vehicle_name));
...
}
...
// Angle Rate Throttle control interface
void AirsimROSWrapper::angleRateThr_cmd_cb(const airsim_ros_pkgs::AngleRateThrottle::ConstPtr &msg, const std::string& vehicle_name)
{
    
    
    std::lock_guard<std::mutex> guard(drone_control_mutex_);
    auto drone = static_cast<MultiRotorROS *>(vehicle_name_ptr_map_[vehicle_name].get());
    drone->angleRateThr_cmd.rollRate = msg->rollRate;
    drone->angleRateThr_cmd.pitchRate = msg->pitchRate;
    drone->angleRateThr_cmd.yawRate = msg->yawRate;
    drone->angleRateThr_cmd.throttle = msg->throttle;
    drone->has_angleRateThr_cmd = true;
}
...
void AirsimROSWrapper::update_commands()
{
    
    
    for (auto &vehicle_name_ptr_pair : vehicle_name_ptr_map_)
    {
    
    
        auto &vehicle_ros = vehicle_name_ptr_pair.second;

        if (airsim_mode_ == AIRSIM_MODE::DRONE)
        {
    
    
            auto drone = static_cast<MultiRotorROS *>(vehicle_ros.get());

            // send control commands from the last callback to airsim
            if (drone->has_vel_cmd)
            {
    
    
                std::lock_guard<std::mutex> guard(drone_control_mutex_);
                get_multirotor_client()->moveByVelocityAsync(drone->vel_cmd.x,
                                                             drone->vel_cmd.y,
                                                             drone->vel_cmd.z,
                                                             control_cmd_duration_,
                                                             msr::airlib::DrivetrainType::MaxDegreeOfFreedom,
                                                             drone->vel_cmd.yaw_mode,
                                                             drone->vehicle_name);
            }
            else if (drone->has_angleRateThr_cmd)
            {
    
    
                std::lock_guard<std::mutex> guard(drone_control_mutex_);
                get_multirotor_client()->moveByAngleRatesThrottleAsync(
                    drone->angleRateThr_cmd.rollRate,
                    drone->angleRateThr_cmd.pitchRate,
                    drone->angleRateThr_cmd.yawRate,
                    drone->angleRateThr_cmd.throttle,
                    control_cmd_duration_,
                    drone->vehicle_name);
            }
            drone->has_vel_cmd = false;
            drone->has_angleRateThr_cmd = false;
        }
...
}

重新编译并且运行，效果如下

2024-02-14 22-13-10

小结

AirSim的C++示例相较于Python而言可以说非常少了，笔者猜测可能AirSim设计的初衷是强化学习、图像等任务，因此为了便于与主流的学习框架对接，所以更推荐使用Python。好在C++接口与Python接口相差不大，通过仔细阅读代码，最终也能够实现AirSim与ROS的联合仿真，希望这篇博客对大家学习AirSim有所启发，感谢各位的阅读。