ardupilot开发 --- Jetson Orin Nano 后篇

0~19

请参考前篇。

20 visp-d455：基于IBVS的Pixhawk无人机视觉伺服

20.1 基础

• 参考：Tutorial: Image-based visual-servoing on a drone equipped with a Pixhawk

• 第三方库： MavSDK，librealsense

• 一些概念：

  • onboard computer：板载计算机、机载计算机
  • IBVS：基于图像的视觉伺服
  • Homogeneous Matrix：齐次变换矩阵
  • rotation 、translations、transformation：旋转、平移、转换
  • Image Moments：图像矩

• 软件：

  • QGC 或 Mission Planner，安装在地面站PC上，windows系统或Ubuntu系统。
  • MavProxy ，安装在机载计算机上。

关于连接、通讯、UDP forward服务：

• 板载计算机与飞控用USB连接。
• 板载计算机与地面站PC：通过局域网连接，进行SSH。例如使用思翼科技的链路产品MK32实现局域网连接，使用MobaXterm实现SSH。
• MavProxy
  运行在：板载计算机上，Ubuntu20.04，安装教程。
  与飞控的连接：USB，即--master=/dev/ttyACM0
  创建3个UDP forward服务，使得其他应用或程序能过通过UDP连接到飞控（与飞控通讯）！
  启动命令如：

  mavproxy.py --master=/dev/ttyACM0 --out=udpout:192.168.30.111:14550 --out=udpout:192.168.30.111:14551 --out=udpout:127.0.0.1:14552
  

• Mission Planner 或 QGC
  运行在：地面站PC，windows系统或Ubuntu系统都可。
  与飞控的连接：通过mavproxy创建的UDP forward服务：192.168.30.111:14550，其中192.168.30.111是地面站PC的IP地址。
• keyboard Control app：
  运行在：地面站PC，windows系统或Ubuntu系统都可。
  与飞控的连接：通过mavproxy创建的UDP forward服务：192.168.30.111:14551，其中192.168.30.111是app所在系统的IP地址。通讯使用MavSDK。
• IBVS app
  视觉伺服程序，visp程序。
  运行在：板载计算机上，Ubuntu20.04.
  与飞控的连接：通过mavproxy创建的UDP forward服务127.0.0.1:14552，其中127.0.0.1是app所在系统的IP地址。通讯使用MavSDK。

Jetson Orin Nano 与飞控的连接和测试

• 使用usb线连接飞控的Type-C口与Jetson Orin Nano的USB口，用mavproxy测试是否能正常连接到飞控：mavproxy.py --master=/dev/ttyACM0
• 错误提示：
  
• 解决方法：
  卸载 modemmanager 然后重启 Jetson Orin Nano.
  

通过MK32提供的局域网SSH到Jetson Orin Nano

• 1）将Jetson Orin Nano的网口与MK32天空端的LAN口用网线进行连接，注意Jetson Orin Nano的接口是RJ45，MK32天空端的接口是GH1.25-8P。若需要制作连接线，MK32天空端的接口定义请参考：A8mini云台相机篇，RJ45接口定义请百度查询。
  
• 2）Jetson Orin Nano设置静态IP
  IP：192.168.144.79（11,12,20,25,26已被占用）
  网关：192.168.144.1
  掩码：255.255.255.0
  MK32的局域网设置请参考：MK32使用手册
  
• 3）用网线将PC的网口连接到MK32遥控器的LAN口。
• 4）PC设置静态IP
  IP：192.168.144.80（11,12,20,25,26已被占用）
  网关：192.168.144.1
  掩码：255.255.255.0
• 5）PC端使用SSH工具进行远程登录到 Jetson Orin Nano
  SSH前先ping一下局域网内的其他IP，验证一下网络状况。
  

一些相关的、有用的例程

• testPixhawkDroneTakeoff.cpp
  简单的起飞然后降落。
• testPixhawkDronePositionAbsoluteControl.cpp
  起飞，方形轨迹飞行
• testPixhawkDronePositionRelativeControl.cpp
  起飞，以起飞点为中心进行方形轨迹飞行
• testPixhawkDroneVelocityControl.cpp
  起飞之后是一个简单的轨迹，使用速度控制测试一些不同的动作。
• autopilot_server.cpp

Linux C++程序的gdb断点调试

• 1）用VSCode打开代码
• 2）开始调试
  注意：
  【1】"program": "/home/jetson/shd/visp-ws/visp-build/modules/robot/testPixhawkDroneTakeoff"中不要使用~来表示用户目录，因为VSCode无法识别！！
  【2】编译时：cmake -DCMAKE_BUILD_TYPE=debug否则无法识别断点！

搭建仿真

• 前提：
  Ardupilot仿真运行在WSL上，visp程序和mavproxy运行在Jetson Orin Nano上！
  WSL和Jetson Orin Nano在同一个局域网！
• 1）在Jetson Orin Nano上安装mavproxy：

  sudo apt-get install python3-dev python3-pip python3-matplotlib python3-lxml python3-pygame
  pip3 install PyYAML mavproxy --user
  echo 'export PATH="$PATH:$HOME/.local/bin"' >> ~/.bashrc
  . ~/.bashrc
  

