Robot localization fusion

edit

Hello!!

I have to fuse two sources: IMU data (accelerometer and gyroscope) and another source that provides the difference between the previous position and the current one by giving a traslation vector and a quaternion.

IMU data is in baselink frame and the other source is in the reference frame (I selected odom). I thought of using a PoseWithCovarianceStamped for the second source, but I'm not sure if robot localization works with variations in pose, so the questions are:

Is there any way I can fuse them with robot_localization? As the second source does not give a complete pose but a change in pose do I have to modify this data? Should I add all the previous estimations plus the current one to get a the "global" pose (instead of the variation) and send that one to EKF along with the IMU?

Thank you.

EDIT:

I was trying to fuse just my estimation to see how it performed, this is the yaml code:

frequency: 5
sensor_timeout: 2

two_d_mode: false

transform_time_offset: 0.0
transform_timeout: 0.0

print_diagnostics: false
debug: true
debug_out_file: /home/javier/pruebas/MasPruebas/Data/file.txt
publish_tf: true
publish_acceleration: false

map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: erlecopter/imu_link  # Defaults to "base_link" if unspecified
world_frame: odom           # Defaults to the value of odom_frame if unspecified

odom0: /Camera_estimation
odom0_config:  [false,  false,  false,
                false,  false,  false,
                true,  true,   true,
                true,  true,  true,
                false,  false,  false]
odom0_differential: false;
odom0_relative: true

odom0_queue_size: 2


#imu0: /erlecopter/imu
#imu0_config:  [false, false, false,
#               false, false, false,
#               false, false, false,
#               true,  true,  true,
#               true, true, true]
#imu0_relative: true
#imu0_queue_size: 100


process_noise_covariance:  [diag(small values)]


initial_estimate_covariance: [diag(1e-6)]

And the main code is something like this:

void ImageCallback()
{
   /*Estimate displacements*/

      double interval=(stamp-Prev_time).toSec();

      nav_msgs::Odometry pub_msg;
      pub_msg.header.seq=seq;
      pub_msg.header.stamp=stamp;
      pub_msg.header.frame_id="odom";

      pub_msg.child_frame_id="odom";

      pub_msg.twist.twist.linear.x=tras.at<double>(0)/interval;
      pub_msg.twist.twist.linear.y=tras.at<double>(1)/interval;
      pub_msg.twist.twist.linear.z=tras.at<double>(2)/interval;

      pub_msg.twist.twist.angular.x=(angles.roll-Prev_angles.roll)/interval;
      pub_msg.twist.twist.angular.y=(angles.pitch-Prev_angles.pitch)/interval;
      pub_msg.twist.twist.angular.z=(angles.yaw-Prev_angles.yaw)/interval;

      pub_msg.twist.covariance=Covariance_default;

      Pub_estimation.publish(pub_msg);

      Prev_time=stamp;

}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "image2angles");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub_img = it.subscribe("/erlecopter/bottom/image_raw", 1, imageCallback);
  ros::Subscriber sub_h = nh.subscribe("/erlecopter/imu", 1, imuCallback);
  ros::Subscriber sub_h2 = nh.subscribe("/odometry/filtered", 1, ScaleCallback);
  Pub_estimation=nh.advertise<nav_msgs::Odometry>("Camera_estimation",1);
  ros::spin();
}

I've checked the Camera_estimation topic and the messages are being published, but odometry/filtered isn't publishing anything.