【踩坑记录】仿真环境使用小车进行Rtabmap 3D Slam(深度摄像头)

【运行背景】

ROS1 20.04 noetic

目录

【运行背景】

【安装步骤】

【安装Rtabmap】

【安装pointcloud-to-laserscan】

【Gazebo仿真】

【小车URDF模型】

【编写launch文件】

【配置Rtabmap】

【Rtabmap配置文件】

【点云转雷达信号】

【整合启动文件】

【运行Rtabmap仿真slam】

 

 


【安装步骤】

【安装Rtabmap】

sudo apt install ros-noetic-rtabmap-*

【安装pointcloud-to-laserscan】

sudo apt install ros-noetic-pointcloud-to-laserscan

【Gazebo仿真】

均在spark-slam-gazebo工作空间下

【小车URDF模型】

编写为spark1_description_rtabmap.urdf,Cartographer需要imu和pointcloud的数据,所以添加这两部分的传感器

a)添加深度摄像头

  <link name="virtual_camera_link" />
  <joint name="virtual_camera_joint"
		type="fixed">
		<origin
      xyz="0 0 0.03"
      rpy="0 0 0" />
		<parent link="camera_Link" />
		<child link="virtual_camera_link" />
	</joint>	
 
 
 <gazebo reference="virtual_camera_link">
     <sensor type="depth" name="camera">
           <always_on>true</always_on>
           <update_rate>20.0</update_rate>
           <camera>
               <horizontal_fov>${60.0*3.14/180.0}</horizontal_fov>
                   <image>
                       <format>R8G8B8</format>
                       <width>640</width>
                       <height>480</height>
                   </image>
                   <clip>
                         <near>0.05</near>
                         <far>5.0</far>
                   </clip>
           </camera>
                <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
                    <cameraName>camera</cameraName>
                    <alwaysOn>true</alwaysOn>
                    <updateRate>20.0</updateRate>
                    <imageTopicName>rgb/image_raw</imageTopicName>
                    <depthImageTopicName>depth/image_raw</depthImageTopicName>
                    <pointCloudTopicName>depth/points</pointCloudTopicName>
                    <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
                    <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
                    <frameName>upper_kinect_pointcloud_link</frameName>
                    <baseline>0.1</baseline>
                    <distortion_k1>0.0</distortion_k1>
                    <distortion_k2>0.0</distortion_k2>
                    <distortion_k3>0.0</distortion_k3>
                    <distortion_t1>0.0</distortion_t1>
                    <distortion_t2>0.0</distortion_t2>
                    <pointCloudCutoff>0.4</pointCloudCutoff>
                    
                </plugin>
            </sensor>
        </gazebo>  

b) 添加imu

<link name="imu_link">
    <visual>
      <origin xyz="0.0 0.0 0.0"/>
      <geometry>
        <cylinder length="0.07" radius="0.05"/>
      </geometry>
    </visual>
  </link>
   <joint
    name="imu_joint"
    type="fixed">
    <origin
      xyz="-0.0 0.0 0.0"
      rpy="0 0 0" />
    <parent
      link="base_footprint" />
    <child
      link="imu_link" />
    <axis
      xyz="0 1 0" />
  </joint>
 
 
<gazebo reference="imu_link">
        <gravity>true</gravity>
        <sensor name="imu_sensor" type="imu">
            <always_on>true</always_on>
            <update_rate>20</update_rate>
            <visualize>true</visualize>
            <topic>__default_topic__</topic>
            <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">  
                <topicName>imu</topicName>             
                <bodyName>imu_link</bodyName>
                <updateRateHZ>20</updateRateHZ>     
                <gaussianNoise>0.0</gaussianNoise>     
                <rpyOffset>0 0 0</rpyOffset>
                <frameName>imu_link</frameName>        
            </plugin>
            <pose>0 0 0 0 0 0</pose>
        </sensor>
    </gazebo>

