對bag中歸一化的加速度乘以g

一、MultiplyGravity.cpp

#include <opencv2/core/mat.hpp>
#include <opencv2/highgui.hpp>
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Imu.h>
#include<sensor_msgs/Image.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#include "cv_bridge/cv_bridge.h"
#define foreach BOOST_FOREACH

using namespace std;
void bagPrepare(string strData,bool bVerbose){
    //自定義bag參數
    string IMAGE0_TOPIC = "/camera_0/image";
    string IMAGE1_TOPIC = "/camera_1/image";
    string IMU_TOPIC = "/BMI088";
    bool STEREO = true;
    string strBag = strData;
    string strDataFolder = strBag.substr(0,strBag.find_last_of("/") + 1);
    string strOutName = strBag.substr(strBag.find_last_of("/") + 1);
    strOutName = strOutName.substr(0,strOutName.find_last_of("."));
    string strOutBag = strDataFolder + strOutName + "-fix.bag";
    rosbag::Bag bag;
    //read rosbag
    bag.open(strBag, rosbag::bagmode::Read);
    rosbag::Bag bag_out;
    //write rosbag
    bag_out.open(strOutBag, rosbag::bagmode::Write);
    std::vector<std::string> topics;
    topics.push_back(IMAGE0_TOPIC);

    if(STEREO)
    {
        topics.push_back(IMAGE1_TOPIC);
    }
    topics.push_back(IMU_TOPIC);

    rosbag::View view(bag, rosbag::TopicQuery(topics));
    double bagStartTimeSec = view.getBeginTime().toSec();
    double bagEndTimeSec = view.getEndTime().toSec();

    int nLidarIndex = 0;
    foreach(rosbag::MessageInstance m, view)
    //for(auto m:view)
    {
        if (m.getTime().toSec() < bagStartTimeSec)
            continue;
        if (m.getTime().toSec() > bagEndTimeSec)
            break;
        if(m.getTopic() == IMAGE0_TOPIC) {
            sensor_msgs::Image::ConstPtr img = m.instantiate<sensor_msgs::Image>();
            uint64_t tm = img->header.stamp.toNSec();
            cv_bridge::CvImageConstPtr ptr;
            ptr = cv_bridge::toCvCopy(img);
            // output to bag
            bag_out.write(IMAGE0_TOPIC,img->header.stamp,img);
            if (bVerbose && ptr) {
                cv::Mat show_img = ptr->image;
                cv::imshow("Image-l", show_img);
                cv::waitKey(5);
            }
        }

        else if(STEREO && m.getTopic() == IMAGE1_TOPIC) {
            sensor_msgs::Image::ConstPtr img = m.instantiate<sensor_msgs::Image>();
            uint64_t tm = img->header.stamp.toNSec();
            cv_bridge::CvImageConstPtr ptr;
            ptr = cv_bridge::toCvCopy(img);
            // output to bag
            bag_out.write(IMAGE1_TOPIC,img->header.stamp,img);
            if (bVerbose && ptr) {
                cv::Mat show_img = ptr->image;
                cv::imshow("Image-r", show_img);
                cv::waitKey(5);
            }
        }

        else if(m.getTopic() == IMU_TOPIC) {
            sensor_msgs::Imu::ConstPtr imu = m.instantiate<sensor_msgs::Imu>();
            sensor_msgs::Imu imu_msg;
            imu_msg.header.stamp = imu->header.stamp;
            imu_msg.angular_velocity.x = imu->angular_velocity.x * M_PI / 180.f;
            imu_msg.angular_velocity.y = imu->angular_velocity.y * M_PI / 180.f;
            imu_msg.angular_velocity.z = imu->angular_velocity.z * M_PI / 180.f;
            imu_msg.angular_velocity_covariance = imu->angular_velocity_covariance;
            imu_msg.linear_acceleration.x = imu->linear_acceleration.x*9.8;
            imu_msg.linear_acceleration.y = imu->linear_acceleration.y*9.8;
            imu_msg.linear_acceleration.z = imu->linear_acceleration.z*9.8;
            imu_msg.linear_acceleration_covariance = imu->linear_acceleration_covariance;
            imu_msg.orientation = imu->orientation;
            imu_msg.orientation_covariance = imu->orientation_covariance;
            // output to bag
            bag_out.write(IMU_TOPIC,imu_msg.header.stamp,imu_msg);
        }
    }
}



int main(int argc,char **argv){
    ros::init(argc, argv, "MultiplyGravity");
    ros::NodeHandle n;
    string str = "/media/gavyn/Gavyn/zhihui/5-9/calibr/0513_calib/2/calb_22.bag";
    bagPrepare(str,true);
    std::cout << "已完成IMU加速度乘以9.8" << endl;
    //ros::Subscriber sub = n.subscribe("IMAGE0_TOPIC",1,bagPrepare);
    //ros::spin();
    return 0;
}

cmakelists.txt

cmake_minimum_required(VERSION 2.8.3)
project(MultiplyGravity)

## 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
  rosbag
  roscpp
  rospy
  std_msgs
        sensor_msgs
        cv_bridge
)
find_package(OpenCV REQUIRED)
include_directories(${catkin_INCLUDE_DIRS}
                    ${OpenCV_INCLUDE_DIRS})

###################################
## catkin specific configuration ##
###################################
catkin_package(
  INCLUDE_DIRS include
  LIBRARIES
  CATKIN_DEPENDS rosbag roscpp rospy std_msgs
  DEPENDS system_lib
)

###########
## Build ##
###########

add_executable(MultiplyGravity src/MultiplyGravity.cpp)

target_link_libraries(MultiplyGravity ${OpenCV_LIBS} ${catkin_LIBRARIES})