0
$\begingroup$

Rosanswers logo

I am trying to use the robot_localization package to fuse absolute attitude from an IMU, velocity in the world_frame from the IMU with a SONAR and pressure sensor (all aboard an AUV).

Following your tutorials I set up the launch file, but when I publish sensor data nothing gets fused at all. In fact I haven't seen a single message on topic /odometry/filtered.

I read all tutorials and everything on answers.ros.org but can't get my head around it. Any ideas? Thanks a lot, Rapha

The sensors frame_id is set to "odom". My launchfile looks like this:



    

    

        

        

      

      
      
      
      


            
      
            
       

      [false, false, false, 
                                      false, false, false, 
                                      true,  true, true, 
                                      false, false, false,
                                      false, false, false]

      [false,  false,  true, 
                                      false, false, false, 
                                      false, false, false, 
                                      false, false, false,
                                      false, false, false]
                                      
      [true, true, false, 
                                     false,  false,  false, 
                                     false, false, false, 
                                     false,  false,  false,
                                     false, false, false]

      [false, false, false, 
                                     true,  true,  true, 
                                     false, false, false, 
                                     false,  false,  false,
                                     true, true, true]                                     

  
       

      

      

      
      

      
      

      [0.03, 0.0, 0.0, 0.0, 0.0, 0.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, 0.0, 0.0,
                                                  0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.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,
                                                  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, 0.0, 0.0, 0.0, 0.00, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0,
                                                  0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004]

    
     
    

The data for the IMU velocity (in ENU frame-->odom) coming in looks like this:


 header: 
  seq: 778403
  stamp: 
    secs: 1415909887
    nsecs: 951959536
  frame_id: odom
twist: 
  twist: 
    linear: 
      x: -4.52138569919
      y: 3.20782521003
      z: 0.199781238464
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0007748282199090048, 0.0, 0.0, 0.0, 0.0, 0.0, 0.000604634932647141, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0015846222091318446, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]


EDIT: Hey Tom,

thanks for the quick answer.

I have checked the rostopics and both sensor topics are being connected correctly to ekf_localization. It turned out to be the missing transform:

Changing the frame_id of the IMU to base_link did start output, but I am not sure everything is in the correct frame now.

The attitude solution from the IMU (which runs its own eKF) is in reference to ENU. More specifically it states that it:

"...is the orientation of the sensor fixed frame with respect to cartesian earth-fixed system"

Q(1): Which frame for the IMUs ENU attitude solution? My understanding is that these values are therefore in odom. Following what you said in Issue 22, I added a static transform (imu-->base_link). And now changed the IMUs frame_id to imu. The odom-->base_link is now being published and the eKF runs.

I turned on the differential integration of the IMU. However, then the IMUs orientation solution doesn't match the node's solution. When I turn the differential integration off, they match (apart from having the opposite sign at some orientations. However, I believe Q=-Q, so that should be fine.)

The second data source (ENU referenced linear velocities from the same IMU) publishes its data in the imu frame. I know that I could fuse in the accelerometer data directly. However, my integration algorithm also handles calibration, deadbands, static offset and a very basic zero velocity detection. Gravity is already removed by the IMU. I will have to compare both approaches.

Q(2): which publish_rate for imu->base_link? Am I right to assume that I will need the transforms to be published at at least the same frequency of the sensor data?


EDIT regarding Tom's clarification and suggestion:

Thanks for the elaborate answer.

I might have not been quite clear on the IMU frame. The frame is just a static transform from base_link to account for different mounting positions and headings of the sensor. So this is not ENU.

I have double checked the frame that the velocity is expressed in. (I checked this on the accelerometer output as it was easier and I do not transform anything within the velocity publishing node) It is actually the body frame:

  • if heading east and moving east, too, there is only a change in x
  • if heading north and moving east, there is a change in y only
  • if heading is in between i.e. NE, there is a change in x and y.
  • rolling the sensor +90deg and moving up will show a change in y

Thank you very much for the clarifying questions. I will leave the velocity messages in my imu frame and for clarification change the transform to 0 offset and 0 rotation.

For the imu orientation data (imu/data) I am not sure I quite understand. So I'll give you an overview of my understanding. The orientation data will always show the offset from ENU. So irrelevant of the starting position it will always show yaw==0deg if orienting the x axis to point east (or +90deg if x axis is pointing north.)

I think I now understand the effect of the differential integration. Since it differentiates the position into velocity it does not matter what heading the initial message states. When starting the robot_localization_node, it will start with a heading and position of 0.

The point I struggle with is the correct frame and that

ekf_localization_node always tries to transform IMU data - both angular velocity and orientation - into base_link.

Q(3) So even if I were to change the frame_id of my imu/data to odom and only use the orientation data in that messages robot_localization_node still wouldn't be interpreted in odom? Or would it try to transform the orientation to base_link and we'd be at the chicken and egg problem again?

Later today I'll try these settings and will update here again.

Again thank you so much for your time and effort.


Originally posted by Raphael Nagel on ROS Answers with karma: 15 on 2014-11-13

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

A few things:

  1. No output at all only occurs when it's not receiving sensor data or it can't transform the data into the target frame. Do rostopic info on all your topics to make sure that they are being published and subscribed to.
  2. Velocities should ideally be reported in the frame of the vehicle (base_link), but if they're not, the package will transform them into base_link anyway before fusing them with the state estimate. In this case, I'm guessing that it's attempting to transform your velocity message from odom->base_link, but can't, as the node that produces that transform is ekf_localization_node itself. It's a chicken-or-egg problem.
  3. However, the odom->base_link transform should be getting generated if any one of the sensors is being successfully fused, so check (1) again, and perhaps post examples of your other sensor messages?