• 2）已知Jetson Orin Nano的IP地址为：192.168.100.197
• 3）在WSL上运行SITL仿真：

  sim_vehicle.py -v ArduCopter --console --out "192.168.100.197:14550" --out "127.0.0.1:14550" -w
  

  其中127.0.0.1:14550可用连接windows端的mission planner，便于查看飞行状态。
• 4）在Jetson Orin Nano上启动MavProxy

  mavproxy.py --master=udp:192.168.100.197:14550 --out=udpout:127.0.0.1:14552
  

  注意：MavProxy生成的缓存文件mav.parm， mav.tlog ，mav.tlog.raw 要定时清理，这些文件保存在运行命令时的所在目录！不然会占用机载计算机的内存空间！
• 5）testPixhawkDroneTakeoff.cpp例程调试和分析
  • /home/jetson/shd/visp-ws/visp/modules/robot/test/servo-pixhawk/testPixhawkDroneTakeoff.cpp
  • 构造一个mavsdk 对象；
  • 飞控已启动后，局域网已正常连接；
  • 建立一个TCP、UDP、串口连接，不成功会抛出异常并终止程序。
  • 发送指令之前先调用isRunning()函数，查看飞机是否在运行。
  • mavsdk的析构函数会导致飞机降落？？
  • 使用类vpRobotMavsdk控制飞机飞行的方式：
    • 1）先使用setPositioningIncertitude()设置定位和偏航的不确定性参数，如：
      drone.setPositioningIncertitude(0.10, vpMath::rad(5.));
      位置和偏航不确定范围为0.1m和5deg。
      这些参数用于你判断飞机是否到设定的期望位置或偏航角期望值，可以用
    • 2）调用takeControl()让飞机进入guided（ardupilot）或off-board（PX4）模式。
    • 3）使用 setPosition() 或 setVelocity() 去控制飞机移动 。
    • 其他
      设置起飞高度：drone.setTakeOffAlt(5.);
      是否打印详细信息：drone.setVerbose(true);

解决【testPixhawkDroneTakeoff.cpp例程能解锁但起飞命令无响应，断点模式下却有响应】问题

解决：先用地面站将飞控设置为guided，再运行testPixhawkDroneTakeoff.cpp例程。

解决【使用最新的mavsdk版本导致visp编译报错】问题

背景：visp中的视觉伺服例程使用的固件是px4，跟本人正在使用Ardupilot有较大区别，例如px4中的off-board而Ardupilot中是guided模式，等等。
通过查阅mavsdk源码可知：mavsdk中对guided模式的一些定义和使用在src\mavsdk\core\ardupilot_custom_mode.h中，并且必须是最新的mavsdk源码版本即main分支。
使用最新的mavsdk代码，编译visp时报错，分析和解决如下：

解决方式：使用合适的构造函数代替：
cpp //mavsdk::Mavsdk m_mavsdk {}; mavsdk::Mavsdk m_mavsdk{mavsdk::Mavsdk::Configuration{mavsdk::Mavsdk::ComponentType::GroundStation}};

解决方式：
cpp #if (VISP_HAVE_MAVSDK_VERSION > 0x010412) passthrough.unsubscribe_message(MAVLINK_MSG_ID_HEARTBEAT,handle); #else passthrough.subscribe_message_async(MAVLINK_MSG_ID_HEARTBEAT, nullptr); #endif
一些条件编译中的报错处理方法大同小异：
大部分都是#if (VISP_HAVE_MAVSDK_VERSION > 0x010412)下面的代码报错，因为visp作者使用的是mavsdk1.4版本，在例程中执行的是else下面的代码if下的代码则不会被执行所以不会被发现，更新mavsdk到最新版本后再编译visp就会出现编译报错！！
解决方式：

修改后的 vpRobotMavsdk.cpp .

visp中的异步编程：std::promise和std::future

std::promise和 std::future 是怎么配合工作的？以及它们在异步编程中扮演的角色是什么？参考百度。

20.2 视觉伺服

知识储备

• 坐标变换矩阵即坐标齐次变换矩阵，包括：旋转变换、平移变换。
  Rotation Matrix
  本质：坐标变换本质上是一种投影变换，是一种投影关系！从系a转换到系b即把系a中的坐标投影到系b.
  应用：2维，3维，n维
  举例：Xb = 旋转矩阵*Xb + 系a原点在系b中的坐标 = Tr * Xa + Ao_b = 齐次变换矩阵 * Xa
  旋转译自rotation ，平移译自translations，转换译自transformation
  坐标旋转的表示方式有多种：方向余弦矩阵、欧拉角、四元素等等，可参考坐标旋转篇。
  
