【走行記録】車を使ったRtabmap 3D Slam（深度カメラ）のシミュレーション環境

開発 2023-09-19 03:40:08 訪問数: null

【運営背景】

ROS1 20.04 ノエティック

目次

【運営背景】

【取り付け手順】

[Rtabmapのインストール]

[pointcloud-to-laserscan をインストールする]

【ガゼボシミュレーション】

【カーURDFモデル】

[起動ファイルの書き込み]

[Rtabmapの設定]

【Rtabmap設定ファイル】

[点群からレーダー信号まで]

【統合起動ファイル】

[Rtabmap シミュレーション スラムを実行する]

 

 

【取り付け手順】

[Rtabmapのインストール]

sudo apt install ros-noetic-rtabmap-*

[pointcloud-to-laserscan をインストールする]

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

【ガゼボシミュレーション】

すべてはスパークスラムガゼボのワークスペースの下にあります

【カーURDFモデル】

spark1_description_rtabmap.urdf として記述され、Cartographer には imu と pointcloud データが必要なので、センサーのこれら 2 つの部分を追加します

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 上の点群が正しく表示されていることを確認し、仮想ジョイントを追加し、それをカメラのリンクに関連付け、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を設定します（この部分は上記のカメラ追加コードと重複します）

 <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>

[起動ファイルの書き込み]

名前は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設定ファイル】

Rtabmapを起動するためのspark_mapping_astrapro.launchファイルを作成します。

<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>

【統合起動ファイル】

上記で使用したノードを起動ファイルに書き込みます

<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>

次に、ガゼボ シミュレーションと 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 シミュレーション スラムを実行する]

端子入力

roslaunch spark1_description spark_rtab_slam_gazebo_rviz.launch

開始したら、簡単な壁とバリアを追加します

 rvizで見る

 キーボード制御による完全なマッピング

 

 Rtabmapクラウドを表示しないように設定する

 map_server 経由でマップを保存

端末に保存された場所が表示されます

 

おすすめ

転載: blog.csdn.net/weixin_44362628/article/details/122744802
おすすめ
ランキング
データ要素とメタデータ
SVNパッチ機能
ポート情報を追加する方法
Linuxの基本的なコマンド--- exportfsの管理NFSマウントされたファイルシステム
Ubuntuの/etc/sysctl.confファイルについての深い理解
ECMAScript JavaScript JScriptActionScript演算子の優先順位
春の学習11 - 統合MyBatisの
Vue3 は応答性の原理を分析します
java.util.zip.ZipException：によって引き起こされるエントリは名前が付いていません
Développement de visualisation grand écran BI code source Java+.Net6 code source grand écran développement grand écran version 3.4.7 partage de code source
アーカイブ
もっと
2024-05-26(0)
2024-05-25(1)
2024-05-24(11)
2024-05-23(35)
2024-05-22(9)
2024-05-21(36)
2024-05-20(5)
2024-05-19(0)
2024-05-18(30)
2024-05-17(6)

コードワールド

© 2018 codetd.com