c)确保rviz上点云显示正确,添加虚拟关节,关联到摄像机的link,调整rpy值(rqy分别对应着xyz轴,遵循右手定理),将点云调整到正确位置

  <link name="upper_kinect_pointcloud_link"/>
<joint name="upper_kinect_pointcloud_joint" type="fixed">
    <origin xyz="0 0 0" rpy="-1.57 0 -1.57"/>
    <parent link="virtual_camera_link"/>
    <child link="upper_kinect_pointcloud_link"/>
</joint>

将深度摄像头插件中的frameName设为新增加的link(这一部分和上述添加摄像头代码有重合部分)

 <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
                    <cameraName>camera</cameraName>
                    <alwaysOn>true</alwaysOn>
                    <updateRate>20.0</updateRate>
                    <imageTopicName>rgb/image_raw</imageTopicName>
                    <depthImageTopicName>depth/image_raw</depthImageTopicName>
                    <pointCloudTopicName>depth/points</pointCloudTopicName>
                    <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
                    <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
                    <frameName>upper_kinect_pointcloud_link</frameName>
                    <baseline>0.1</baseline>
                    <distortion_k1>0.0</distortion_k1>
                    <distortion_k2>0.0</distortion_k2>
                    <distortion_k3>0.0</distortion_k3>
                    <distortion_t1>0.0</distortion_t1>
                    <distortion_t2>0.0</distortion_t2>
                    <pointCloudCutoff>0.4</pointCloudCutoff>
                    
                </plugin>

【编写launch文件】

取名为spark_rtabmap_slam_gazebo_rviz.launch,内容如下:

<?xml version="1.0"?>
<launch>
  <!-- 设置launch文件的参数 -->
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="true"/>
    <arg name="debug" default="false"/>
    <arg name="model" default="$(find spark1_description)/urdf/spark1_description_rtabmap.urdf"/>
 
   
    <!-- 运行gazebo仿真环境 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="debug" value="$(arg debug)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>
 
      <!-- 运行joint_state_publisher节点,发布机器人的关节状态  -->
      <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
 
       <!-- 运行robot_state_publisher节点,发布tf  -->
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" >
            <param name="publish_frequency" type="double" value="50.0" />
        </node>
 
     <!-- 在gazebo中加载机器人模型-->
     <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
     <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
          args="-unpause -urdf -model marobot1 -param robot_description -x 0.0 -y 0.0"/>
 
	      
      	<!--创建新的终端,spark键盘控制 “wsad”分别代表“前后左右”-->
  	  <node pkg="spark_teleop" type="keyboard_control.sh" name="kc_2d" />
 
</launch>

【配置Rtabmap】

【Rtabmap配置文件】

编写spark_mapping_astrapro.launch文件启动Rtabmap

