PX4 avoidance学习

jike28

已于 2024-04-22 16:09:04 修改

阅读量910

点赞数 24

文章标签：学习

于 2024-04-15 10:16:56 首次发布

本文链接：https://blog.csdn.net/jike28/article/details/136721625

版权

1. 开源库

1.5. tf

tf：transform，ROS中管理3D坐标系变换的工具。

tf之static_transform_publisher-CSDN博客

2. 名称解释

1.2.1. FOV：Field of View of the sensor

光学镜头基本的参数和术语解释-电子发烧友网

是指镜头能拍摄到的最大视场范围。

视场角可分为对角线视场角（FOV-D)、水平视场角（FOV-H)、以及垂直视场角（FOV-V）。

对角线视场角最大，水平视场角次之，垂直视场角最小。通常我们所讲的视场角一般是指数码摄像模组的对角线视场角。

1.2.2. FCU

飞行控制组件

1.2.3. 三维坐标系

原文地址：【鸢尾花书系列】数学要素-Chapter6三维坐标系 - 知乎

三维空间中也可以构造类似平面极坐标的坐标系统。书中给我们讲解了两种坐标：

球坐标系 (spherical coordinate system) 和圆柱坐标系 (cylindrical coordinate system)

球坐标系

我们如果要在空间中定位点除了基本的xyz还有怎样的方法能定位？
大学物理中我们学过自由度是指描述一个系统状态所需独立参数的个数。
在物理学中，自由度可以用来描述一个系统的最小坐标数

其中，r 是 P 与原点 O 之间距离，也叫径向距离 (radial distance)；
θ 是 OP 连线和 z 轴正方向夹角，叫做极角 (polar angle)；
OP 连线在 xy 平面投影线为 OH，φ 是 OH 和 x 轴正方向夹角，叫做方位角 (azimuth angle)。

1.2.4. SLAM

Simultaneous Localization and Mapping：同步定位和地图绘制

3. 代码阅读

local_scanner.h

4. PX4 firmware与PX4 avoidance的通信

参考文档：

Path Planning Interface | PX4 User Guide (main)

5. PX4-avoidance编译

假设px4-avoidance的代码放在目录：~/catkin_ws/src

编译完成后，相关的可执行程序和动态库会生成在：/root/catkin_ws/devel/

root@ubuntu:~/catkin_ws/devel# ls -lh
total 44K
lrwxrwxrwx  1 root root   63 Mar 14 05:33 cmake.lock -> /root/catkin_ws/devel/.private/catkin_tools_prebuild/cmake.lock
lrwxrwxrwx  1 root root   59 Mar 14 05:33 env.sh -> /root/catkin_ws/devel/.private/catkin_tools_prebuild/env.sh
drwxr-xr-x  5 root root 4.0K Mar 14 05:34 include
drwxr-xr-x  8 root root 4.0K Mar 14 05:34 lib
lrwxrwxrwx  1 root root   69 Mar 14 05:33 local_setup.bash -> /root/catkin_ws/devel/.private/catkin_tools_prebuild/local_setup.bash
lrwxrwxrwx  1 root root   67 Mar 14 05:33 local_setup.sh -> /root/catkin_ws/devel/.private/catkin_tools_prebuild/local_setup.sh
lrwxrwxrwx  1 root root   68 Mar 14 05:33 local_setup.zsh -> /root/catkin_ws/devel/.private/catkin_tools_prebuild/local_setup.zsh
lrwxrwxrwx  1 root root   63 Mar 14 05:33 setup.bash -> /root/catkin_ws/devel/.private/catkin_tools_prebuild/setup.bash
lrwxrwxrwx  1 root root   61 Mar 14 05:33 setup.sh -> /root/catkin_ws/devel/.private/catkin_tools_prebuild/setup.sh
lrwxrwxrwx  1 root root   67 Mar 14 05:33 _setup_util.py -> /root/catkin_ws/devel/.private/catkin_tools_prebuild/_setup_util.py
lrwxrwxrwx  1 root root   62 Mar 14 05:33 setup.zsh -> /root/catkin_ws/devel/.private/catkin_tools_prebuild/setup.zsh
drwxr-xr-x 10 root root 4.0K Mar 14 05:34 share

