Revision history [back]

Publisher must be initialized. The following sample code to publish IMU message might be helpful:

void pubIMU(double tsIMU, double gyro_x, double gyro_y, double gyro_z, double acc_x, double acc_y, double acc_z)
{
       sensor_msgs::Imu imuMsg;

//  imuMsg.header.stamp = ros::Time::now();

    imuMsg.header.frame_id = "imu"; //

    imuMsg.angular_velocity.x = gyro_x;
    imuMsg.angular_velocity.y = gyro_y;
    imuMsg.angular_velocity.z = gyro_z;

    imuMsg.linear_acceleration.x = acc_x;
    imuMsg.linear_acceleration.y = acc_y;
    imuMsg.linear_acceleration.z = acc_z;

    pub_imu.publish(imuMsg);
}
void readCSV(const string &strPath2Dataset)
{
       // some function to extract info from a sample csv file:
       // ....
       // ....
       // ....

       pubIMU(timeStampIMU[i], gyro_X[i], gyro_Y[i], gyro_Z[i], acc_X[i], acc_Y[i], acc_Z[i]);
       // ^^^^^real function I'd like to publish^^^^^, ****modify based on your own need!!!
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "imuPub");
    ros::NodeHandle nh;

    // all your publisher must be defined in the very beginning:
    pub_imu     = nh.advertise      <sensor_msgs::Imu>                  ("/imu_raw"     ,1);
    pub_cam     = nh.advertise      <geometry_msgs::PoseStamped>  ("/cam_pose"  ,1);
    pub_path    = nh.advertise      <nav_msgs::Path>                    ("/cam_path"        ,1);

    while(ros::ok())
    {
        readCSV("test.csv");
        ros::spinOnce();
    }
    return 0;
}

Publisher must be initialized. The following sample code to publish IMU message might be helpful:

void pubIMU(double tsIMU, double gyro_x, double gyro_y, double gyro_z, double acc_x, double acc_y, double acc_z)
{
       sensor_msgs::Imu imuMsg;

//  imuMsg.header.stamp = ros::Time::now();

    imuMsg.header.frame_id = "imu"; //

    imuMsg.angular_velocity.x = gyro_x;
    imuMsg.angular_velocity.y = gyro_y;
    imuMsg.angular_velocity.z = gyro_z;

    imuMsg.linear_acceleration.x = acc_x;
    imuMsg.linear_acceleration.y = acc_y;
    imuMsg.linear_acceleration.z = acc_z;

    pub_imu.publish(imuMsg);
}
void readCSV(const string &strPath2Dataset)
{
       // some function to extract info from a sample csv file:
       // ....
       // ....
       // ....

       pubIMU(timeStampIMU[i], gyro_X[i], gyro_Y[i], gyro_Z[i], acc_X[i], acc_Y[i], acc_Z[i]);
       // ^^^^^real function I'd like to publish^^^^^, ****modify based on your own need!!!
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "imuPub");
    ros::NodeHandle nh;

    // all your publisher must be defined in the very beginning:
    pub_imu     = nh.advertise      <sensor_msgs::Imu>                  ("/imu_raw"     ,1);
    pub_cam     = nh.advertise      <geometry_msgs::PoseStamped>  ("/cam_pose"  ,1);
    pub_path    = nh.advertise      <nav_msgs::Path>                    ("/cam_path"        ,1);

    while(ros::ok())
    {
        readCSV("test.csv");
        ros::spinOnce();
    }
    return 0;
}

Publisher must be initialized. The following sample code to publish IMU message might be helpful:

void pubIMU(double tsIMU, double gyro_x, double gyro_y, double gyro_z, double acc_x, double acc_y, double acc_z)
{
       sensor_msgs::Imu imuMsg;

//  imuMsg.header.stamp = ros::Time::now();

    imuMsg.header.frame_id = "imu"; //

    imuMsg.angular_velocity.x = gyro_x;
    imuMsg.angular_velocity.y = gyro_y;
    imuMsg.angular_velocity.z = gyro_z;

    imuMsg.linear_acceleration.x = acc_x;
    imuMsg.linear_acceleration.y = acc_y;
    imuMsg.linear_acceleration.z = acc_z;

    pub_imu.publish(imuMsg);
}
void readCSV(const string &strPath2Dataset)
{
       // some function to extract info from a sample csv file:
       // ....
       // ....
       // ....

       pubIMU(timeStampIMU[i], gyro_X[i], gyro_Y[i], gyro_Z[i], acc_X[i], acc_Y[i], acc_Z[i]);
       // ^^^^^real function I'd like to publish^^^^^, ****modify based on your own need!!!
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "imuPub");
    ros::NodeHandle nh;

    // all your publisher must be defined in the very beginning:
    pub_imu     = nh.advertise      <sensor_msgs::Imu>                  ("/imu_raw"     ,1);
    pub_cam     = nh.advertise      <geometry_msgs::PoseStamped>   ("/cam_pose"   ,1);
    pub_path    = nh.advertise      <nav_msgs::Path>                    ("/cam_path"     ,1);

    while(ros::ok())
    {
        readCSV("test.csv");
        ros::spinOnce();
    }
    return 0;
}