目录
一实验准备
1.1 前置准备
cd catkin_ws #进入工作空间
catkin_create_pkg jubot_demo urdf xacro #创建功能包、添加依赖
cd jubot_demo/
mkdir urdf
mkdir launch
mkdir meshes #存放渲染机器人模型的文件
mkdir config #存放rviz配置的文件
1.1.1 urdf模型+rviz可视化
新建两个文档，jubot_base.urdf（放urdf文件夹下）
display_jubot_base_urdf.launch（放launch文件夹下）
config里的rviz文件是保存生成的，不用写
1.1.2 urdf文件
Unified Robot Description Format，统一机器人描述格式，简称为URDF
上图展示了模型的环节（link）与关节（joint）坐标关系，在基础模型之上，我们为机器人添加尺寸大小。由于每个环节的参考系都位于该环节的底部，关节也是如此，所以在表示尺寸大小时，只需要描述其相对于连接的关节的相对位置关系即可。URDF中的 origin 域就是用来表示这种相对关系。
如果我们为机器人的关节添加 axis 旋转轴参数，那么该机器人模型就可以具备基本的运动学参数。
关于参数欧拉角rpy，是roll（滚转角）、pitch（俯仰角）、yaw（偏航角），分别对应绕x轴、y轴、z轴
<?xml version="1.0" ?>
<robot name="jubot">
<!--base_car-->>
<link name="base_link">
<visual>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<cylinder radius="0.20" length="0.16"/>
</geometry>
<material name="yellow">
<color rgba="1 0.4 0 1"/>
</material>
</visual>
</link>
<!--left_wheel-->>
<joint name="left_wheel_joint" type="continuous">
<origin xyz="0.0 0.19 -0.05" rpy="0.0 0.0 0.0"/>
<parent link="base_link"/>
<child link="left_wheel_link"/>
<axis xyz="0.0 1.0 0.0"/>
</joint>
<link name="left_wheel_link">
<visual>
<origin xyz="0.0 0.0 0.0" rpy="1.5707 0.0 0.0"/>
<geometry>
<cylinder radius="0.06" length="0.025"/>
</geometry>
<material name="white">
<color rgba="1 1 1 0.9"/>
</material>
</visual>
</link>
<!--right_wheel-->>
<joint name="right_wheel_joint" type="continuous">
<origin xyz="0.0 -0.19 -0.05"/>
<parent link="base_link"/>
<child link="right_wheel_link"/>
<axis xyz="0.0 1.0 0.0"/>
</joint>
<link name="right_wheel_link">
<visual>
<origin xyz="0.0 0.0 0.0" rpy="1.5707 0.0 0.0"/>
<geometry>
<cylinder radius="0.06" length="0.025"/>
</geometry>
<material name="white">
<color rgba="1 1.0 1.0 0.9"/>
</material>
</visual>
</link>
<!--front_caster-->
<joint name="front_caster_joint" type="continuous">
<origin xyz="0.18 0.0 -0.095" rpy="0.0 0.0 0.0"/>
<parent link="base_link"/>
<child link="front_caster_link"/>
<axis xyz="0.0 1.0 0.0"/>
</joint>
<link name="front_caster_link">
<visual>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.95"/>
</material>
</visual>
</link>
<!--back_caster-->
<joint name="back_caster_joint" type="continuous">
<origin xyz="-0.18 0.0 -0.095" rpy="0.0 0.0 0.0"/>
<parent link="base_link"/>
<child link="back_caster_link"/>
<axis xyz="0.0 1.0 0.0"/>
</joint>
<link name="back_caster_link">
<visual>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.95"/>
</material>
</visual>
</link>
</robot>
1.1.3 launch文件
<launch>
<!-- 设置机器人模型路径参数 -->
<param name="robot_description" textfile="$(find jubot_demo)/urdf/jubot_base.urdf" />
<!-- 运行joint_state_publisher节点，发布机器人的关节状态 -->
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<!-- 运行robot_state_publisher节点，发布tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- 运行rviz可视化界面 -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find jubot_demo)/config/jubot_urdf.rviz" required="true" />
</launch>
1.1.4 图形化显示
urdf语法工具：
sudo apt-get install liburdfdom-tools
在urdf文件夹下打开终端检查语法：
check_urdf jubot_base.urdf
在urdf文件夹下打开终端，图形化显示URDF模型
urdf_to_graphiz jubot_base.urdf
会生成文件
启动launch文件
roslaunch jubot_demo display_jubot_base_urdf.launch
rviz
1.2 实践
1.2.1建模
创建工作空间
mkdir -p ~/catkin_ws/src/tutorials // 创建文件夹
cd ~/catkin_ws/src/tutorials
mkdir launch // 存放 launch 文件
mkdir urdf // 存放小车模型文件
mkdir world // 存放地图文件
sudo gazebo // 打开gazebo，后续保存文件可能需要管理员权限
点击左上角Edit，再点击倒数第二个，进入编辑页面。
点击Wall，建模墙壁
点击 file 的 save as 保存模型文件，地图绘制好后，保存为 .world 地图文件（文件后缀名一定要是 .world），将 .world 文件复制到 ~/catkin_ws/src/world 文件夹内，地图环境就搭建好了
1.2.2小车
小车模型文件分为两部分，分别为 myrot.xacro 和 myrot.gazebo.xacro ，具体内容如下：
myrot.xacro
<?xml version="1.0"?>
<robot name="mybot" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:include filename="$(find tutorials)/urdf/mybot.gazebo.xacro" />
<link name="base_footprint"/>
<joint name="base_joint" type="fixed">
<parent link="base_footprint"/>
<child link="base_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="base_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.001" />
</inertial>
<visual>
<geometry>
<box size="0.25 0.16 0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="blue">
<color rgba="0 0 0.8 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.25 0.16 0.05"/>
</geometry>
</collision>
</link>
<link name="right_wheel_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
</inertial>
<visual>
<geometry>
<cylinder length="0.02" radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.025"/>
</geometry>
</collision>
</link>
<joint name="right_wheel_joint" type="continuous">
<axis xyz="0 0 -1"/>
<parent link="base_link"/>
<child link="right_wheel_link"/>
<origin rpy="1.5707 0 0" xyz=" 0.1 -0.09 -0.03"/>
</joint>
<link name="left_wheel_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
</inertial>
<visual>
<geometry>
<cylinder length="0.02" radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.025"/>
</geometry>
</collision>
</link>
<joint name="left_wheel_joint" type="continuous">
<axis xyz="0 0 -1"/>
<parent link="base_link"/>
<child link="left_wheel_link"/>
<origin rpy="1.5707 0 0" xyz="0.1 0.09 -0.03"/>
</joint>
<link name="ball_wheel_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
</inertial>
<visual>
<geometry>
<sphere radius="0.025"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.025"/>
</geometry>
</collision>
</link>
<joint name="ball_wheel_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="ball_wheel_link"/>
<origin rpy="0 0 0" xyz="-0.10 0 -0.03"/>
</joint>
<!-- imu sensor -->
<link name="imu">
<visual>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="imu_joint" type="fixed">
<parent link="base_link"/>
<child link="imu"/>
<origin xyz="0.08 0 0.025"/>
</joint>
<!-- camera -->
<link name="base_camera_link">
<visual>
<geometry>
<box size="0.02 0.03 0.03"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="camera_joint" type="fixed">
<parent link="base_link"/>
<child link="base_camera_link"/>
<origin xyz="0.1 0 0.025"/>
</joint>
<!-- laser lidar -->
<link name="base_laser_link">
<visual>
<geometry>
<cylinder length="0.06" radius="0.04"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="laser_joint" type="fixed">
<parent link="base_link"/>
<child link="base_laser_link"/>
<origin xyz="0 0.0 0.06"/>
</joint>
</robot>
myrot.gazebo.xacro
<?xml version="1.0"?>
<robot name="mybot" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:arg name="laser_visual" default="false"/>
<xacro:arg name="camera_visual" default="false"/>
<xacro:arg name="imu_visual" default="false"/>
<gazebo reference="base_link">
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="left_wheel_link">
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<kp>500000.0</kp>
<kd>10.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
<fdir1>1 0 0</fdir1>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="right_wheel_link">
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<kp>500000.0</kp>
<kd>10.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
<fdir1>1 0 0</fdir1>
<material>Gazebo/FlatBlack</material>
</gazebo>
<gazebo reference="ball_wheel_link">
<mu1>0.1</mu1>
<mu2>0.1</mu2>
<kp>500000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
<material>Gazebo/FlatBlack</material>
</gazebo>
<gazebo reference="imu">
<sensor type="imu" name="imu">
<always_on>true</always_on>
<visualize>$(arg imu_visual)</visualize>
</sensor>
<material>Gazebo/FlatBlack</material>
</gazebo>
<gazebo>
<plugin name="mybot_controller" filename="libgazebo_ros_diff_drive.so">
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<odometrySource>world</odometrySource>
<publishOdomTF>true</publishOdomTF>
<robotBaseFrame>base_footprint</robotBaseFrame>
<publishWheelTF>false</publishWheelTF>
<publishTf>true</publishTf>
<publishWheelJointState>true</publishWheelJointState>
<legacyMode>false</legacyMode>
<updateRate>30</updateRate>
<leftJoint>left_wheel_joint</leftJoint>
<rightJoint>right_wheel_joint</rightJoint>
<wheelSeparation>0.180</wheelSeparation>
<wheelDiameter>0.05</wheelDiameter>
<wheelAcceleration>10</wheelAcceleration>
<wheelTorque>100</wheelTorque>
<rosDebugLevel>na</rosDebugLevel>
</plugin>
</gazebo>
<gazebo>
<plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
<alwaysOn>true</alwaysOn>
<bodyName>imu</bodyName>
<frameName>imu</frameName>
<topicName>imu</topicName>
<serviceName>imu_service</serviceName>
<gaussianNoise>0.0</gaussianNoise>
<updateRate>0</updateRate>
<imu>
<noise>
<type>gaussian</type>
<rate>
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</rate>
<accel>
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</accel>
</noise>
</imu>
</plugin>
</gazebo>
<gazebo reference="base_laser_link">
<material>Gazebo/FlatBlack</material>
<sensor type="ray" name="rplidar_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>$(arg laser_visual)</visualize>
<update_rate>7</update_rate>
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>0.5</resolution>
<min_angle>0.0</min_angle>
<max_angle>6.28319</max_angle>
</horizontal>
</scan>
<range>
<min>0.120</min>
<max>12.0</max>
<resolution>0.015</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_rplidar_controller" filename="libgazebo_ros_laser.so">
<topicName>scan</topicName>
<frameName>base_laser_link</frameName>
</plugin>
</sensor>
</gazebo>
<gazebo reference="base_camera_link">
<sensor type="camera" name="csi Camera">
<always_on>true</always_on>
<visualize>$(arg camera_visual)</visualize>
<camera>
<horizontal_fov>1.085595</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.03</near>
<far>100</far>
</clip>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>30.0</updateRate>
<cameraName>/</cameraName>
<frameName>base_camera_link</frameName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
</robot>
将小车模型文件 myrot.xacro 和 myrot.gazebo.xacro 放到 urdf/文件夹下
1.2.3ROS 运行环境
编写 .launch 文件，放到 ~/catkin/src/tutorials/launch 文件夹
gazebo_world.launch：
<launch>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find tutorials)/world/room.world"/> // 注意这里是你的地图文件名
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="headless" value="false"/>
<arg name="debug" value="false"/>
</include>
</launch>
simulation_robot.launch：
<launch>
<arg name="x_pos" default="0.0"/>
<arg name="y_pos" default="0.0"/>
<arg name="z_pos" default="0.0"/>
<param name="/use_sim_time" value="true" />
<include file="$(find tutorials)/launch/gazebo_world.launch"/>
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find tutorials)/urdf/mybot.xacro" />
<node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf" args="-urdf -model mybot.xacro -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
</launch>
CMakeLists.txt：
cmake_minimum_required(VERSION 2.8.3)
project(tutorials)
find_package(catkin REQUIRED COMPONENTS)
package.xml：
<?xml version="1.0"?>
<package format="2">
<name>tutorials</name>
<version>0.0.0</version>
<description>The tutorials package</description>
<maintainer email="nan@todo.todo">nanorobot</maintainer>
<license>TODO</license>
</package>
放在 ~/catkin_ws/src/tutorials 文件夹下 在 ~/catkin_ws 文件夹下 catkin 编译
catkin build
source 一下
source devel/setup.bash
运行 launch 文件
roslaunch tutorials simulation_robot.launch
二. 手机广角津贴的标定
2.1实验准备
安装usb_cam相机驱动
sudo apt-get install ros-noetic-usb-cam
安装标定功能包
sudo apt-get install ros-noetic-camera-calibration
usb_cam提供了一个launch文件，可以直接roslaunch运行，打开usb_cam_node 和 image_view节点。launch文件在/opt/ros/noetic/share/usb_cam/launch文件夹下。直接在这个目录打开bash然后运行。执行该命令
roslaunch usb_cam usb_cam-test.launchl
launch文件内容
<launch>
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
<param name="video_device" value="/dev/video0" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="pixel_format" value="yuyv" />
<param name="color_format" value="yuv422p" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
</node>
<node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
<remap from="image" to="/usb_cam/image_raw"/>
<param name="autosize" value="true" />
</node>
</launch>
2.2摄像头标定
准备标定板，打印一张棋盘格
开始标定，bash中输入：
rosrun camera_calibration cameracalibrator.py --size 11x8 --square 0.02 image:=/usb_cam/image_raw
运行之后会出现下面这个窗口，然后用你的标定板缓慢各式各样的移动，会发现右边那几个进度条在变化，你要做的就是让这些血条变绿。X表示左右移动，Y表示上下，Size表示远近，Skew表示倾斜
当所有进度条都变成绿色后，CALIBRATE按钮由灰色变成深绿色，点击CALIBRATE。等待直到出现以下结果
点击SAVE按钮后在/tmp目录下，出现了一个压缩包，这个压缩包存放了标定结果和图片。其中的.yaml就是标定结果。
2.3测试
打开你的ros工作目录，在src下创建一个功能包，在这个功能包下创建config和launch目录，config存放刚才的标定结果yaml文件，launch目录存放launch文件，来运行节点。
launch文件内容如下：
<launch>
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
<param name="video_device" value="/dev/video0" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="pixel_format" value="yuyv" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
<param name="camera_name" value="my_camera"/>
<param name="camera_info_url" type="string" value="file://$(find bingda_tutorials)/config/ost.yaml"/>
</node>
</launch>
重要的是后面两个参数，一个是摄像头名称，你自定义，一个是info_url，写你的yaml文件路径
接下来launch一下：
使用echo将camera_info话题输出，可以看到摄像头的标定参数已经加载在话题中了
rostopic echo /usb_cam/camera_info
三、编译代码进行实验。
3.1配置环境
Pangolin安装
直接从github下载下来。解压之后进行编译：注意安装0.5版本的，不然会出错 cd Pangolin mkdir build && cd build cmake .. sudo make -j8
Eigen、Sophus直接git下来即可
Ceres安装
先安装依赖项：sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3 libgflags-dev libgoogle-glog-dev libgtest-dev
git下来
编译：
cd ceres-solver
mkdir build && cd build
cmake ..
sudo make -j8
sudo make install
g2o安装
安装依赖：sudo apt-get install qt5-qmake qt5-default libqglviewer-dev-qt5 libsuitesparse-dev libcxsparse3 libcholmod3
git下来
编译：
cd g2o
mkdir build && cd build
cmake ..
sudo make -j8
sudo make install
DBoW3安装
git下来
mkdir build
cd build/
cmake ..
make
sudo make install
3.2环境配置
进入你的ROS工作空间的src目录下，下载ORB_SLAM2的安装包
配置环境：
在控制台输入：export ROS_PACKAGE_PATH=R O S P A C K A G E P A T H : 你的 O R B S L A M 2 的目录 / E x a m p l e s / R O S ，比如在我的虚拟机就是 e x p o r t R O S P A C K A G E P A T H = {ROS_PACKAGE_PATH}:你的ORB_SLAM2的目录/Examples/ROS，比如在我的虚拟机就是export ROS_PACKAGE_PATH=ROS
P ACKAGE
P ATH:你的ORB
S LAM2的目录/Examples/ROS，比如在我的虚拟机就是exportROS
P ACKAGE
P ATH={ROS_PACKAGE_PATH}:~/catkin_ws/src/ORB_SLAM2/Examples/ROS
进入ORB_SLAM2文件夹下
cd ~/catkin_ws/src/ORB_SLAM2
sudo chmod +x build.sh
./build.sh
接着使用ROS编译
sudo chmod +x build_ros.sh
./build_ros.sh