root@ubuntu:~/catkin_ws/devel/lib# ls -lh
total 44K
drwxr-xr-x 2 root root 4.0K Mar 14 05:33 avoidance
drwxr-xr-x 2 root root 4.0K Mar 14 05:34 global_planner
lrwxrwxrwx 1 root root   60 Mar 14 05:33 libavoidance.so -> /root/catkin_ws/devel/.private/avoidance/lib/libavoidance.so
lrwxrwxrwx 1 root root   70 Mar 14 05:34 libglobal_planner.so -> /root/catkin_ws/devel/.private/global_planner/lib/libglobal_planner.so
lrwxrwxrwx 1 root root   76 Mar 14 05:34 liblocal_planner_nodelet.so -> /root/catkin_ws/devel/.private/local_planner/lib/liblocal_planner_nodelet.so
lrwxrwxrwx 1 root root   68 Mar 14 05:34 liblocal_planner.so -> /root/catkin_ws/devel/.private/local_planner/lib/liblocal_planner.so
lrwxrwxrwx 1 root root   82 Mar 14 05:34 libsafe_landing_planner.so -> /root/catkin_ws/devel/.private/safe_landing_planner/lib/libsafe_landing_planner.so
drwxr-xr-x 2 root root 4.0K Mar 14 05:34 local_planner
drwxr-xr-x 2 root root 4.0K Mar 14 05:34 pkgconfig
drwxr-xr-x 3 root root 4.0K Mar 14 05:34 python3
drwxr-xr-x 2 root root 4.0K Mar 14 05:34 safe_landing_planner

root@ubuntu:~/catkin_ws/devel/lib/local_planner# ls -lh
total 4.0K
lrwxrwxrwx 1 root root 81 Mar 14 05:34 local_planner_node -> /root/catkin_ws/devel/.private/local_planner/lib/local_planner/local_planner_node

6. 仿真运行中的进程

root@ubuntu:~/catkin_ws/devel/lib/local_planner# ps -ef|grep local_planner
root      128587   50740  2 22:25 pts/1    00:00:00 /usr/bin/python3 /opt/ros/noetic/bin/roslaunch local_planner local_planner_stereo.launch
root      128649  128587 14 22:25 ?        00:00:03 /root/catkin_ws/devel/lib/local_planner/local_planner_node __name:=local_planner_node __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/local_planner_node-9.log
root      128656  128587 21 22:25 ?        00:00:05 /opt/ros/noetic/lib/rviz/rviz -d /root/catkin_ws/src/avoidance/local_planner/resource/local_planner.rviz __name:=rviz __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/rviz-10.log
root      129066    1943  0 22:25 pts/0    00:00:00 grep --color=auto local_planner

root@ubuntu:~/catkin_ws/devel/lib/local_planner# ps -ef|grep avoid
root      128631  128587  0 22:25 ?        00:00:00 /bin/sh /opt/ros/noetic/lib/gazebo_ros/gzserver -e ode /root/catkin_ws/src/avoidance/avoidance/sim/worlds/simple_obstacle.world __name:=gazebo __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/gazebo-6.log
root      128656  128587 35 22:25 ?        00:00:34 /opt/ros/noetic/lib/rviz/rviz -d /root/catkin_ws/src/avoidance/local_planner/resource/local_planner.rviz __name:=rviz __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/rviz-10.log
root      128839  128631 29 22:25 ?        00:00:28 gzserver -e ode /root/catkin_ws/src/avoidance/avoidance/sim/worlds/simple_obstacle.world -s /opt/ros/noetic/lib/libgazebo_ros_paths_plugin.so -s /opt/ros/noetic/lib/libgazebo_ros_api_plugin.so __name:=gazebo __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/gazebo-6.log
root      129071    1943  0 22:27 pts/0    00:00:00 grep --color=auto avoid