• 单应性矩阵（单应矩阵、齐次矩阵、投影变换矩阵）
  Homogeneous Matrix
  本质：是一种坐标转换关系，是两个图像坐标系之间的转换关系矩阵。
  应用：图像投影。
  什么是图像投影？有什么作用？
  对于3D空间的一系列点(某个场景)，在不同视觉角度拍摄得到的图像分别记作P1、P2，P1P2中的同一个点p在P1P2的图像坐标系(2D的)的坐标记作【x1,y1】、【x2,y2】,那么它们之间的转换关系可以用单应矩阵表示：【x2,y2】= H * 【x1,y1】
  应用场景：交通场景中，例如需要获得俯视视角下的街道平面各智能体的坐标。而摄像机的位置可能无法做到完全俯视。此时可以标注一些地标，例如地板正方形瓷砖的角作为对应点，之后通过确定的H矩阵来将摄像机的影像转换为俯视视角的坐标。
  参考：Homography matrix（单应矩阵）简介
  计算方式：
  在visp中单应矩阵可以通过3D空间坐标旋转矩阵得到，如：

  vpRxyzVector c1_rxyz_c(vpMath::rad(-10.0), vpMath::rad(0), 0);
  vpRotationMatrix c1Rc(c1_rxyz_c); // Rotation between (c1) and (c)  //c1->c的3d空间坐标旋转矩阵
  vpHomogeneousMatrix c1Mc(vpTranslationVector(), c1Rc); // Homogeneous matrix between (c1) and (c) //c1->c的2d图像(平面)坐标旋转矩阵，即单应矩阵
  

  对于单应矩阵的估计请参考：这里或Tutorial: Homography estimation from points
  
  
  
• 图像矩（Image Moments）

  • 源自：数学学科中随机变量的矩

  • 领域：数字图像处理、计算机视觉、深度学习

  • 对象：二值图像、灰度图像

  • 概念：图像的矩可以概括为图像的某些像素点的灰度值（像素值，在灰度图中像素值即灰度值）的加权平均值，或者是图像具有类似功能或意义的属性的加权平均值。
    意义：可以通过图像的矩来获得图像的一些有用的性质，包括面积、轮廓、几何中心（重心、质心）、方向（特征向量、主轴）等信息 ，这些信息对于图像识别、形状匹配和目标跟踪等应用非常有用。

  • 分类：

    • 原点矩
      (p+q)阶原点矩：
      
    • 中心距
      (p+q)阶原点矩：
      
    • 中心归一化距
      (p+q)阶中心归一化距：
      

  • 矩的应用：

    • 目标区域的面积 = 目标区域的像素个数 = m00
      与零阶混合原点矩有关！
    • 目标区域的质心坐标 =【m01/m00, m10/00】
      与 一阶混合原点矩有关！
    • 目标区域的方向
      与二阶混合中心距有关！
      主轴斜率 = mu20/mu11
      副轴斜率 = mu11/mu20
      

  • opencv中的图像矩介绍
    空间矩：double m00,m10,m01,m20,m11,m02,m30,m21,m12,m03
    中心矩：double mu20,mu11,mu02,mu30,mu21,mu12,mu03
    中心归一化矩：double nu20,nu11,nu02,nu30,nu21,nu12,nu03

  • 参考文献
    https://zh.wikipedia.org/wiki//wiki/矩_(图像)
    https://zhuanlan.zhihu.com/p/519713049
    https://blog.csdn.net/weixin_43212588/article/details/132674015
    https://blog.csdn.net/shuiyixin/article/details/104646531/

分析与实践

• 相关概念
  IBVS、视觉伺服、图像矩视觉特征、坐标变换、mavsdk、guided模式、Visp…

• IBVS视觉伺服理论请参考：视觉伺服篇

• 坐标变换理论请参考：坐标变换篇

• 图像矩视觉特征的介绍请参考：
  【0】Image-based visual-servoing
  【1】硕士论文：基于图像的四旋翼无人机视觉伺服控制-郑栋梁
  【2】硕士论文：基于无标定视觉伺服的定位研究-王博

• 坐标系示意图：
  
  注意：文档 https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-pixhawk-vs.html 中画出的坐标系示意图(下图)是错误的！！因此原源代码中的一些注释也是错误的！！
  

• 需要根据相机的安装位置调整的坐标系变换矩阵
  终端坐标系e：视觉伺服中的重要坐标系，视觉伺服计算得到的运动控制量最终要转换到该坐标系下，在该案例中终端坐标系即FRD飞机机体坐标系。
  相机坐标系c：IBVS视觉伺服推导的重要坐标系。
  中间坐标系c1，原点与c重合，轴向与c同向但与e平行。
  例如：相机的安装位置如下图所示，并且相机的镜头微微向下倾斜10°，则e到c的坐标齐次变换矩阵cMe计算方式如下：
  

  //! [compute cMe]
  vpRxyzVector c1TOc_xyz(vpMath::rad(-10.0), vpMath::rad(0), vpMath::rad(0)); // c1 ~> c 的xyz轴旋转量！
  vpRotationMatrix c1Rc(c1TOc_xyz); // 旋转矩阵：c -> c1 
  vpRotationMatrix cRc1 = c1Rc.inverse(); // 旋转矩阵：c1 -> c, 旋转矩阵是正交的，因此 c1Rc^-1==c1Rc^T
  vpHomogeneousMatrix cMc1(vpTranslationVector(), cRc1); // 齐次变换矩阵：c1 -> c
  vpRotationMatrix c1Re {0, 1, 0, -1, 0, 0, 0, 0, 1}; // 旋转矩阵：e -> c1 
  vpTranslationVector e0_c1(0, 0, -0.1); // 平移关系：e系原点在c1系中的坐标
  vpHomogeneousMatrix c1Me(e0_c1, c1Re); // 齐次变换矩阵：e -> c1
  vpHomogeneousMatrix cMe = cMc1 * c1Me; // 齐次变换矩阵：e -> c
  vpVelocityTwistMatrix cVe(cMe);  // 伺服系统控制量Vc=[vx,vy,vz,wx,wy,wz]的坐标变换矩阵：e -> c，从飞机机体系FRD到相机系RDF
  //! [compute cMe]
  task.set_cVe(cVe);
  //eJe推导：全自由度控制量Vc=[vx,vy,vz,wx,wy,wz]^T，只允许vx,vy,vz,wz可控，令[vx,vy,vz,wx,wy,wz]=[vx,vy,vz,0,0,wz]=eJe*[vx,vy,vz,wz]
  //则可知eJe=[1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 0; 0 0 0 0; 0 0 0 1]
  //那么IBVS伺服系统的状态空间变为：dot_S = L * cVe * eJe * Ve = L * cVe * eJe * [vx,vy,vz,wz]
  //其中：cVe是6x6矩阵，eJe是6x4矩阵，Ve是终端系的4x1速度矢量
  vpMatrix eJe(6, 4, 0);//哪些速度自由度是可控的！！！！！！！！！！！！
  eJe[0][0] = 1;//vx可控
  eJe[1][1] = 1;//vy可控
  eJe[2][2] = 1;//vz可控
  eJe[5][3] = 1;//wz可控
  task.set_eJe(eJe);
  

  visp对于${{}^{c}M}_e,{{}^{c}V}_e,{{}^{e}J}_e$的解释请参考：Image-based visual-servoing.

