标签：launch true 0.0 camera Slam link Rtabmap 3D
【运行背景】
ROS1 20.04 noetic
目录
【安装步骤】
【安装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
启动后，添加简易围墙和障碍物
在rviz上查看
通过键盘控制完成建图
将Rtabmap cloud设为不显示
通过map_server保存地图
终端显示保存的位置
标签：launch,true,0.0,camera,Slam,link,Rtabmap,3D 来源： https://blog.csdn.net/weixin_44362628/article/details/122744802
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。