root@ubuntu:~/catkin_ws/devel/lib/local_planner# ps -ef|grep px4
root      128620  128587 18 22:25 ?        00:00:24 /root/Firmware/Firmware/build/px4_sitl_default/bin/px4 /root/Firmware/Firmware/build/px4_sitl_default/etc -s etc/init.d-posix/rcS -i 0 __name:=sitl_0 __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/sitl_0-4.log

root@ubuntu:~/Firmware/Firmware/build/px4_sitl_default/bin# ps -ef|grep ros
root      128587   50740  0 22:25 pts/1    00:00:01 /usr/bin/python3 /opt/ros/noetic/bin/roslaunch local_planner local_planner_stereo.launch
root      128595  128587  0 22:25 ?        00:00:01 /usr/bin/python3 /opt/ros/noetic/bin/rosmaster --core -p 11311 -w 3 __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/master.log
root      128605  128587  1 22:25 ?        00:00:09 /opt/ros/noetic/lib/rosout/rosout __name:=rosout __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/rosout-1.log
root      128613  128587  2 22:25 ?        00:00:19 /opt/ros/noetic/lib/tf/static_transform_publisher 0 0 0 -1.57 0 -1.57 fcu camera_link 10 __name:=tf_depth_camera __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/tf_depth_camera-2.log
root      128614  128587  3 22:25 ?        00:00:25 python3 /opt/ros/noetic/lib/rqt_reconfigure/rqt_reconfigure __name:=rqt_reconfigure __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/rqt_reconfigure-3.log
root      128620  128587 18 22:25 ?        00:02:13 /root/Firmware/Firmware/build/px4_sitl_default/bin/px4 /root/Firmware/Firmware/build/px4_sitl_default/etc -s etc/init.d-posix/rcS -i 0 __name:=sitl_0 __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/sitl_0-4.log
root      128626  128587  5 22:25 ?        00:00:40 /opt/ros/noetic/lib/mavros/mavros_node __name:=mavros __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/mavros-5.log
root      128631  128587  0 22:25 ?        00:00:00 /bin/sh /opt/ros/noetic/lib/gazebo_ros/gzserver -e ode /root/catkin_ws/src/avoidance/avoidance/sim/worlds/simple_obstacle.world __name:=gazebo __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/gazebo-6.log
root      128641  128587 18 22:25 ?        00:02:12 /opt/ros/noetic/lib/stereo_image_proc/stereo_image_proc __name:=stereo_image_proc __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/stereo-stereo_image_proc-8.log
root      128649  128587 15 22:25 ?        00:01:51 /root/catkin_ws/devel/lib/local_planner/local_planner_node __name:=local_planner_node __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/local_planner_node-9.log
root      128656  128587 86 22:25 ?        00:10:15 /opt/ros/noetic/lib/rviz/rviz -d /root/catkin_ws/src/avoidance/local_planner/resource/local_planner.rviz __name:=rviz __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/rviz-10.log
root      128839  128631 27 22:25 ?        00:03:16 gzserver -e ode /root/catkin_ws/src/avoidance/avoidance/sim/worlds/simple_obstacle.world -s /opt/ros/noetic/lib/libgazebo_ros_paths_plugin.so -s /opt/ros/noetic/lib/libgazebo_ros_api_plugin.so __name:=gazebo __log:=/root/.ros/log/c3e8d96e-ed8c-11ee-8ddd-b3c7839cdd97/gazebo-6.log
root      129340    1943  0 22:37 pts/0    00:00:00 grep --color=auto ros

7. 安装librealsense

更新kernel：sudo apt-get update && sudo apt-get upgrade && sudo apt-get dist-upgrade

创建目录：/root/realsense

拉取代码：

cd /root/realsense

git clone https://github.com/IntelRealSense/librealsense.git

安装依赖：

sudo apt-get install libssl-dev libusb-1.0-0-dev libudev-dev pkg-config libgtk-3-dev cmake

sudo apt-get install libglfw3-dev libgl1-mesa-dev libglu1-mesa-dev at