• 定义视觉特征期望值
  在相机系下定义4个期望特征点，然后将期望值转换到图像系下：

  // Desired distance to the target
  double Z_d = (opt_has_distance_to_tag ? opt_distance_to_tag : 1.); //相机到二维码的 期望控制距离！
  // Define the desired polygon corresponding the the AprilTag CLOCKWISE  //视觉特征期望值！
  double X[4] = { tagSize / 2., tagSize / 2., -tagSize / 2., -tagSize / 2. }; //定义4个【相机系下的特征点期望值】！
  double Y[4] = { tagSize / 2., -tagSize / 2., -tagSize / 2., tagSize / 2. };
  std::vector<vpPoint> vec_P, vec_P_d;
  vpHomogeneousMatrix cdMo(0, 0, Z_d, 0, 0, 0);//AprilTag机体系到相机系的【期望齐次变换矩阵】！
  for (int i = 0; i < 4; i++) {
    vpPoint P_d(X[i], Y[i], 0);//相机系下的视觉特征期望值！
    P_d.track(cdMo); //计算【图像系下的视觉特征期望值】，结果保存在 P_d 的祖父类 pTracker 的 cP 中！
    vec_P_d.push_back(P_d);//视觉特征期望值向量！
  }
  

• 起飞

  #ifdef CONTROL_UAV//这个宏定义是本例程中所有控制飞机指令的 使能
    //drone.doFlatTrim();//自动校正加速度计和陀螺仪
    drone.takeOff(false,15,true);//起飞，takeOff包含了：guided -> 解锁 -> 起飞 -> 位置保持
  #endif
  

• 获取当前帧

  rs.acquire(I);//获取当前图像
  

• 二维码识别

  //使用当前帧检测AprilTags的位姿，结果保存在cMo_vec
  std::vector<vpHomogeneousMatrix> cMo_vec;
  detector.detect(I, tagSize, cam, cMo_vec); 
  

• 在计算IBVS控制量前更新当前帧的图像矩视觉特征
  略…

• 计算飞机机体系下的IBVS控制量（速度矢量）

  if (detector.getNbObjects() != 0) 
  {
    vpColVector ve = task.computeControlLaw();//根据IBVS理论计算得到飞机的6D速度控制量[u,v,w,p,q,r]
  }
  

• 飞机运动控制
  发送速度指令：

  #ifdef CONTROL_UAV
    drone.setVelocity(ve);//发送控制飞机速度的指令，ve是飞机FRD body坐标系下的速度矢量。4dim，vx,vy,vz,wz.
  #endif
  

  发送停止运动指令：

  #ifdef CONTROL_UAV
    drone.stopMoving(); // In this case, we stop the drone  //没有检测到AprilTags则停止飞行防止失控！
  #endif
  

• RTSP推流
  使用ffmpeg推流，主要逻辑：

  std::string rtsp_server_url = "rtsp://127.0.0.1:554/live/0";
  std::stringstream command;
  command << "ffmpeg -loglevel error ";//log打印等级设置为：有报错时才打印到窗口
  // inputfile options
  command << "-y "  // overwrite output files
  << "-an " // disable audio
  << "-f rawvideo " // force format to rawvideo
  << "-vcodec rawvideo "  // force video rawvideo ('copy' to copy stream)
  << "-pix_fmt bgr24 "  // set pixel format to bgr24 or gray //灰度流和彩色流这个参数要变一变不然会显示异常！！
  << "-s 640x480 "  // set frame size (WxH or abbreviation)  640x480
  << "-r 10 "; // set frame rate (Hz value, fraction or abbreviation)
  command << "-i - ";
  // outputfile options
  command 
  << "-c:v libx264 "  // Hyper fast Audio and Video encoder
  << "-pix_fmt yuv420p "  // set pixel format to yuv420p
  << "-tune:v zerolatency "
  << "-preset ultrafast " // set the libx264 encoding preset to ultrafast
  << "-f rtsp "; // force format to flv for rtmp, rtsp for rtsp
  command << rtsp_server_url;
  FILE *fp = nullptr;
  // 在子进程中调用 ffmpeg 进行推流
  fp = popen(command.str().c_str(), "w");
  if (fp != nullptr) 
    std::cout << "fp open success!" << std::endl;
  else 
  {
    std::cout << "fp open fail!" << std::endl;
    pclose(fp);
  }
  while(1) {
  //识别结果
  vpImage<vpRGBa> Ioverlay;
  vpDisplay::getImage(I, Ioverlay);//获取识别结果图像
  cv::Mat frame;
  vpImageConvert::convert(Ioverlay, frame);
  if(!frame.empty()) 
    fwrite(frame.data, sizeof(char), frame.total() * frame.elemSize(), fp);//推流
  }
  

