gazebo仿真IMU和双目视觉

环境：ubuntu16.04 ros kinetic

1、安装ROS-Academy-for-Beginners
这个我也是按照网上教程来的，我贴一下网址：
https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/chapter1/1.5.html

2、配置双目相机
部分参考：https://blog.csdn.net/csdnzsx/article/details/104236366
这个博客有些部分说的不够清晰，我再详细解读一下：

2.1 编译urdf文件：
在 ROS-Academy-for-Beginners/robot_sim_demo/urdf 文件夹中，编辑xbot-u.gazebo文件
把上面参考博客中的双目相机sdf文件粘贴进去，加入到 一段之后

2.2 编译节点订阅文件：
回到主目录home

mkdir -p ~/opcv_ros/src
cd ~/opcv_ros/
catkin_make
gedit ~/.basrc

把 source ~/opcv_ros/devel/setup.bashrc加入最后
保存退出

source devel/setup.bash

创建包

catkin_create_pkg opcv_src std_msgs roscpp

可以用下面的命令查看功能包的依赖项：
rospack depends opcv_ros

在opcv_ros中添加订阅图像的节点

cd opcv_ros/src/opcv_ros/src
touch subscribImage.cpp

cpp文件里的内容参考上面的链接中的第四部分：编写程序读取双目相机的左右图

编写CMakeLists.txt文件：
打开home/opcv_ros/src/opcv_ros/CMakeLists.txt
首先在文件最后位置添加：

include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(subscribImage src/subscribImage.cpp)
target_link_libraries(subscribImage ${catkin_LIBRARIES})
target_link_libraries( subscribImage ${OpenCV_LIBS} )

然后，在find_package(catkin REQUIRED COMPONENTS)中添加cv_bridge image_transport

接着在括号外添加：

find_package(OPENCV REQUIRED)

保存，退出

编译：

cd ~/opcv_ros/
catkin_make

2.3 运行，接收图像

3、IMU
先用rostopic list看了一下现在发布的Topic：

可以看到，现在topic有IMU的和双目图像的
那我们只需要订阅IMU的消息就可以了，我继续写在了subscribImage的同一个文件夹，命名为subscribIMU.cpp。
我直接把代码传一下：

#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 

using namespace std;
using namespace sensor_msgs;
using namespace message_filters;
using namespace geometry_msgs;

queue<sensor_msgs::Imu> imu_buf;

string imu_topic = "/imu";

void imu_callback(const sensor_msgs::Imu &imu_msg)
{
 
    imu_buf.push(imu_msg);
    cout<<"imu:  角速度"<<imu_msg.angular_velocity<<" ";
    cout<<"加速度"<<imu_msg.linear_acceleration<<endl;
    
}


int main(int argc, char **argv)
{
    
  ros::init(argc, argv, "subscribIMU");
  ros::NodeHandle n;
 
  //创建消息订阅者，订阅IMU、feature、restart、match_points的topic，执行各自回调函数
  ros::Subscriber sub_imu = n.subscribe(imu_topic, 2000, imu_callback);
                                        
  //这一部分接收的是feature_tracker_node发布的在cur帧的所有特征点的信息
  //ros::Subscriber sub_image = n.subscribe(img_topic, 2000, feature_callback);   
  
  ros::spin();
  
  return 0;
}

cmake_minimum_required(VERSION 3.0.2)
project(opcv_ros)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  cv_bridge
  image_transport
  tf
)

find_package(OpenCV REQUIRED)
find_package(Eigen3)



include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)



include_directories(include ${catkin_INCLUDE_DIRS})


#IMAGE
add_executable(subscribImage src/subscribImage.cpp)
target_link_libraries(subscribImage ${catkin_LIBRARIES})
target_link_libraries( subscribImage ${OpenCV_LIBS} )

#IMU
add_executable(subscribIMU src/subscribIMU.cpp)
target_link_libraries(subscribIMU ${catkin_LIBRARIES})
target_link_libraries( subscribIMU ${OpenCV_LIBS} )

运行就还是和Image的一样：

rosrun opcv_ros subscribIMU