安装：

./scripts/setup_udev_rules.sh

查看kernel版本：

root@ubuntu:~# cat /proc/version
Linux version 5.4.0-173-generic (buildd@lcy02-amd64-101) (gcc version 9.4.0 (Ubuntu 9.4.0-1ubuntu1~20.04.2)) #191-Ubuntu SMP Fri Feb 2 13:55:07 UTC 2024

因版本大于5.13，执行以下命令：

./scripts/patch-realsense-ubuntu-lts-hwe.sh

cd /root/realsense

mkdir build && cd build

cmake ../ -DBUILD_EXAMPLES=true

sudo make uninstall && make clean && make && sudo make install

8. 启动

avoidance.launch依赖的参数：

<launch>
    <arg name="ns" default="/"/>
    <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
    <arg name="gcs_url" default="" />   <!-- GCS link is provided by SITL -->
    <arg name="tgt_system" default="1" />
    <arg name="tgt_component" default="1" />

    <!-- Launch MavROS -->
    <group ns="$(arg ns)">
        <include file="$(find mavros)/launch/node.launch">
            <arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
            <!-- Need to change the config file to get the tf topic and get local position in terms of local origin -->
            <arg name="config_yaml" value="$(find local_planner)/resource/px4_config.yaml" />
            <arg name="fcu_url" value="$(arg fcu_url)" />
            <arg name="gcs_url" value="$(arg gcs_url)" />
            <arg name="tgt_system" value="$(arg tgt_system)" />
            <arg name="tgt_component" value="$(arg tgt_component)" />
        </include>
    </group>

    <!-- Launch cameras -->
<node pkg="tf" type="static_transform_publisher" name="tf_camera" required="true"
 args="0.3 0.32 -0.11 0 0 0 fcu camera_link 10"/>
<include file="$(find local_planner)/launch/rs_depthcloud.launch">
<arg name="required"              value="true"/>
<arg name="namespace"             value="camera" />
<arg name="tf_prefix"             value="camera" />
<arg name="serial_no"             value="238222071892"/>
<arg name="depth_fps"             value="15"/>
</include>

      <!-- launch node to throttle depth images for logging -->
      <node name="drop_camera_depth" pkg="topic_tools" type="drop" output="screen"
        args="/camera/depth/image_rect_raw 29 30">
      </node>
      <node name="drop_camera_ir" pkg="topic_tools" type="drop" output="screen"
        args="/camera/infra1/image_rect_raw 29 30">
      </node>
    <!-- Launch avoidance -->
    <env name="ROSCONSOLE_CONFIG_FILE" value="$(find local_planner)/resource/custom_rosconsole.conf"/>
    <arg name="pointcloud_topics" default="[/camera/depth/color/points]"/>

    <node name="local_planner_node" pkg="local_planner" type="local_planner_node" output="screen" required="true" >
      <param name="goal_x_param" value="0" />
      <param name="goal_y_param" value="0"/>
      <param name="goal_z_param" value="4" />
      <rosparam param="pointcloud_topics" subst_value="True">$(arg pointcloud_topics)</rosparam>
    </node>
    
    <!-- set or toggle rqt parameters -->
    <node name="rqt_param_toggle" pkg="local_planner" type="rqt_param_toggle.sh" />

</launch>

static_transform_publisher的入参

fcu_url:

$(find mavros)：这个在环境里是ros mavros路径，我的环境是/opt/ros/noetic/share/mavros/

jike28

关注

24
点赞
踩
13

收藏

觉得还不错? 一键收藏
1
评论
PX4 avoidance学习

更新kernel：sudo apt-get update && sudo apt-get upgrade && sudo apt-get dist-upgrade。$(find mavros)：这个在环境里是ros mavros路径，我的环境是/opt/ros/noetic/share/mavros/编译完成后，相关的可执行程序和动态库会生成在：/root/catkin_ws/devel/假设px4-avoidance的代码放在目录：~/catkin_ws/src。
复制链接

扫一扫