千层山 万重浪 比不过县城一碗汤 多加辣 不要糖 没事儿别看老板娘
血过眉 眼飙泪 不吃完夜宵不许醉 别追别追别追我叫你别追 今夜我陪你化成灰
使用的模型为waffle_pi，目的是在turtlebot3上使用深度相机kinect而不是激光雷达来进行空间信息的获取，从而实现相应算法如rtabmap。
在机器人底盘上添加kinect
①构建kinect描述文件kinect_gazebo.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="kinect_camera">
<xacro:macro name="kinect_camera" params="prefix:=camera">
<!-- Create kinect reference frame -->
<!-- Add mesh for kinect -->
<link name="${prefix}_link">
<origin xyz="0 0 0" rpy="0 0 0"/>
<visual>
<origin xyz="0 0 0" rpy="0 0 ${M_PI/2}"/>
<geometry>
<mesh filename="package://mbot_description/meshes/kinect.dae" />
</geometry>
</visual>
<collision>
<geometry>
<box size="0.07 0.3 0.09"/>
</geometry>
</collision>
</link>
<joint name="${prefix}_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
<parent link="${prefix}_link"/>
<child link="${prefix}_frame_optical"/>
</joint>
<link name="${prefix}_frame_optical"/>
<gazebo reference="${prefix}_link">
<sensor type="depth" name="${prefix}">
<always_on>true</always_on>
<update_rate>20.0</update_rate>
<camera>
<horizontal_fov>${60.0*M_PI/180.0}</horizontal_fov>
<image>
<format>R8G8B8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>8.0</far>
</clip>
</camera>
<plugin name="kinect_${prefix}_controller" filename="libgazebo_ros_openni_kinect.so">
<cameraName>${prefix}</cameraName>
<alwaysOn>true</alwaysOn>
<updateRate>10</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>${prefix}_frame_optical</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>
</xacro:macro>
</robot>
位于turtlebot3_description/urdf文件夹下
②添加kinect外观文件，kinect.jpg Kinect.tga Kinect.dae
位于turtlebot3_description/meshes/urdf文件夹下
③将kinect放到底盘上，改写turtlebot3_waffle_pi.urdf.xacro文件
根据kinect_gazebo.xacro的定义，调用kinect要定义prefix即命名前缀，设置为kinect，还要定义kinect在三个轴上的初始位置，即文件中的kinect_offset_x、kinect_offset_y、kinect_offset_z
④缩放，若无kinect_gazebo.xacro文件中的scale，即尺度缩放，kinect和整个turtlebot差不多大，所以选取0.4作为缩放尺度
⑤查看，运行turtlebot3_gazebo中的turtlebot3_world.launch
可以看到，此时已出现kinect，且位置正确。
在原有的底盘上移走激光雷达
①修改turtlebot3_waffle_pi.gazebo.xacro文件，将原有的激光雷达模块注释掉，如图
②修改turtlebot3_waffle_pi.urdf.xacro文件，将原有的激光雷达模块注释掉，如图
③查看，运行turtlebot3_gazebo中的turtlebot3_world.launch
可以看到，激光雷达已经移除
调用kinect数据进行rtabmap三维建图
①安装rtabmap
sudo apt-get install ros-kinetic-rtabmap-ros
②编写rtabmap启动文件，即rtabmap_demo.launch
<launch>
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg name="rgb_topic" value="/kinect/rgb/image_raw" />
<arg name="depth_topic" value="/kinect/depth/image_raw" />
<arg name="camera_info_topic" value="/kinect/rgb/camera_info" />
<arg name="depth_camera_info_topic" value="/kinect/depth/camera_info" />
<arg name="frame_id" value="base_footprint"/>
<arg name="visual_odometry" value="false"/>
<arg name="odom_topic" value="/odom"/>
<arg name="rtabmap_args" value="--delete_db_on_start"/>
<arg name="rviz" value="true"/>
<arg name="rtabmapviz" value="true"/>
</include>
</launch>
里面添加robot_state_publisher节点来发布xacro中定义好的机器人的相对关节信息
③启动turtlebot3的仿真环境
roslaunch turtlebot3_gazebo turtlebot3_world.launch
④启动键盘控制节点
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
⑤启动rtabmap建图
roslaunch turtlebot3_gazebo rtabmap_demo.launch
⑥通过键盘控制机器人运动，实时三维建图
tf.TransformListener()
-
Twist - 线速度角速度
通常被用于发送到/cmd_vel话题，被base controller节点监听，控制机器人运动
inear.x指向机器人前方，linear.y指向左方，linear.z垂直向上满足右手系，平面移动机器人常常linear.y和linear.z均为0
angular.z代表平面机器人的角速度，因为此时z轴为旋转轴 -
nav_msgs/Odometry - 里程计（位姿+线速度角速度+各自协方差）
通常，发布到/cmd_vel topic然后被机器人（例如/base_controller节点）监听到的Twist消息是不可能准确地转换为实际的机器人运动路径的，误差来源于机器人内部传感器误差（编码器），标定精度，以及环境影响（地面是否打滑平整）；因此我们可以用更准确的Odometry消息类型类获取机器人位姿（/odom到/base_link的tf变换）。在标定准确时，该消息类型与真实的机器人位姿误差大致在1%量级（即使在rviz中显示的依然误差较大）。
需要坐标变换的原因就在于机器人在执行任务的过程中，需要知道自己的准确位置并且知道世界中其余元素的相对位置。tf库是由两大模块组成：Broadcaster和Listener。前者用于把变换的信息（平移量和旋转量）发送给整个系统，后者接收这些坐标变换的信息并存储到指定变量中。
self.tf_listener = tf.TransformListener()
rospy.sleep(2)
创建TransformListener对象监听坐标系变换，这里需要sleep 2s用于tf缓冲。
可以通过以下API获取tf变换，保存在TransformListener对象中，通过lookupTransform获取：
# TransformListener.waitForTransform('ref_coordinate', 'moving_coordinate', rospy.Time(), rospy.Duration(1.0))
self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
遇到的问题
Could not get transform from base_link to kinect_frame_optical after 0.200000 seconds (for stamp=14.
发现rviz能识别自定义的base_link，world以及kinect_frmae_optical(urdf里面定义的),但是不能获取frame与frame之间的变换关系（TF显示不出frame的信息），我在想应该少了一个joint_states发布器，于是在launch中包含下面的节点：
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
PluginlibFactory: The plugin for class 'octomap_rviz_plugin/OccupancyGrid' failed toload.
无法加载插件，很明显是缺少库，参考以下网址发现在之前网页里有依赖库的安装https://emanual.robotis.com/docs/en/platform/turtlebot3/simulation/#ros-1-simulation
> mkdir c:\ws\turtlebot3\src
> cd c:\ws\turtlebot3\src
> catkin_init_workspace
> git clone -b melodic-devel https://github.com/ROBOTIS-GIT/turtlebot3_msgs
> git clone -b melodic-devel https://github.com/ROBOTIS-GIT/turtlebot3_simulations
> git clone -b melodic-devel https://github.com/ROBOTIS-GIT/turtlebot3
> git clone -b melodic-devel https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver
> cd c:\ws\turtlebot3
> rosdep update
> rosdep install --from-paths src --ignore-src -r -y
> catkin_make
> devel\setup.bat
rm -rf * 这个命令的意思是:删除当前目录下的所有文件.
一般情况下用不到这个命令,而且这个命令很危险，如果一旦执行,会删除当前目录下所有的文件,而且不能够恢复.因此,应避免使用。
rm 命令 可以删除一个目录中的一个或多个文件或目录，也可以将某个目录及其下属的所有文件及其子目录均删除掉。对于链接文件，只是删除整个链接文件，而原有文件保持不变。
rm -rf [文件名] 删除该文件或文件夹
rm -rf * 删除目录下所有文件和文件夹
