文章目录
一、在ROS中完成一个机器小车的模型创建,传感器配置,仿真环境的设置,并控制机器小车进行运动。
1.在 Rviz 中显示一个机器小车
1.1 准备:新建功能包,导入依赖
创建一个新的功能包,名称自定义,导入依赖包:urdf与xacro
在当前功能包下,再新建几个目录:
urdf: 存储 urdf 文件的目录
meshes:机器人模型渲染文件(暂不使用)
config: 配置文件
launch: 存储 launch 启动文件
1.2 核心:编写 urdf 文件
box_urdf.urdf
<!-- <robot name="mycar">
<link name="base_link">
<visual>
<geometry>
<box size="0.5 0.2 0.1" />
</geometry>
</visual>
</link>
</robot> -->
<robot name="mycar">
<!-- 设置 base_footprint -->
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="0.001" />
</geometry>
</visual>
</link>
<link name="base_link">
<visual>
<geometry>
<cylinder radius="0.1" length="0.08" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="yellow">
<color rgba="0.8 0.3 0.1 0.5" />
</material>
</visual>
</link>
<joint name="base_link2base_footprint" type="fixed">
<parent link="base_footprint" />
<child link="base_link"/>
<origin xyz="0 0 0.055" />
</joint>
<link name="left_wheel">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.015" />
</geometry>
<origin xyz="0 0 0" rpy="1.5705 0 0" />
<material name="black">
<color rgba="0.0 0.0 0.0 1.0" />
</material>
</visual>
</link>
<joint name="left_wheel2base_link" type="continuous">
<parent link="base_link" />
<child link="left_wheel" />
<origin xyz="0 0.1 -0.0225" />
<axis xyz="0 1 0" />
</joint>
<link name="right_wheel">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.015" />
</geometry>
<origin xyz="0 0 0" rpy="1.5705 0 0" />
<material name="black">
<color rgba="0.0 0.0 0.0 1.0" />
</material>
</visual>
</link>
<joint name="right_wheel2base_link" type="continuous">
<parent link="base_link" />
<child link="right_wheel" />
<origin xyz="0 -0.1 -0.0225" />
<axis xyz="0 1 0" />
</joint>
<link name="front_wheel">
<visual>
<geometry>
<sphere radius="0.0075" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="black">
<color rgba="0.0 0.0 0.0 1.0" />
</material>
</visual>
</link>
<joint name="front_wheel2base_link" type="continuous">
<parent link="base_link" />
<child link="front_wheel" />
<origin xyz="0.0925 0 -0.0475" />
<axis xyz="1 1 1" />
</joint>
<link name="back_wheel">
<visual>
<geometry>
<sphere radius="0.0075" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="black">
<color rgba="0.0 0.0 0.0 1.0" />
</material>
</visual>
</link>
<joint name="back_wheel2base_link" type="continuous">
<parent link="base_link" />
<child link="back_wheel" />
<origin xyz="-0.0925 0 -0.0475" />
<axis xyz="1 1 1" />
</joint>
</robot>
1.3 核心:在 launch 文件集成 URDF 与 Rviz
box_launch.launch
<launch>
<!-- 将 urdf 文件内容设置进参数服务器 -->
<param name="robot_description" textfile="$(find jubot_demo)/urdf/box_urdf.urdf" />
<!-- 启动 rviz -->
<!-- <node pkg="rviz" type="rviz" name="rviz" /> -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find jubot_demo)/config/rviz/show_four_wheel_car.rviz" />
<!-- 启动机器人状态和关节状态发布节点 -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />
<!-- 启动图形化的控制关节运动节点 -->
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />
</launch>
1.4 在 Rviz 中显示机器小车模型
安装一个检查urdf语法的工具
sudo apt-get install liburdfdom-tools
在urdf文件夹下打开终端检查语法
check_urdf box_urdf.urdf
在urdf文件夹下打开终端,图形化显示URDF模型
urdf_to_graphiz box_urdf.urdf
退到主目录catkin_ws下,执行catkin_make
执行成功之后,再执行catkin_make install
在主目录catkin_make下配置环境变量source devel/setup.bash
检测环境变量是否配置成功echo $ROS_PACKAGE_PATH
启动launch文件
roslaunch jubot_demo box_launch.launch
2.添加摄像头雷达传感器
2.1URDF优化xacro
在urdf文件夹中新建一个xacro文件夹,并在xacro文件夹中新建my_base.urdf.xacro文件
<robot name="my_base" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="PI" value="3.141"/>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0" />
</material>
<xacro:property name="base_footprint_radius" value="0.001" /> <!-- base_footprint 半径 -->
<xacro:property name="base_link_radius" value="0.1" />
<xacro:property name="base_link_length" value="0.08" />
<xacro:property name="earth_space" value="0.015" />
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="${base_footprint_radius}" />
</geometry>
</visual>
</link>
<link name="base_link">
<visual>
<geometry>
<cylinder radius="${base_link_radius}" length="${base_link_length}" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="yellow">
<color rgba="0.5 0.3 0.0 0.5" />
</material>
</visual>
</link>
<joint name="base_link2base_footprint" type="fixed">
<parent link="base_footprint" />
<child link="base_link" />
<origin xyz="0 0 ${earth_space + base_link_length / 2 }" />
</joint>
<xacro:property name="wheel_radius" value="0.0325" />
<xacro:property name="wheel_length" value="0.015" />
<xacro:macro name="add_wheels" params="name flag">
<link name="${name}_wheel">
<visual>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_length}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
<material name="black" />
</visual>
</link>
<joint name="${name}_wheel2base_link" type="continuous">
<parent link="base_link" />
<child link="${name}_wheel" />
<origin xyz="0 ${flag * base_link_radius} ${-(earth_space + base_link_length / 2 - wheel_radius) }" />
<axis xyz="0 1 0" />
</joint>
</xacro:macro>
<xacro:add_wheels name="left" flag="1" />
<xacro:add_wheels name="right" flag="-1" />
<xacro:property name="support_wheel_radius" value="0.0075" />
<xacro:macro name="add_support_wheel" params="name flag" >
<link name="${name}_wheel">
<visual>
<geometry>
<sphere radius="${support_wheel_radius}" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="black" />
</visual>
</link>
<joint name="${name}_wheel2base_link" type="continuous">
<parent link="base_link" />
<child link="${name}_wheel" />
<origin xyz="${flag * (base_link_radius - support_wheel_radius)} 0 ${-(base_link_length / 2 + earth_space / 2)}" />
<axis xyz="1 1 1" />
</joint>
</xacro:macro>
<xacro:add_support_wheel name="front" flag="1" />
<xacro:add_support_wheel name="back" flag="-1" />
</robot>
2.2集成launch文件
在 launch 文件中直接加载 xacro
box_launch:
<launch>
<!-- 将 urdf 文件内容设置进参数服务器 -->
<!-- <param name="robot_description" textfile="$(find jubot_demo)/urdf/box_urdf.urdf" /> -->
<!-- 使用xacro优化urdf文件 -->
<param name="robot_description" command="$(find xacro)/xacro $(find jubot_demo)/urdf/xacro/my_base.urdf.xacro" />
<!-- 启动 rviz -->
<!-- <node pkg="rviz" type="rviz" name="rviz" /> -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find jubot_demo)/config/rviz/show_four_wheel_car.rviz" />
<!-- 启动机器人状态和关节状态发布节点 -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />
<!-- 启动图形化的控制关节运动节点 -->
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />
</launch>
加载robot_description时使用command属性,属性值就是调用 xacro 功能包的 xacro 程序直接解析 xacro 文件。
启动launch:
roslaunch jubot_demo box_launch.launch
2.3 添加摄像头和雷达传感器
在xacro文件夹中新建“my_camera.urdf.xacro”"“my_car_camera.urdf.xacro”“my_laser.urdf.xacro”文件
my_camera.urdf.xacro:
<!-- 摄像头相关的 xacro 文件 -->
<robot name="my_camera" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 摄像头属性 -->
<xacro:property name="camera_length" value="0.01" /> <!-- 摄像头长度(x) -->
<xacro:property name="camera_width" value="0.025" /> <!-- 摄像头宽度(y) -->
<xacro:property name="camera_height" value="0.025" /> <!-- 摄像头高度(z) -->
<xacro:property name="camera_x" value="0.08" /> <!-- 摄像头安装的x坐标 -->
<xacro:property name="camera_y" value="0.0" /> <!-- 摄像头安装的y坐标 -->
<xacro:property name="camera_z" value="${base_link_length / 2 + camera_height / 2}" /> <!-- 摄像头安装的z坐标:底盘高度 / 2 + 摄像头高度 / 2 -->
<!-- 摄像头关节以及link -->
<link name="camera">
<visual>
<geometry>
<box size="${camera_length} ${camera_width} ${camera_height}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<material name="black" />
</visual>
</link>
<joint name="camera2base_link" type="fixed">
<parent link="base_link" />
<child link="camera" />
<origin xyz="${camera_x} ${camera_y} ${camera_z}" />
</joint>
</robot>
my_laser.urdf.xacro:
<!--
小车底盘添加雷达
-->
<robot name="my_laser" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 雷达支架 -->
<xacro:property name="support_length" value="0.15" /> <!-- 支架长度 -->
<xacro:property name="support_radius" value="0.01" /> <!-- 支架半径 -->
<xacro:property name="support_x" value="0.0" /> <!-- 支架安装的x坐标 -->
<xacro:property name="support_y" value="0.0" /> <!-- 支架安装的y坐标 -->
<xacro:property name="support_z" value="${base_link_length / 2 + support_length / 2}" /> <!-- 支架安装的z坐标:底盘高度 / 2 + 支架高度 / 2 -->
<link name="support">
<visual>
<geometry>
<cylinder radius="${support_radius}" length="${support_length}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<material name="red">
<color rgba="0.8 0.2 0.0 0.8" />
</material>
</visual>
</link>
<joint name="support2base_link" type="fixed">
<parent link="base_link" />
<child link="support" />
<origin xyz="${support_x} ${support_y} ${support_z}" />
</joint>
<!-- 雷达属性 -->
<xacro:property name="laser_length" value="0.05" /> <!-- 雷达长度 -->
<xacro:property name="laser_radius" value="0.03" /> <!-- 雷达半径 -->
<xacro:property name="laser_x" value="0.0" /> <!-- 雷达安装的x坐标 -->
<xacro:property name="laser_y" value="0.0" /> <!-- 雷达安装的y坐标 -->
<xacro:property name="laser_z" value="${support_length / 2 + laser_length / 2}" /> <!-- 雷达安装的z坐标:支架高度 / 2 + 雷达高度 / 2 -->
<!-- 雷达关节以及link -->
<link name="laser">
<visual>
<geometry>
<cylinder radius="${laser_radius}" length="${laser_length}" />
</geometry>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<material name="black" />
</visual>
</link>
<joint name="laser2support" type="fixed">
<parent link="support" />
<child link="laser" />
<origin xyz="${laser_x} ${laser_y} ${laser_z}" />
</joint>
</robot>
my_car_camer.urdf.xacro:
<!-- 组合小车底盘与摄像头与雷达 -->
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:include filename="my_base.urdf.xacro" />
<xacro:include filename="my_camera.urdf.xacro" />
<xacro:include filename="my_laser.urdf.xacro" />
</robot>
2.3修改launch文件
<launch>
<!-- 将 urdf 文件内容设置进参数服务器 -->
<!-- <param name="robot_description" textfile="$(find jubot_demo)/urdf/box_urdf.urdf" /> -->
<!-- 使用xacro优化urdf文件 -->
<!-- <param name="robot_description" command="$(find xacro)/xacro $(find jubot_demo)/urdf/xacro/my_base.urdf.xacro" /> -->
<param name="robot_description" command="$(find xacro)/xacro $(find jubot_demo)/urdf/xacro/my_car_camera.urdf.xacro" />
<!-- 启动 rviz -->
<!-- <node pkg="rviz" type="rviz" name="rviz" /> -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find jubot_demo)/config/rviz/show_four_wheel_car.rviz" />
<!-- 启动机器人状态和关节状态发布节点 -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />
<!-- 启动图形化的控制关节运动节点 -->
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />
</launch>
2.4启动launch
roslaunch jubot_demo box_launch.launch
3.Rviz控制机器人模型运动
3.1安装 Arbotix
sudo apt-get install ros-melodic-arbotix
3.2 添加 arbotix 所需的配置文件
在config文件夹下新建hello.yaml文件
hello.yaml:
# 该文件是控制器配置,一个机器人模型可能有多个控制器,比如: 底盘、机械臂、夹持器(机械手)....
# 因此,根 name 是 controller
controllers: {
# 单控制器设置
base_controller: {
#类型: 差速控制器
type: diff_controller,
#参考坐标
base_frame_id: base_footprint,
#两个轮子之间的间距
base_width: 0.2,
#控制频率
ticks_meter: 2000,
#PID控制参数,使机器人车轮快速达到预期速度
Kp: 12,
Kd: 12,
Ki: 0,
Ko: 50,
#加速限制
accel_limit: 1.0
}
}
3.3在launch 文件中配置 arbotix 节点
在box_launch.launch中添加下列代码:
<!-- 配置 arbotix 节点 -->
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
<rosparam file="$(find jubot_demo)/config/hello.yaml" command="load" />
<param name="sim" value="true" />
</node>
3.4启动 launch 文件并控制机器人模型运动
启动launch文件
roslaunch jubot_demo box_launch.launch
二、 参考课件资料,在ROS中利用ROS工具完成你手机广角津贴(畸变较为严重)的标定,写出详细过程和输出结果。
1.ROS中使用摄像头
1.1 验证摄像头设备可用
使用命令ls /dev/检查当前设备列表
如果设备列表中有video*设备,则摄像头设备在当前系统中有被正确的识别。
1.2 安装cheese
sudo apt install cheese
1.3 在虚拟机设置中将usb兼容性改成3.1
2.获取摄像头驱动包
2.1 usb_cam
usb_cam可以通过apt方式直接安装二进制包,如果使用其他版本的ROS,自行替换命令中的noetic字段
sudo apt install ros-melodic-usb-cam
2.3 rosrun方式运行
先启动roscore,然后运行驱动节点usb_cam_node
roscore
rosrun usb_cam usb_cam_node
摄像头启动后启用rqt工具查看图像
rqt_image_view
2.4 roslaunch方式运行
usb_cam包中提供了一个launch文件例程usb_cam-test.launch
roslaunch usb_cam usb_cam-test.launch
在这个launch文件中除了启动usb_cam_node节点外还会包含一个image_view用于预览摄像头画面
三、在ROS下,下载编译orbslam2代码,并在kitti数据集的一个序列图像中进行实验。
1.ORB-SLAM2 安装和编译
1.1 下载ORB Slam2依赖
Orb Slam2
需要下载的依赖项:
Pangolin0.5 、Eigen3.1以上、 Sophus、Ceres、OpenCV、g2o、DBoW3
这些依赖项的github地址集成:https://github.com/Gongkaka/slambook2/tree/master/3rdparty
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
OpenCV安装
可参考以前的博客
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
2.安装编译ORB_SLAM2
进入你的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=
ROSPACKAGEPATH:你的ORBSLAM2的目录/Examples/ROS,比如在我的虚拟机就是exportROSPACKAGEPATH={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
3.运行Kitee数据集
3.1 数据集准备
3.2 启动!!!
执行命令:ORB-SLAM2支持单目、双目和RGBD数据,这里选择mono_kitti的单目数据集来运行和调试。
运行命令格式为:
./mono_kitti path_to_vocabulary path_to_settings path_to_sequence
该命令共有4个参数:
mono_kitti:为ORB-SLAM2编译生成的针对MONO_KITTI数据集的可执行文件,一般路径为ORB-SLAM2工程目录下Examples/Monocular/mono_kitti;
path_to_vocabulary为所选词典的路径,路径为ORB_SLAM2/Vocabulary/ORBvoc.txt;
path_to_settings为配置文件,存放在ORB-SLAM2/Examples/Monocular下,运行时选择了KITTI00-02.yaml(可选择别的),该配置文件中存放的相机的内参和外参,提取特征时候图像金字塔相关参数,可视化过程中用到的参数;
path_to_sequence为数据集路径,这个根据自己下载的kitti数据集路径来指定。这里在工作空间中建立文件夹dataset来存放数据集00,故目录为 …/dataset/sequences/00/ (…/表示上层目录,./表示当前目录)
我的执行,如下:在ORB_Slam2目录下,开启终端,输入:
./Examples/Monocular/mono_kitti Vocabulary/ORBvoc.txt Examples/Monocular/KITTI00-02.yaml …/dataset/sequences/00/