Amov二次开发
https://wiki.amovlab.com/public/prometheus-wiki/%E5%BF%AB%E9%80%9F%E4%B8%8A%E6%89%8B/Prometheus%E4%BB%BF%E7%9C%9F%E7%8E%AF%E5%A2%83%E9%85%8D%E7%BD%AE_Ubuntu/prometheus_px4%E9%85%8D%E7%BD%AE.html按这个wiki先把基础的环境配好。
下面是关于mid360激光雷达，以及fast-lio建图，再到octomap建图的脚本流程，后续还要加检测，所以在该飞机上增加一个单目相机。
1.首先创建新的飞机模型p600_mid360_cam
参考上述路径下的p600即可，只把其中的sdf文件进行修改，在其中增加如下：
<!-- Monocular Camera --> <link name='camera_link'> <pose>0.1 0 0.2 0 0 0</pose> <inertial> <mass>0.015</mass> <inertia> <ixx>4.15e-6</ixx> <ixy>0</ixy> <ixz>0</ixz> <iyy>2.407e-6</iyy> <iyz>0</iyz> <izz>2.407e-6</izz> </inertia> </inertial> <collision name='camera_collision'> <geometry> <box> <size>0.01 0.01 0.01</size> </box> </geometry> </collision> <visual name='camera_visual'> <geometry> <box> <size>0.01 0.01 0.01</size> </box> </geometry> </visual> <sensor name='camera' type='camera'> <camera name='__default__'> <horizontal_fov>1.047</horizontal_fov> <image> <width>640</width> <height>480</height> </image> <clip> <near>0.1</near> <far>100</far> </clip> <lens> <type>custom</type> <custom_function> <c1>1.05</c1> <c2>4</c2> <f>1</f> <fun>tan</fun> </custom_function> <scale_to_hfov>1</scale_to_hfov> </lens> </camera> <always_on>1</always_on> <update_rate>30</update_rate> <visualize>1</visualize> <plugin name='camera_plugin' filename='libgazebo_ros_camera.so'> <robotNamespace></robotNamespace> <alwaysOn>true</alwaysOn> <imageTopicName>/uav{{ mavlink_id }}/camera/image_raw</imageTopicName> <cameraInfoTopicName>/uav{{ mavlink_id }}/camera/camera_info</cameraInfoTopicName> <updateRate>30.0</updateRate> <cameraName>usb_cam</cameraName> <frameName>uav{{ mavlink_id }}/camera_link</frameName> <CxPrime>320.5</CxPrime> <Cx>320.5</Cx> <Cy>240.5</Cy> <hackBaseline>0</hackBaseline> <focalLength>277.191356</focalLength> <distortionK1>0.0</distortionK1> <distortionK2>0.0</distortionK2> <distortionK3>0.0</distortionK3> <distortionT1>0.0</distortionT1> <distortionT2>0.0</distortionT2> </plugin> </sensor> </link> <joint name='camera_joint' type='fixed'> <child>camera_link</child> <parent>base_link</parent> </joint>2.增加新的脚本，实现整个流程
#!/bin/bash # 脚本描述: 启动带MID360激光雷达和单目相机的无人机，进行SLAM建图和路径规划 UAV_ID=1 echo "======================================" echo "启动顺序:" echo "1. roscore" echo "2. Gazebo仿真环境 (p600_mid360_cam)" echo "3. 无人机控制节点" echo "4. FAST-LIO SLAM建图" echo "5. Octomap转化" echo "6. Ego-Planner路径规划" echo "======================================" gnome-terminal --window -e 'bash -c "roscore; exec bash"' \ --tab -e 'bash -c "sleep 3; roslaunch prometheus_gazebo sitl_indoor_1uav_P600_mid360_cam.launch uav1_id:='$UAV_ID' uav1_init_x:=-0.0 uav1_init_y:=-0.0; exec bash"' \ --tab -e 'bash -c "sleep 4; roslaunch prometheus_uav_control uav_control_main_indoor_mid360.launch uav_id:='$UAV_ID'; exec bash"' \ --tab -e 'bash -c "sleep 4; roslaunch fast_lio mapping_mid360_gazebo.launch; exec bash"' \ --tab -e 'bash -c "sleep 5; roslaunch prometheus_gazebo mid360_to_octomap.launch; exec bash"' \ --tab -e 'bash -c "sleep 6; roslaunch ego_planner sitl_ego_octomap_mid360.launch uav_id:='$UAV_ID'; exec bash"' \脚本放在Prometheus/Scripts/simulation/ego_planner/p600_ego_mid360_cam.sh中,但我们这里要做一些修改，
1.先把mid360.sdf调用的mid360.csv文件的路径修改，之前是硬解码，改成自己的电脑路径：修改之后，要对该插件.cpp文件进行编译，输入以下命令：
cd Prometheus
catkin_make --source Simulator/realsense_gazebo_plugin --build build/realsense_gazebo_plugin
（更多的编译命令在Promethues下的compile_all.sh脚本中寻找）
修改完后可以输入如下命令查看输出：
rostopic echo /livox/liadar如果不断有输出说明修改成功。
3.增加新的airframe文件来让px4加载，具体操作如下：
# 查看现有配置文件内容
cat /home/lsc/prometheus_px4/ROMFS/px4fmu_common/init.d-posix/airframes/1023_p600_mid360
# 创建新的配置文件
sudo cp /home/lsc/prometheus_px4/ROMFS/px4fmu_common/init.d-posix/airframes/1023_p600_mid360 /home/lsc/prometheus_px4/ROMFS/px4fmu_common/init.d-posix/airframes/1029_p600_mid360_cam_indoor
# 修改配置文件头部的模型名称
sudo sed -i 's/p600_mid360/p600_mid360_cam_indoor/g' /home/lsc/prometheus_px4/ROMFS/px4fmu_common/init.d-posix/airframes/1029_p600_mid360_cam_indoor
其实就是复制之前的p600配置，写一个文件即可。修改完这个之后就发现imu有输出了：
rostopic hz /uav1/mavros/imu/data输入上述命令即可查看。
至此fast-lio建图所需的两个输入节点应该都有输出了。
4.修改fast-lio的laserMapping.cpp文件，将其中的日志文件路径修改为软解码，
先在此处修改路径：
在然后在156行加入如下函数：
static void init() { // 从环境变量获取日志目录，默认为用户主目录 const char* env_dir = std::getenv("FAST_LIO_LOG_DIR"); if (env_dir != nullptr) { log_dir = env_dir; } else { // 获取用户主目录 const char* home = std::getenv("HOME"); if (home != nullptr) { log_dir = std::string(home) + "/odometry_logs/"; } else { log_dir = "/tmp/odometry_logs/"; } }并且在205行下增加定义：
std::string OdometryLogger::current_filename;修改完后编译，去compile_all.sh中寻找命令：
# 编译Fast-lio模块 catkin_make --source Modules/FAST_LIO --build build/FAST_LIO编译之后重新启动fast-lio节点，rosnode info /laserMapping就可以看到fast-lio订阅和发布的话题，这里我们主要看一下两个话题是否有输出：
rostopic hz /uav1/mid360_world_point rostopic hz /uav1/mid360_lidar_point，一般这俩有输出的话，就可以在rviz中看到图像显示了：
5.