Ask Your Question
0

No odometry/filtered output from robot_localization

asked 2020-05-08 04:51:18 -0500

Aquas gravatar image

updated 2020-05-10 05:39:55 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

The frame_id of the PoseWithCovarianceStamped message seems to be that of the IMU, is this intended?

schizzz8 gravatar image schizzz8  ( 2020-05-08 04:55:32 -0500 )edit

Is "Camera_estimation" the name of the topic where you publish the pose?

schizzz8 gravatar image schizzz8  ( 2020-05-08 05:00:24 -0500 )edit

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:

ros::Publisher Pub_estimation;

Pub_estimation=nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("Camera_estimation",1);

Do I upload the full code?

Aquas gravatar image Aquas  ( 2020-05-08 08:33:16 -0500 )edit

If on that topic you're publishing the pose of the camera the frame_id should be a global frame like odom or something similar

schizzz8 gravatar image schizzz8  ( 2020-05-08 09:49:54 -0500 )edit

It 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).

Aquas gravatar image Aquas  ( 2020-05-09 11:36:36 -0500 )edit

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.

schizzz8 gravatar image schizzz8  ( 2020-05-11 03:12:57 -0500 )edit

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 :( )

Aquas gravatar image Aquas  ( 2020-05-11 05:04:45 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2020-05-11 05:52:04 -0500

schizzz8 gravatar image

updated 2020-05-14 01:34:30 -0500

It is very likely that you don't have a valid TF tree, i.e. the imu_link is not connected to the odom frame.

To double-check run:

rosrun tf tf_echo odom erlecopter/imu_link

EDIT

What you are missing is the part that publishes a TF between the odom and the camera frame.

If, as you say:

The camera sends a PoseWithCovarianceStamped message that tells the displacement between the previous frame and the current one

Then, what you have to do is to integrate over time the displacements to obtain the global transformation between the camera and a fixed reference frame.

edit flag offensive delete link more

Comments

Failure at 1589196125.549205710 Exception thrown:"odom" passed to lookupTransform argument target_frame does not exist. The current list of frames is:

And it continues like this until I do Ctl+C, without outputting any frame

Aquas gravatar image Aquas  ( 2020-05-11 06:23:13 -0500 )edit

Fix the TF tree and you will be done!

schizzz8 gravatar image schizzz8  ( 2020-05-11 06:48:08 -0500 )edit

I'll have to look further into how to create that TF tree, because I have no idea

Aquas gravatar image Aquas  ( 2020-05-13 08:32:46 -0500 )edit

Load a URDF and use the robot_state_publisher to publish the static transforms. Or just use a simple launch file and add TF2 static_transform_publisher between base_link and imu_link

hashirzahir gravatar image hashirzahir  ( 2020-05-14 04:20:39 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2020-05-08 04:51:18 -0500

Seen: 263 times

Last updated: May 14 '20