• 图像叠加显示

  //识别结果 I
  vpImage<vpRGBa> Ioverlay;
  vpDisplay::getImage(I, Ioverlay);//获取识别结果图像
  cv::Mat frame;
  vpImageConvert::convert(Ioverlay, frame);
  //IBVS误差曲线
  vpImage<vpRGBa> I_plot;
  plotter.getImage(I_plot);//获取当前误差曲线图像
  //将误差曲线和识别结果进行图像叠加显示
  cv::Mat frame_plot;
  cv::Mat frame_overlay;
  vpImageConvert::convert(I_plot, frame_plot);
  cv::addWeighted(frame, 0.6, frame_plot, 0.4, 0, frame_overlay);//opencv中的图像叠加函数
  vpImageConvert::convert(frame_overlay, overlayImg);
  display2.display(overlayImg);
  display2.flush(overlayImg);
  

• 编译和运行
  使用帮助请查阅README.md。

• 结果分析
  
  该例程中使用的图像矩视觉特征解释请参考：Image-based visual-servoing
  Xn：重心矩视觉特征的x分量误差！
  Yn：重心矩视觉特征的y分量误差！
  an：目标区域面积视觉特征误差！控制相机到AprilTag中心的距离！
  atan(1/rho)")：灭点视觉特征误差！两条灭点直线相互平行时(不要求水平只要求相互平行)该误差为0！
  

• 其他
  在代码中引用外部库xxxx.h xxxx.so
  CMakeLists.txt中：
  
  pkg-config --cflags --libs gstreamer-1.0

• 一些疑惑

  • 二维码颠倒之后伺服控制量是否正常？？
    与摆放位置无关！伺服控制量都是正常的！以第一次识别到二维码时的二维码姿态来计算初始灭点水平线！！！
  • 在SITL中由guided模式切换到手动模式如 loiter 或 atlHold 为什么会自动降落？
    因为在SITL中，虚拟遥控器的油门遥感是默认保持最低位的，因此在由guided模式切换到手动模式如loiter或atlHold时会表现为自动下高度。
  • 如何将鼠标点击事件转换成命令行输入的形式？左击开始，再左击停止，右击停止并降落！！
    待解决…
  • Ardupilot的位置估计和姿态估计是基于哪个坐标系下的？
    地固坐标系：NED local frame（或称 NED Ardupilot reference frame）
    飞机机体坐标系1：FRD body frame
    飞机机体坐标系2：FLU body frame
    位置估计：FRD body frame 原点在 NED local frame 中的坐标【posX，posY，posZ】
    姿态估计：FRD body frame 与 NED local frame 间的旋转关系，一般用欧拉角表示：【俯仰角，横滚角，偏航角】，航天航空领域一般使用东北天的旋转顺序来计算欧拉角。
    运动控制量是基于哪个坐标系下计算得到的？
    最终的控制量是基于按个坐标系下的量 ？
    姿态角、姿态角速度控制：body frame FRD
    速度控制：body frame FRD or Local frame NED ？
    位置控制：body frame FRD or Local frame NED ？
    位置、姿态测量传感器是基于哪个坐标系下的量？
    GPS：WGS84
    视觉定位Vicon：Vicon ENU reference frame
    动作捕捉器MoCAP：MoCAP ENU reference frame
  • 如何解决一开始就检测不到Apriltag报的段错误？
    
    解决：添加判断检测到Apriltag再绘制误差曲线if (condition && (detector.getNbObjects() != 0))

• 项目源码：https://github.com/huangyangl/visp

• 代码：servoPixhawkDroneIBVS-test.cpp
  有多个感叹号“！！！！！”的注释是需要留意的地方！

• 代码 vpRobotMavsdk.cpp

• 类vpPlot添加一个函数

  //debug 20240628
  void vpPlot::getImage(vpImage<vpRGBa> &Idest)
  {
    display->flush(I);
    display->getImage(I, Idest);
  }
  

