1. 开源库
1.1. Eigen
1.1.1. Eigen::Vector3f
【GAMES101】eigen常用语法_eigen 定义三维向量-优快云博客
1.2. catkintools
ROS 2设计文章系列之三——通用构建工具开发设计 - 知乎
1.3. rviz
1.4. Nodelet
1.5. tf
tf:transform,ROS中管理3D坐标系变换的工具。
tf之static_transform_publisher-优快云博客
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/