真实环境安装livox-360 ROS版本驱动
1. 安装Livox SDK2
git clone https://github.com/Livox-SDK/Livox-SDK2.git
cd Livox-SDK2/
mkdir build && cd build
cmake ..
make -j
sudo make install
卸载sdk
$ sudo rm -rf /usr/local/lib/liblivox_lidar_sdk_*
$ sudo rm -rf /usr/local/include/livox_lidar_*
2. 安装ros驱动
git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2
cd livox_ros_driver2
source /opt/ros/noetic/setup.sh
# For ros1
./build.sh ROS1
3. 连接激光雷达与PC
网口线连接是通过enx…
设置激光雷达ip,子网掩码,网关如下
修改/src/livox_ros_driver2/config/MID360_config.json
中对应的IP地址
其中 "host_net_info"
中的所有ip都为激光雷达的ip
"lidar_configs"
中的"ip"
为192.168.1.1xx,其中xx为激光雷达SN码的最后两位
{
"lidar_summary_info" : {
"lidar_type": 8
},
"MID360": {
"lidar_net_info" : {
"cmd_data_port": 56100,
"push_msg_port": 56200,
"point_data_port": 56300,
"imu_data_port": 56400,
"log_data_port": 56500
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.50",
"cmd_data_port": 56101,
"push_msg_ip": "192.168.1.50",
"push_msg_port": 56201,
"point_data_ip": "192.168.1.50",
"point_data_port": 56301,
"imu_data_ip" : "192.168.1.50",
"imu_data_port": 56401,
"log_data_ip" : "",
"log_data_port": 56501
}
},
"lidar_configs" : [
{
"ip" : "192.168.1.126",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"x": 0,
"y": 0,
"z": 0
}
}
]
}
XTDrone安装livox-360 ROS驱动
参考链接:https://blog.csdn.net/qq_32761549/article/details/134602816
1. 安装相关依赖
安装livox_ros_driver
cd catkin_ws/src
git clone https://github.com/Livox-SDK/livox_ros_driver.git
cd ..
catkin_make
安装livox360激光雷达仿真模型
cd ~/catkin_ws/src
git clone https://github.com/Luchuanzhao/Livox_simulation_customMsg.git
mv Livox_simulation_customMsg livox_laser_simulation
通过修改~/catkin_ws/src/livox_laser_simulation/src/livox_points_plugin.cpp
源码将CustomMsg点云格式为PointCloud
添加mid360.csv
这个文件在home/byy/catkin_ws/src/livox_laser_simulation/scan_mode
文件夹
#将101行改为自己所需要的点云格式:0->PointCloud;1/2->PointCloud2;3->CustomMsg
publishPointCloudType = 0;
#注释掉340、341行的坐标变换代码,要不然rviz中得不到图像
// tfBroadcaster->sendTransform(
// tf::StampedTransform(tf, ros::Time::now(), raySensor->ParentName(), raySensor->Name()));
2. 编写lidar模型和启动文件
首先,在meshes文件夹中引入提供的mid360.dae、mid360_rules.dae文件
创建一个lidar+imu连接的模型来模拟mid360
在~/catkin_ws/src/livox_laser_simulation/urdf
创建mid360_IMU_platform.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="example">
<!-- Base Footprint -->
<link name="base_footprint" />
<!-- Base Link -->
<joint name="footprint" type="fixed" >
<parent link="base_footprint" />
<child link="link_platform" />
<origin xyz="0 0 0.05" rpy="0 0 0" />
</joint>
<link name="link_platform" >
<visual>
<geometry>
<box size="0.5 0.5 0.1" />
</geometry>
</visual>
<collision>
<geometry>
<box size="0.5 0.5 0.1" />
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0"/>
<mass value="5"/><!-- add weight -->
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
</link>
<!--lidar -->
<joint name="lidar_platform" type="fixed" >
<parent link="link_platform" />
<child link="livox_base" />
<origin xyz="0 0 0.08" rpy="0 0 0" />
</joint>
<xacro:include filename="$(find livox_laser_simulation)/urdf/livox_mid360.xacro"/>
<!-- <xacro:Livox_Mid360 name="livox"/> -->
<!--imu -->
<link name="imu_base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry >
<box size="0.03 0.03 0.03" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry >
<box size="0.03 0.03 0.03" />
</geometry>
</collision>
<inertial>
<mass value="0.001"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>
<gazebo reference="imu_base_link">
<material>Gazebo/Green</material>
<turnGravityOff>true</turnGravityOff>
</gazebo>
<joint name="imu_platform_joint" type="fixed">
<parent link="link_platform"/>
<child link="imu_base_link"/>
<origin xyz="0.05 0 0.065" rpy="0 0 0" />
<axis xyz="0 0 1" />
</joint>
<gazebo reference="imu_base_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>200</update_rate>
<visualize>true</visualize>
<topic>/livox/imu</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>/livox/imu</topicName>
<bodyName>imu_base_link</bodyName>
<updateRateHZ>200.0</updateRateHZ>
<gaussianNoise>0.00329</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_base_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
</robot>
在urdf文件夹中创建一个雷达模型,livox_mid360.xacro文件
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:property name="M_PI" value="3.14159"/>
<xacro:property name="laser_min_range" value="0.1"/>
<xacro:property name="laser_max_range" value="200.0"/>
<xacro:property name="ros_topic" value="scan"/>
<xacro:property name="samples" value="24000"/>
<xacro:property name="downsample" value="1"/>
<xacro:macro name="null_inertial">
<inertial>
<mass value="0.1"/>
<inertia ixx="0.01" ixy="0" ixz="0"
iyy="0.01" iyz="0"
izz="0.01"/>
</inertial>
</xacro:macro>
<xacro:macro name="Livox_Mid_gazebo_sensor" params="visualize:=True update_rate:=10 resolution:=0.002 noise_mean:=0.0 noise_stddev:=0.01 name:=livox">
<gazebo reference="${name}">
<sensor type="ray" name="laser_${name}">
<pose>0 0 0 0 0 0</pose>
<visualize>${visualize}</visualize>
<update_rate>${update_rate}</update_rate>
<!-- This ray plgin is only for visualization. -->
<plugin name="gazebo_ros_laser_controller" filename="liblivox_laser_simulation.so">
<ray>
<scan>
<horizontal>
<samples>100</samples>
<resolution>1</resolution>
<min_angle>${0}</min_angle>
<max_angle>${2*M_PI}</max_angle>
</horizontal>
<vertical>
<samples>360</samples>
<resolution>1</resolution>
<min_angle>${-7.22/180*M_PI}</min_angle>
<max_angle>${55.22/180*M_PI}</max_angle>
</vertical>
</scan>
<range>
<min>${laser_min_range}</min>
<max>${laser_max_range}</max>
<resolution>${resolution}</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>${noise_mean}</mean>
<stddev>${noise_stddev}</stddev>
</noise>
</ray>
<visualize>${visualize}</visualize>
<samples>${samples}</samples>
<downsample>${downsample}</downsample>
<csv_file_name>package://livox_laser_simulation/scan_mode/mid360.csv</csv_file_name>
<ros_topic>${ros_topic}</ros_topic>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
<xacro:macro name="Livox_Mid40" params="visualize:=True name:=livox">
<link name="${name}_base">
<xacro:null_inertial/>
<visual>
<origin xyz="0.00 0 0.0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://livox_laser_simulation/meshes/mid360.dae">
</mesh>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" /> <geometry> <mesh
filename="package://livox_laser_simulation/meshes/mid360_rules.dae">
</mesh> </geometry>
</collision>
</link>
<link name="${name}">
<xacro:null_inertial/>
</link>
<joint name="${name}_to_${name}_base_joint" type="fixed">
<parent link="${name}_base"/>
<child link="${name}"/>
<origin rpy="0 0 0" xyz="0.0 1.0 0.2"/>
</joint>
<xacro:Livox_Mid_gazebo_sensor name="${name}" visualize="${visualize}"/>
</xacro:macro>
<xacro:Livox_Mid40 name="livox"/>
<!--xacro:include filename="$(find livox_laser_simulation)/urdf/standardrobots_oasis300.xacro"/>
<xacro:link_oasis name="oasis"/-->
</robot>
编写启动文件
在~/catkin_ws/src/livox_laser_simulation/launch
中创建mid360_IMU_platform.launch
<?xml version="1.0" ?>
<launch>
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<!-- Start gazebo and load the world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch" >
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="headless" value="$(arg headless)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
</include>
<!-- Spawn the platform -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find livox_laser_simulation)/urdf/mid360_IMU_platform.xacro' " />
<node pkg="gazebo_ros" type="spawn_model" name="spawn_model" args="-urdf -param /robot_description -model example"/>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="30.0" />
</node>
<!-- RViz -->
<arg name="rviz" default="true"/>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find livox_laser_simulation)/rviz/livox_simulation.rviz"/>
</launch>