<launch>
  <arg name="database_path"     default="rtabmap.db"/>
  <arg name="rgbd_odometry"     default="false"/>
  <arg name="rtabmapviz"        default="false"/>
  <arg name="localization"      default="false"/>
  <arg name="simulation"        default="false"/>
  <arg name="sw_registered"     default="false"/>
  <arg     if="$(arg localization)" name="args"  default=""/>
  <arg unless="$(arg localization)" name="args"  default="--delete_db_on_start"/>

  <arg name="rgb_topic" default="/camera/rgb/image_raw"/>
  <arg name="depth_topic" default="/camera/depth/image_raw"/>
  <arg name="camera_info_topic" default="/camera/rgb/camera_info"/>
 
  <arg name="wait_for_transform"  default="0.2"/> 

  <!-- Mapping -->
  <group ns="rtabmap">
 
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">
	  <param name="database_path"       type="string" value="$(arg database_path)"/>
	  <param name="frame_id"            type="string" value="base_footprint"/>
	  <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
	  <param name="subscribe_depth"     type="bool"   value="true"/>
	  <param name="subscribe_scan"      type="bool"   value="true"/>
	  <param name="map_negative_poses_ignored" type="bool" value="true"/>
	  <param name="queue_size"	     type="string"  value="10"/>
          <!-- When sending goals on /rtabmap/goal topic, use actionlib to communicate with move_base --> 
	  <param name="use_action_for_goal" type="bool" value="true"/>
          <remap from="move_base"            to="/move_base"/>
	
	  <!-- inputs -->
	  <remap from="scan"            to="/scan"/>
	  <remap from="rgb/image"       to="$(arg rgb_topic)"/>
  	  <remap from="depth/image"     to="$(arg depth_topic)"/>
  	  <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>

          <!-- Fix odom covariance as in simulation the covariance in /odom topic is high (0.1 for linear and 0.05 for angular) -->
          <param unless="$(arg rgbd_odometry)" name="odom_frame_id" value="odom"/>
          <param unless="$(arg rgbd_odometry)" name="odom_tf_linear_variance" value="0.001"/>
          <param unless="$(arg rgbd_odometry)" name="odom_tf_angular_variance" value="0.001"/>
  	  
  	  <!-- output -->
  	  <remap from="grid_map" to="/map"/>
	
	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
	  <param name="RGBD/ProximityBySpace"        type="string" value="true"/>   <!-- Local loop closure detection (using estimated position) with locations in WM -->
	  <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="false"/>  <!-- Set to false to generate map correction between /map and /odom -->
	  <param name="Kp/MaxDepth"                  type="string" value="4.0"/>
	  <param name="Reg/Strategy"                 type="string" value="0"/>      <!-- Loop closure transformation: 0=Visual, 1=ICP, 2=Visual+ICP -->
	  <param name="Icp/CorrespondenceRatio"      type="string" value="0.3"/>
	  <param name="Vis/MinInliers"               type="string" value="15"/>      <!-- 3D visual words minimum inliers to accept loop closure -->
	  <param name="Vis/InlierDistance"           type="string" value="0.1"/>    <!-- 3D visual words correspondence distance -->
	  <param name="RGBD/AngularUpdate"           type="string" value="0.1"/>    <!-- Update map only if the robot is moving -->
	  <param name="RGBD/LinearUpdate"            type="string" value="0.1"/>    <!-- Update map only if the robot is moving -->
	  <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/> 
	  <param name="Rtabmap/TimeThr"              type="string" value="0"/>
	  <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
	  <param name="Reg/Force3DoF"                type="string" value="true"/>
	  <param name="GridGlobal/MinSize"           type="string" value="20"/>

	  
	  <!-- localization mode -->
	  <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
	  <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
	  <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/> 
    </node>
   
    <!-- Odometry : ONLY for testing without the actual robot! /odom TF should not be already published. -->
    <node if="$(arg rgbd_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <param name="frame_id"                    type="string" value="base_footprint"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
      <param name="Reg/Force3DoF"               type="string" value="true"/>
      <param name="Vis/InlierDistance"          type="string" value="0.05"/>
      
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
    </node>
    
    <!-- visualization with rtabmapviz -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
  	  <param name="subscribe_depth"             type="bool" value="true"/>
      <param name="subscribe_scan"              type="bool" value="true"/>
      <param name="frame_id"                    type="string" value="base_footprint"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
    
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="scan"            to="/scan"/>
    </node>
    
  </group>
</launch>

    主要修改rgb/image,depth/image,rgb/camera_info和/scan的相关话题,由于采用仿真环境,摄像机相关话题名称由URDF文件中确定,详细可查看上文深度摄像头部分

【点云转雷达信号】

编写pointcloud2scan.launch

<!--spark camera pointcloud to laserscan-->
<launch>
    <arg name="point2laser"	default="/depth/points"/>
    <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
      <remap from="cloud_in" to="$(arg point2laser)"/>
      <remap from="scan" to="/scan" />
      <rosparam>
        target_frame: camera_link
        transform_tolerance: 0.01
        min_height: 0.0
        max_height: 1.0
        
        angle_min: -1.5708
        angle_max: 1.5708
        angle_increment: 0.0087
        scan_time: 0.3333
        range_min: 0.45
        range_max: 4.0
        use_inf: true
        
        #concurrency_level affects number of pc queued for processing and the number of threadsused
        # 0: Detect number of cores
        # 1: Single threaded
        # 2: inf : Parallelism level
        concurrency_level: 1
      </rosparam>
    </node>
