No odometry/filtered output from robot_localization
Hello!
I'm trying to fuse IMU and a camera estimation but I receive no output through /odometry/filtered. If I try with IMU alone, it does send infromation through that topic, but when I try with the camera estimation,I have no response from the EKF node.
The camera sends a PoseWithCovarianceStamped message that tells the displacement between the previous frame and the current one in the UAV coordinate frame (called erlecopter/imu_link):
geometry_msgs::PoseWithCovarianceStamped pub_msg;
pub_msg.header.seq=seq;
pub_msg.header.stamp=msg->header.stamp;
pub_msg.header.frame_id="erlecopter/imu_link";
pub_msg.pose.pose.position.x=tras.at<double>(0);
pub_msg.pose.pose.position.y=tras.at<double>(1);
pub_msg.pose.pose.position.z=tras.at<double>(2);
pub_msg.pose.pose.orientation=Euler2Quat(angles);
pub_msg.pose.covariance=Covariance_default;
Pub_estimation.publish(pub_msg);
where, the publisher node has been defined like this:
ros::Publisher Pub_estimation;
Pub_estimation=nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("Camera_estimation",1);
This message is properly as I have seen thanks to rostopic echo.
My parameters file for the EKF node is like this (for the camera only fusion):
frequency: 1
sensor_timeout: 2
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
publish_tf: true
publish_acceleration: false
map_frame: map
odom_frame: odom
base_link_frame: erlecopter/imu_link
world_frame: odom
pose0: Camera_estimation
pose0_config: [true, true, true,
true, true, true,
false, false, false,
false, false, false,
false, false, false]
process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0 ...
The frame_id of the PoseWithCovarianceStamped message seems to be that of the IMU, is this intended?
Is "Camera_estimation" the name of the topic where you publish the pose?
Yes, Camera_estimation is the topic name, and I have modified its data so that instead of being with respect to the camera frame, it is with respect to the IMU one. That's why I use the same frame_id for both IMU and the camera estimation. I use those two commands:
Do I upload the full code?
If on that topic you're publishing the pose of the camera the
frame_idshould be a global frame like odom or something similarIt is not the pose of the camera but the displacement of the UAV between the previous measure and the current one (it is extracted from a camera but it is in the UAV frame of reference).
mmm...usually is better to express these quantities in global frames. Btw, check that the tf tree is ok, i.e. that there is a connection between the odom frame and the imu_link. You can use rqt_tf_tree.
rqt_tf_tree says no data received. I have tried with rqt_graph and this is what I get: https://drive.google.com/file/d/1WRko... ( I cannot upload photos :( )