• README.md
  README.md

  # cmake 
  cd ~/shd/visp-ws/visp-build
  cmake ../visp -DCMAKE_BUILD_TYPE=debug -DUSE_BLAS/LAPACK=GSL
  cmake ../visp -DCMAKE_BUILD_TYPE=release -DUSE_BLAS/LAPACK=GSL
  # make 
  cd ~/shd/visp-ws/visp-build
  make -j6 servoPixhawkDroneIBVS-test
  # run servo-pixhawk in jetson
  cd ~/shd/visp-ws/visp-build/example/servo-pixhawk
  ./servoPixhawkDroneIBVS-test --help
  ./servoPixhawkDroneIBVS-test --tag-size 0.12 --co udp://:14550 --distance-to-tag 2 --verbose --rtsp --cambottom
  # run mavproxy in jetson
  mavproxy.py --master=/dev/ttyACM0 --out=udpout:127.0.0.1:14550
  # wsl sitl
  wsl
  cd ~/code/hyl-git-repo-ws/sitl-master
  sim_vehicle.py -v ArduCopter --console --out "192.168.100.197:14550" --out "127.0.0.1:14550" -w
  # RTSP server
  cd ZLMediaKit/release/linux/Debug
  ./MediaServer -h
  sudo ./MediaServer -d -l 0 &
  sudo killall -2 MediaServer
  # RTSP pull
  ffplay -rtsp_transport tcp -i rtsp://127.0.0.1:554/live/0
  # user guid
  1. 叠加图像显示的开启和关闭
  由宏定义 #define DISP_OverLayImg 决定。
  2. 叠加图像的RTSP推流
  开启：--rtsp
  关闭：缺省--rtsp，默认关闭
  如果开启RTSP，必须先运行MediaServer以开启RTSP服务器再运行程序servoPixhawkDroneIBVS-test，否则servoPixhawkDroneIBVS-test程序会运行报错。
  默认推流地址(目前只支持在代码中修改)：rtsp://127.0.0.1:554/live/0
  3. 二维码的尺寸
  --tag-size 0.12
  4. 飞机到二维码的期望距离
  默认为1
  --distance-to-tag 2
  5. 与飞控的连接方式
  --co udp://:14550
  6. 运行程序过程中显示冗余打印信息
  --verbose
  7. 默认图像处理频率为30fps
  目前只支持在代码中改动后重新编译。
  8. 相机的安装位置
  安转在飞机正下方，相机x轴与飞机机体y轴同向： --cambottom
  安转在飞机正左方，相机x轴与飞机机体x轴同向： 缺省--cambottom，默认关闭
  9. 开启、关闭视觉伺服、降落
  开启、关闭视觉伺服：左击图像，或者在命令行输入 0关闭，1开启
  关闭视觉伺服并降落：右击图像，或命令行输入7
  10. 运行顺序
  （1）在机载计算机上运行mavproxy：mavproxy.py --master=/dev/ttyACM0 --out=udpout:127.0.0.1:14550
  （2）在机载计算机上运行 RTSP server：cd ZLMediaKit/release/linux/Debug & sudo ./MediaServer -d -l 0 &
  （3）在机载计算机上运行servo-pixhawk app：./servoPixhawkDroneIBVS-test --tag-size 0.12 --co udp://:14550 --distance-to-tag 2 --verbose --rtsp --cambottom
  

• 测试
  
  
  
  

• 结论
  硬件方案：英伟达 jetson orin nano 机载计算机 +因特尔D455深度相机。
  软件方案：ViSP。
  通讯工具：orin nano 通过网线接入MK32天空端的局域网，PC通过网线或热点接入MK32地面端安卓遥控器的局域网，PC就可以通过SSH工具（如MobaXterm）连接到 orin nano 以控制IBVS程序的启停。
  测试目标：实现无人机对二维码的实时跟踪。
  测试结果：视觉伺服控制结果基本符合预期，基本能实现二维码的识别、定位和跟踪，测试过程中发现经常出现二维码识别失败的问题，初步推断可能是由于太阳光照太强、相机抖动频繁、二维码尺寸太小、识别算法参数不适当导致，这需要后续继续优化代码和实施更多的重复测试来解决这些问题。

21 visp_ros 软件包

代码：https://github.com/lagadic/visp_ros

21.1 一些概念

• why？
  为什么要使用ros？直接使用传统的Visp程序实现IBVS视觉伺服不好吗？
  传统的Visp程序是一个以main函数为主(main中包含一个while死循环)的C++程序，可运行在Linux上。所有功能都要写在main函数中，包括图像帧获取、RTSP推流、IBVS控制量计算、发送pixhawk平台控制指令等，这样杂揉在一起，但凡某个环节出现异常都会导致整个进程的崩溃，不合理。
  传统的ViSP程序控制流如下图：
  
  ros 是一个分布式的平台，ros可以将传统的ViSP程序进行分布式化，将各个子功能模块编译成一个个的ros节点，节点间可以很容易地通过主题的发布和订阅进行信息交换！某个节点的崩溃不会影响其他节点的功能，分布式的好处就是强大的信息共享机制和模块化的运行机制。例程参考这里：Using visp_ros to produce a ROS node.
  完全ros化的visp程序的控制流如下图：
  
  当然，还有一种半ros化的视觉伺服实现方式，就是利用visp_ros 软件包中提供的visp_ros/vpROSGrabber.h可以让传统的ViSP程序与 ros 节点之间进行信息交换，注意此处的ViSP程序并不是一个ros节点！例程参考这里：How to grab images from ROS.
  该法的控制流如下图：
  