</launch>

【整合启动文件】

将上面用到的节点编写到launch文件

<include file="$(find spark1_description)/launch/pointcloud2scan.launch">
		<arg name="point2laser"	value="$(arg point2laser)"/>
	</include>


  	<!-- Move base -->
  	<include file="$(find spark_navigation)/launch/includes/move_base.launch.xml"/>


	<!-- rviz -->
	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find spark_rtabmap)/rviz/spark_rtab_map_astrapro.rviz"/>
	
	<!-- rtabmap -->
  	<include file="$(find spark_rtabmap)/launch/spark_mapping_astrapro.launch">
		<arg name="localization"                  value="true"/>
	</include>

再将gazebo仿真和rtabmap结合一起,修改之前的spark_rtabmap_slam_gazebo_rviz.launch文件

<?xml version="1.0"?>
<launch>

    <!-- 设置launch文件的参数 -->
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="true"/>
    <arg name="debug" default="false"/>
    <arg name="model" default="$(find spark1_description)/urdf/spark1_description_rtabmap.urdf"/>
    <arg name="point2laser" default="/camera/depth_registered/points"/>
   
    <!-- 运行gazebo仿真环境 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <!--<arg name="world_name" value="$(arg world_name)" /> -->
        <arg name="debug" value="$(arg debug)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>

        <!-- 运行joint_state_publisher节点,发布机器人的关节状态  -->
        <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>

        <!-- 运行robot_state_publisher节点,发布tf  -->
        <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" >
            <param name="publish_frequency" type="double" value="50.0" />
        </node>

    	<!-- 在gazebo中加载机器人模型-->
    	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
    	<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
          	args="-unpause -urdf -model marobot1 -param robot_description -x 0.0 -y 0.0"/>
	
	<include file="$(find spark1_description)/launch/pointcloud2scan.launch">
		<arg name="point2laser"	value="$(arg point2laser)"/>
	</include>


  	<!-- Move base -->
  	<include file="$(find spark_navigation)/launch/includes/move_base.launch.xml"/>


	<!-- rviz -->
	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find spark_rtabmap)/rviz/spark_rtab_map_astrapro.rviz"/>
	
	<!-- rtabmap -->
  	<include file="$(find spark_rtabmap)/launch/spark_mapping_astrapro.launch">
		<arg name="localization"                  value="true"/>
	</include>


  	
	<!--创建新的终端,spark键盘控制 “wsad”分别代表“前后左右”-->
  	<node pkg="spark_teleop" type="keyboard_control.sh" name="kc_2d" />
  	
	<!--创建新的终端,确定是否保存地图-->
  	<node pkg="spark_teleop" type="cmd_save_map.sh" name="csm_2d" />



</launch>

【运行Rtabmap仿真slam】

终端输入

roslaunch spark1_description spark_rtab_slam_gazebo_rviz.launch 

启动后,添加简易围墙和障碍物

【踩坑记录】仿真环境使用小车进行Rtabmap 3D Slam(深度摄像头)

 在rviz上查看

【踩坑记录】仿真环境使用小车进行Rtabmap 3D Slam(深度摄像头)

 通过键盘控制完成建图

【踩坑记录】仿真环境使用小车进行Rtabmap 3D Slam(深度摄像头)

 【踩坑记录】仿真环境使用小车进行Rtabmap 3D Slam(深度摄像头)

 将Rtabmap cloud设为不显示

【踩坑记录】仿真环境使用小车进行Rtabmap 3D Slam(深度摄像头)

 通过map_server保存地图

【踩坑记录】仿真环境使用小车进行Rtabmap 3D Slam(深度摄像头)

终端显示保存的位置

 

上一篇:pycharm使用


下一篇:逻辑运算符 &&与& ||与| 的区别