文章目录
一ROS中完成一个机器小车的模型创建,传感器配置,仿真环境的设置,并控制机器小车进行运动
1创建工作区
mkdir -p ~/catkin_ws/src/tutorials // 创建文件夹
cd ~/catkin_ws/src/tutorials
mkdir launch // 存放 launch 文件
mkdir urdf // 存放小车模型文件
mkdir world // 存放地图文件
2gazebo绘制地图
1打开gazebo
sudo gazebo // 打开gazebo,后续保存文件可能需要管理员权限所以用sudo
2进入编辑界面
3点击 wall 后可绘制墙壁
4点击 Add Texture 可修改墙壁纹理
5点击 file 的 save as保存模型文件
6地图绘制好后,保存为 .world地图文件
7将 .world 文件复制到 ~/catkin_ws/src/world 文件夹内
3小车模型
小车模型通常有两种,一种是 urdf,一种是 sacro.
1.urdf 小车模型文件1:myrot.urdf,具体内容如下:
<?xml version="1.0"?>
<robot name="mybot">
<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>
</robot>
上述小车模型文件没有传感器配置。
2.xacro小车模型文件分为两部分,分别为 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>
3将小车模型文件 myrot.xacro 和 myrot.gazebo.xacro放到 ~/catkin_ws/src/tutorials/urdf/文件夹下
4ROS 运行环境和小车模型
1.编写 .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>
2.编写 CMakeLists.txt 和 package.xml 文件
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>
3.编写好后,放在 ~/catkin_ws/src/tutorials 文件夹下
4在 ~/catkin_ws 文件夹下 catkin 编译
catkin build
5source一下否则会报错
source devel/setup.bash
6运行 launch 文件
roslaunch tutorials simulation_robot.launch
7效果
5查看传感器数据
1可以在终端通过 rostopic list 查看 ros 节点数据
二ROS工具完成手机广角津贴的标定
1标定的目的
相机标定的目的是为了确定相机内部参数和外部参数,以便修正图像并将图像坐标映射到世界坐标,或者从世界坐标映射到图像坐标。这个过程是计算机视觉和三维感知任务的关键步骤之一,它有以下主要目的:
1减小畸变:相机镜头和传感器可能引入径向和切向畸变,使得图像中的直线变得弯曲或者物体的形状不准确。通过标定相机,可以矫正这些畸变,使图像更准确。
2确定内/外部参数:内部参数包括焦距、主点坐标和相机畸变参数。这些参数是描述相机如何捕捉世界的重要因素,它们的准确值对于计算深度、距离和姿态等任务至关重要,相机内参标定就是为了获取精确的内部参数。外部参数包括相机的位置和方向。这些参数描述了相机相对于世界坐标系的位置和朝向,允许将图像坐标映射到世界坐标,这对于机械臂的抓取很有帮助。
3相机姿态估计:相机标定还可用于估计相机在拍摄图像时的姿态,即相机的旋转和平移。这对于虚拟现实、增强现实、机器人导航和定位等应用至关重要。
2步骤
1下载ros-noetic-camera-calibration
sudo apt install ros-noetic-camera-calibration -y
2修改参数为需要校准的相机的参数
3运行校准程序
rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.025 image:=/right_arm_camera/color/image_raw camera:=/right_arm_camera
当X、Y、Size、Skew结果都为绿色,非常良好时,点击CALIBRATE进行标定计算
3查看的标定结果
1查看解压的标定结果
三下载编译orbslam2代码
1安装三方库
1安装必要的依赖项
sudo apt-get update
sudo apt-get install git gcc g++ vim make cmake
2安装Pangolin
安装依赖项
sudo apt-get install libglew-dev libboost-dev libboost-thread-dev libboost-filesystem-dev
sudo apt-get install ffmpeg libavcodec-dev libavutil-dev libavformat-dev libswscale-dev libpng-dev
配置并编译
cd Pangolin
mkdir build && cd build
cmake -DCPP11_NO_BOOST=1 ..
make
sudo make install
验证
cd ../examples/HelloPangolin
mkdir build && cd build
cmake ..
make
./HelloPangolin
2安装OpenCV3
1安装依赖项
(注意opencv3与orbslam2相对应)
sudo add-apt-repository "deb http://security.ubuntu.com/ubuntu xenial-security main"
sudo apt update
sudo apt-get install build-essential libgtk2.0-dev libavcodec-dev
sudo apt-get install libavformat-dev libjpeg.dev libtiff5.dev
sudo apt-get install libswscale-dev libjasper-dev
sudo apt-get install libcanberra-gtk-module libcanberra-gtk3-module
#或者
sudo apt-get install libcanberra-gtk*
2配置并编译
mkdir build && cd build
cmake -D CMAKE_BUILD_TYPE=Release -D CMAKE_INSTALL_PREFIX=/usr/local ..
#电脑性能差可去掉-j4,性能很好的可增加数字(线程)
make -j4
sudo make install
3查询OpenCV版本、库以及头文件目录的三个命令来确保上面的OpenCV安装步骤都正常
pkg-config --modversion opencv
pkg-config --cflags opencv
pkg-config --libs opencv
3安装Eigen3
sudo apt-get install libeigen3-dev
源码安装
cd eigen3
mkdir build && cd build
cmake ..
make
sudo make install
安装后头文件在
/usr/local/include/eigen3/
复制头文件到/usr/local/include
sudo cp -r /usr/local/include/eigen3/Eigen /usr/local/include
4安装ORB-SLAM2
1安装编译ORB-SLAM2
git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2
cd ORB_SLAM2
//赋予shell文件运行权限
chmod +x build.sh
./build.sh
chmod +x 赋予shell文件运行权限,注意系统若硬件不行(线程少),将build.sh里的make -j修改为make
5在kitti数据集的一个序列图像中进行实验
1 数据集准备
https://www.cvlibs.net/datasets/kitti/eval_odometry.php
2启动
执行命令:ORB-SLAM2支持单目、双目和RGBD数据,这里选择mono_kitti的单目数据集来运行和调试。
运行命令格式为:
./mono_kitti path_to_vocabulary path_to_settings path_to_sequence
3效果
gen3
sudo apt-get install libeigen3-dev
源码安装
cd eigen3
mkdir build && cd build
cmake ..
make
sudo make install
安装后头文件在
/usr/local/include/eigen3/
复制头文件到/usr/local/include
sudo cp -r /usr/local/include/eigen3/Eigen /usr/local/include
4安装ORB-SLAM2
1安装编译ORB-SLAM2
git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2
cd ORB_SLAM2
//赋予shell文件运行权限
chmod +x build.sh
./build.sh
chmod +x 赋予shell文件运行权限,注意系统若硬件不行(线程少),将build.sh里的make -j修改为make
5在kitti数据集的一个序列图像中进行实验
1 数据集准备
https://www.cvlibs.net/datasets/kitti/eval_odometry.php
2启动
执行命令:ORB-SLAM2支持单目、双目以及RGBD数据,这里选择mono_kitti的单目数据集来运行和调试。
运行命令格式为:
./mono_kitti path_to_vocabulary path_to_settings path_to_sequence
3效果