gazebo小车使用stereo相机运行ORB_SLAM2
修改urdf文件
在工作空间中找到urdf模型,我的是
urdf/mybot.gazebo.xacro
将stereo相机添加进去(替换原来的相机)
ORB_SLAM2下载安装方法见我之前的博客
<!-- stereo camera -->
<gazebo reference="base_camera_link">
<sensor type="multicamera" name="stereocamera">
<always_on>true</always_on>
<update_rate>10</update_rate>
<visualize>false</visualize>
<camera name="left">
<pose>0 0 0 0 0 0</pose>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
<camera name="right">
<pose>0 -0.07 0 0 0 0</pose>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
<plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
<cameraName>stereocamera</cameraName>
<alwaysOn>true</alwaysOn>
<updateRate>10</updateRate>
<cameraName>stereocamera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link_optical</frameName>
<baseline>0.07</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>
</plugin>
</sensor>
</gazebo>
编译工作空间、修改topic
将下载好的ORB_slam2放到工作空间里,进行编译
cd ORB_SLAM2
cmake ..
./build_ros.sh
cd ..
cd ..
(回到工作空间路径下)
catkin_make
查看之前修改的机器人urdf模型文件得到机器人的stereo左右相机topic,修改ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/ros_stereo.cc文件下订阅的摄像头信息
cd ORB_SLAM2
./build_ros.sh
编译功能包
运行demo
运行gazebo
运行slam2-stereo相机
rosrun ORB_SLAM2 Stereo src/ORB_SLAM2/Vocabulary/ORBvoc.txt src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Stereo_setting.yaml false
运行键盘控制控制机器人运动
rosrun robotcar teleop_robot_key
通过键盘控制小车运动,可以看到有两个窗口。CURRENT Frame和Map viewer。
运行结果
在CURRENT Frame窗口中看到摄像机实时检测到的画面和特征点,在Map viewer窗口中摄像机(即机器人)运行的轨迹和特征点的点云(3维)信息。
现在通过键盘控制机器人遍历地图。
在运行过程中特征点检测停止并且Map viwer停止更新。但是图像传输没有断。
可以看到current frame中显示track lost 轨迹丢失,因为是仿真环境,特征点较少,经历了一段没有特征点的情况会导致机器人无法获知轨迹的状态。如果是在现实环境中做ORB_SLAM2就可以有效避免这种情况。