EDIT: Also, turn debug mode off unless you want to eat up disk space in a hurry. If you want to run your node for a few seconds with all of the sensor data coming in and then send me that log, that will help, but in general, don't run with that on.

EDIT in response to update. Forgive me if I go over anything you already know. I just want to make sure we're on the same page.

Q1: Let me back up a bit. First, in your system, we actually have three separate coordinate frames (at least with regard to this question, you probably have more):

  1. odom
  2. base_link
  3. imu (which is equivalent to the ENU frame)

Just so we're clear, both your IMU's velocity message frame_id (as reported in xsens/velocity) and its orientation frame_id (as reported in imu/data) are now reported in the imu (i.e., ENU) frame, correct?

Assuming this is true, let's consider the velocity first. If your robot heads directly east, your IMU velocity sensor reads only +X velocity, and if it drives straight north, you get only +Y, correct?

(Incidentally, this would surprise me. If you're using accelerations, the velocities you generate, unless you transform them, are going to be in the body frame of the IMU, but not in the ENU frame. However, the rest of my answer assumes you did mean that the velocities are reported in the ENU frame. Apologies if I misunderstood.)

Assuming I'm still correct, then what you need is a transform from base_link->imu, which you said you have. Question: how did you generate this transform? It can't be static, as it will constantly change depending on the heading of the robot. For example, if you drive northeast, in the imu (ENU) frame, you'd get equal values for X and Y velocity. However, in base_link, you'd only have +X velocity. Now if you turned around and drove southwest, you'd have -X and -Y velocity in the imu frame, but would still have +X velocity in the base_link frame. Essentially, your base_link->imu transform would always have to contain the vehicle's current heading. It may be easier to edit your velocity node so that it automatically applies this transform internally before publishing, and then you can just publish the velocity with the base_link frame_id.

Before I respond to your issue regarding the differential setting, let me make sure we're on the same page with the odom frame. When you first start your robot, the odom frame is automatically aligned with your starting position and orientation. If you're facing northeast, then your robot still believes it's at (0, 0) with a heading of 0 (simplifying to 2D for illustration). This obviously is not the same as the ENU frame, which always has the same orientation. So at the starting point, your odom frame heading is 0, but your imu (ENU) frame heading is, say, pi/4. Again, you have a couple options here. First, if you didn't create a base_link->imu transform, you could now create an odom->imu transform (SEE NOTE AT THE END OF THIS UPDATE). In our example, you would transform a new IMU heading by -pi/4 to get the odom frame heading. That transform would be static, but you'd have to set it programmatically when you first start running. A second (and much easier) option is to simply turn the differential setting on. This will effectively treat the initial orientation as a "zero point" for all future measurements, so your first measurement has a heading of pi/4, but that becomes 0, and future measurements would be relative to it. The differential behavior is a bit more complicated than that, but for this question, that's all that's relevant.

To summarize: I would edit the IMU velocity generation node to output velocities in the base_link frame. I would then turn on the differential setting for your IMU so that the orientation starts out at zero. Feel free to upload a bag and I'll check it out at some point.

Q2: That seems reasonable, sure. That's really a tf question. I'm using message filters to ensure that transforms are available for any message I receive.

END OF UPDATE NOTE The statements I made above regarding transforming the IMU data are only partially true. I still need to fix this. Right now, ekf_localization_node always tries to transform IMU data - both angular velocity and orientation - into base_link. I either need to let users specify a target frame for IMU orientation and velocity data, or simply make it so all orientation data gets transformed into odom, and all angular velocity and acceleration data get transformed into base_link.


EDIT: Responding to update that contains Q3:

OK, so your velocities are being reported in the body frame. In this case, what I would do is:

  1. Generate a base_link->imu transform that contains the IMU's orientation and position w.r.t the body frame.
  2. Change your IMU data's frame to be imu.
  3. Enable the differential setting for your IMU.

What you want to avoid (at least in this case; this is not generally true) is to require ekf_localization_node to use the very transform it's meant to be generating (e.g., odom->base_link). Re: the coordinate frame of the IMU orientation data, part of the issue is the IMU message itself. It has only one frame_id specified in the message, but the orientation data is usually reported in a world-fixed frame, and the angular velocity and linear acceleration is usually reported in another (e.g., the base_link frame). ekf_localization_node will try to transform the data into the frames it requires using the frame_id of the IMU message. While it treats orientation and velocity/acceleration data separately, it's still going to use the frame_id in that message as the source frame when it attempts to carry out the transform. So what is the target frame? For orientation data, it ought to be your world_frame (e.g., map or odom), but it can't be if your IMU transform is relative to your base_link frame, as you're back to square one with your issue (the chicken-or-egg problem). For angular velocity and acceleration data, it's the body frame (e.g., base_link). The problem is that we cannot define an odom->imu transform and a base_link->imu transform, as that would be dual-parenting in the tf tree, which would break the tree.

In any case, for you, the differential setting for your IMU ought to fix the orientation issue anyway.


Originally posted by Tom Moore with karma: 13689 on 2014-11-13

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by Raphael Nagel on 2014-11-17:
Hi Tom,

I have implemented your last suggestions and it does seem to work now. However, without integrating the next sensor (i.e. Position from Sonar) I cannot really investigate further - the position drifts a lot. I'll update here once that the Sonar is in place. Thank you for your help.

Comment by Tom Moore on 2014-11-17:
Yeah, double-integration of acceleration data will usually result in a lot of drift. Glad I could help, and good luck!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.