• 使用ROS的意义
  便于分布式部署由不同语言编写的、实现不同功能的节点程序。例如以python编写的视线GPIO控制的节点、以C++编写的IBVS节点、以C++编写的读取D455相机的节点、RTSP推流节点等等。

• 包含的Visp类
  使用方式与ViSP classes类似。
  vpROSGrabber 用于获取图像帧
  vpROSRobot 用于控制机器人平台
  vpROSRobotPioneer
  vpROSRobotFrankaCoppeliasim

• 包含的节点
  visp_ros 软件包包含了一些允许控制特定机器人平台的ros节点，平台如Pioneer mobile robot、Parrot bebop2 drone、pixhawk等…

• 包含一些有用的demo

21.2 ViSP ROS package

ROS package的用法和解释请参看：ViSP ROS package

• ViSP C++库的ROS package
  安装后就可以在ROS调用，根据版本来安装对应的包ros--visp，如：ros-<noetic>-visp
• 其他ViSP相关的ROS package
  • vision_visp，如用于ViSP与ROS的数据转换等功能。注意vision_visp是一个元包，包含多个包。
  • visp_ros，如用于ViSP中的视觉伺服计算等功能。
  • demo_pioneer，Pioneer robot相关的功能等。

vision_visp 的编译和用法；
visp_ros 的编译和用法；
demo_pioneer 的编译和用法。

21.2 视觉伺服demo

• 安装 ROS

  • 版本选择：安装 ROS Noetic
  • 查看兼容要求：REP 3: Target Platforms.
  • 安装步骤：Ubuntu install of ROS Noetic
  • 配置环境变量

    echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
    source ~/.bashrc
    

• 创建并初始化 catkin 工作空间

  cd ~/shd
  mkdir -p catkin-ws/src
  cd catkin-ws
  catkin_make
  echo "source ~/shd/catkin-ws/devel/setup.bash" >> ~/.bashrc
  source ~/.bashrc
  printenv | grep ROS
  

• 编译 ViSP（安装ViSP库）
  从package.xml可知ViSP库是软件包vision_visp和visp_ros的依赖库，所以需要将ViSP源码编译成为库文件(xxx.so, xxx.h)，并且需要将ViSP库的存放路径在环境变量VISP_DIR中指定。
  如果你在ViSP源码中做了某些代码改动，你就必须以编译源码的方式安装ViSP库才能应用这些改动。否则你可以使用apt的方式来安装ViSP的预编译库：apt-get install ros-noetic-visp，若如此，在后续的步骤中你就可以去掉这些命令参数：-DVISP_DIR=${VISP_DIR} 、--skip-keys=visp。
  编译ViSP：

  cd ~/shd/visp-ws/visp-build
  cmake ../visp -DUSE_BLAS/LAPACK=GSL -DCMAKE_BUILD_TYPE=release -DBUILD_SHARED_LIBS=ON
  make -j4
  echo "export VISP_DIR=~/shd/visp-ws/visp-build" >> ~/.bashrc # 如果已存在则略过这步，用printenv | grep visp
  source ~/.bashrc
  

  以上编译命令执行完毕后ViSP库文件会被安装到~/shd/visp-ws/visp-build目录中！
  如果你想将ViSP库文件安装到ros包默认安装目录/opt/ros/noetic，请这样编译ViSP：

  cd ~/shd/visp-ws/visp-build
  cmake ../visp -DUSE_BLAS/LAPACK=GSL -DCMAKE_BUILD_TYPE=release -DCMAKE_INSTALL_PREFIX=/opt/ros/noetic #指定安装目录
  make -j4
  sudo make install
  

• 编译 vision_visp
  从package.xml可知vision_visp是visp_ros的依赖，因此在编译visp_ros之前要先编译vision_visp。
  （1）下载vision_visp源码

  cd ~/shd/catkin-ws/src
  git clone https://github.com/lagadic/vision_visp.git -b $ROS_DISTRO
  

  （2）安装vision_visp的依赖
  vision_visp是一个元包其中包含的多个相关的软件包。catkin元包的编译和依赖安装和catkin普通软件包的方法类似。

  cd ~/shd/catkin-ws
  sudo rosdep init # 若之前执行过则跳过这步！
  rosdep update # 这个命令不要使用sudo
  rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO --skip-keys visp
  

  各个包的依赖在各自的 package.xml中 查看。
  其中--skip-keys=visp表示跳过安装官方预编译的ViSP库（官方预编译的ViSP库的包名为ros-noetic-visp），因为我们已经在前面步骤中通过编译自己的ViSP源码来生成ViSP库，只需要在后续的编译中是指明ViSP库的存放路径即可。--from-paths src表示先在指定的目录中查找依赖包，若没找到再从ros官网安装这些依赖包。
  （3）编译 vision_visp
  编译vision_visp元包中的所有软件包：

  cd ~/shd/catkin-ws
  catkin_make -DVISP_DIR=${VISP_DIR} 
  

  编译vision_visp元包中的某些软件包：

  cd ~/shd/catkin-ws
  catkin_make -DVISP_DIR=${VISP_DIR} --pkg visp_bridge visp_camera_calibration 
  

  注意1：因为ViSP库并没有安装到ros默认目录/opt/ros/noetic所以编译vision_visp时需要指定ViSP库的安装路径，如-DVISP_DIR=${VISP_DIR}。
  注意2：由于版本问题，最新的ViSP源码可能与当前vision_visp元包中的某些包不兼容，因此编译vision_visp元包时建议只编译visp_bridge、visp_camera_calibration 这两个包，因为visp_ros的依赖也只是visp_bridge而已！！

• 编译 visp_ros
  （1）下载visp_ros软件包源码
  下载到与vision_visp同一个catkin空间中。

  cd ~/shd/catkin-ws/src
  git clone https://github.com/lagadic/visp_ros.git -b $ROS_DISTRO
  

  （2）安装visp_ros的依赖
  其实可以跳过这一步，因为在用rosdep安装vision_visp的依赖后已经满足visp_ros的依赖要求。

  cd ~/shd/catkin-ws/
  rosdep update
  rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO  --skip-keys visp
  

  其中--skip-keys=visp表示跳过安装官方预编译的ViSP库，因为我们已经在前面步骤中通过编译自己的ViSP源码来生成ViSP库并安装在${VISP_DIR}目录中。--from-paths src表示先在指定的目录中查找依赖包，若没找到再从ros官网安装这些依赖包。
  （3）编译visp_ros

  cd ~/shd/catkin-ws
  catkin_make -DVISP_DIR=${VISP_DIR} --pkg visp_ros
  

  编译visp_ros时报错提示找不到nav_msgs/Odometry.h
  
  原因：没有安装nav_msgs。nav_msgs是bebop_autonomy包的依赖，而本人没有安装bebop_autonomy（用不到）。
  解决：在visp_ros包的package.xml中添加：<build_depend>nav_msgs</build_depend> ，重新运行安装依赖的指令然后重新编译visp_ros完美解决该问题。

• 其他

  • 查看已安装的ViSP包：

    cd /opt/ros/noetic
    find . -name *visp*
    dpkg -l "*visp*"
    

  • 卸载通过rosdep install安装的官方预编译ViSP库：

    sudo apt-get --purge remove ros-noetic-visp
    

    visp的其他ros版本的包名格式为：ros-rosdistro-visp。

• 运行visp_ros中的节点

  source ~/shd/catkin-ws/devel/setup.bash
  roslaunch visp_ros bebop_servo_node.launch
  # 或：
  roslaunch visp_ros bebop_servo_nodelet.launch
  

• 代码分析
  主要实现代码是：
  catkin-ws/src/visp_ros/tutorial/bebop2/visual_servo_node.cpp
  catkin-ws/src/visp_ros/tutorial/bebop2/visual_servo_nodelet.cpp
  其中visual_servo_nodelet.cpp以nodelet节点的形式启动！
  关于nodelet节点的用法请参考：
  https://index.ros.org/p/nodelet_tutorial_math/github-ros-common_tutorials/#noetic
  https://blog.csdn.net/qq_34493401/article/details/128890508

• 参考文献
  https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-bridge-ros.html
  https://wiki.ros.org/vision_visp
  https://wiki.ros.org/visp_ros#Controlling_a_Parrot_Bebop2_drone_by_Visual-Servoing
  https://wiki.ros.org/visp_ros/Tutorials/Howto_install_visp_ros
  How to do visual servoing with Parrot Bebop 2 drone using visp_ros
  https://github.com/lagadic/visp_ros/blob/rolling/README.md
  https://github.com/lagadic/vision_visp/blob/rolling/visp_bridge/README.md

bebop_autonomy 包分析：

package.xml注释：
<!-- build_depend version_gte="3.12.6">parrot_arsdk</build_depend -->
安装依赖：
rosdep update
rosdep install --from-paths src -i  --rosdistro $ROS_DISTRO  --skip-keys visp
编译：
visp_ros只用到了bebop_driver包，因此只需要编译bebop_driver包
catkin_make -DVISP_DIR=${VISP_DIR} --pkg bebop_driver
需要运行bebop_driver包的哪些节点？
src/bebop_autonomy/bebop_driver/launch/bebop_nodelet.launch
节点加载过程：
（1）继承Nodelet的类会被编译成为一个Nodelet插件（实际上是一个动态库）。
（2）class BebopDriverNodelet : public nodelet::Nodelet  
可知 BebopDriverNodelet类会被编译成一个Nodelet插件（动态库）。
（3）在BebopDriverNodelet::onInit()中写你的项目逻辑。
（4）先运行一个nodelet管理器。
<node pkg="nodelet" type="nodelet" name="bebop_nodelet_manager" args="manager" output="screen"/> 
（5）通过nodelet管理器启动一个nodelet节点，并让该节点加载一个名为BebopDriverNodelet的nodelet插件。
    <node pkg="nodelet" type="nodelet" name="bebop_nodelet"
      args="load bebop_driver/BebopDriverNodelet bebop_nodelet_manager"> 
    </node>

查看geometry_msgs/Twist消息：rosmsg show geometry_msgs/Twist
cd ~/shd/catkin-ws/src
catkin_create_pkg ardupilot_driver std_msgs rospy roscpp
cd ~/shd/catkin-ws/
catkin_make -DVISP_DIR=${VISP_DIR} --pkg ardupilot_driver
创建一个读取D455的节点：
待续…

点赞、收藏、关注哟！