Ask Your Question
0

Simulating robot_localization on pioneer3at with xsens 300 AHRS

asked 2015-01-26 05:19:04 -0500

Orso gravatar image

updated 2015-02-07 09:40:40 -0500

I am testing robot_localization using the receive_xsens stack listed here: link text, and the ros-pioneer3at stack listed here: link text.

I set up the frame id's to match those in the pioneer3at simulation stack as follows:

map_frame = Pioneer3AT/map
odom_frame = Pioneer3AT/odom
base_link_frame = Pioneer3AT/base_link
world_frame = Pioneer3AT/odom

I configured xsens imu to produce orientation, angular velocity, and acceleration. The imu0 configuration looks like this:

<param name="imu0" value="/imu/data" />

I will incorporate p3at hardware later with wheel encoders. For now trying to fuse simulated odometry from the ros-pioneer3at stack. Specifically, I have configured the odom0 as follows:

<param name="odom0" value="Pioneer3AT/pose" />

After configuring, I launch the ros-pioneer3at simulation, then launch the receive_xsens hardware, then launch the ekf_localization to fuse the sensor and simulation data.

There is no data being published to odometry/filtered. I am not sure where to start the debugging.

-----edit 1-----

Here is the node graph:

rosgraph.png

-----edit 2-----

Thanks for the insight!

Here is the script for /Pioneer3AT/pose:

import rospy
import tf
from nav_msgs.msg  import Odometry
def subCB(msg):
  P = (msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z )
  Q = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)
  br = tf.TransformBroadcaster()
  br.sendTransform(P, Q, rospy.Time.now(), "/Pioneer3AT/base_link", "world")
if __name__ == '__main__':
  rospy.init_node('tf_from_pose')
  rospy.Subscriber("/Pioneer3AT/pose", Odometry, subCB)
  rospy.spin()

Also, here is the ekf_localization_node launch file I used to try and fuse the xsens imu hardware with the ros-pioneer3at simulation:

<launch>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
<param name="frequency" value="30"/>  
<param name="sensor_timeout" value="0.1"/>  
<param name="two_d_mode" value="false"/>

<param name="map_frame" value="/Pioneer3AT/map"/>
<param name="odom_frame" value="/Pioneer3AT/odom"/>
<param name="base_link_frame" value="/Pioneer3AT/base_link"/>
<param name="world_frame" value="/Pioneer3AT/odom"/>

<param name="odom0" value="/Pioneer3AT/pose"/>
<param name="imu0" value="/imu/data"/>

<rosparam param="odom0_config">[true, true, true, 
                                true, true, true, 
                                false,  false, false, 
                                false, false, false,
                                false, false, false]</rosparam>

<rosparam param="imu0_config">[false, false, false, 
                               true,  true,  true, 
                               false, false, false, 
                               true,  true,  true,
                               true,  true,  true]</rosparam>

  <param name="odom0_differential" value="false" />
  <param name="imu0_differential" value="false" />
  <param name="imu0_remove_gravitational_acceleration" value="true"/>


  <param name="odom0_pose_rejection_threshold" value="5"/>
  <param name="imu0_pose_rejection_threshold" value="0.3"/>
  <param name="imu0_angular_velocity_rejection_threshold" value="0.1"/>
  <param name="imu0_linear_acceleration_rejection_threshold" value="0.1"/> -->

  <rosparam param="process_noise_covariance">
[0.05, 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.05, 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.06, 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 ...
(more)
edit retag flag offensive close merge delete

Comments

your topics might be namespaced to "pioneer3AT/imu/data" for example. can you post a picture of the ros node graph from rqt?

l0g1x gravatar image l0g1x  ( 2015-01-26 10:24:55 -0500 )edit
1

Seems like you only have one input source (imu). I may be wrong, but im pretty sure you may need at least two. (wheel odometry, GPS, etc..)

l0g1x gravatar image l0g1x  ( 2015-01-26 11:08:25 -0500 )edit

@Tom Moore:

--Pioneer3AT/pose is of the type nav_msgs.msg/Odometry. I have included the scripts in my original post, along with my ekf_localization_node, and a sample message from each sensor.

--Still trying to get one sensor at a time up and running.

--Not able to run /imu/data alone.

Orso gravatar image Orso  ( 2015-01-26 17:00:34 -0500 )edit

Would you mind just posting one example of each sensor message? You don't need to do record all of them with -p. Just do rostopic echo, copy a single message, and paste it in the question. I'm not trying to debug all your data. I just want to see one sample message of each. Thanks!

Tom Moore gravatar image Tom Moore  ( 2015-01-28 16:25:14 -0500 )edit

Ha, no, sorry, I meant the entire message, complete with frame_ids. Like this (click the "more" link in the question to see the whole thing).

Tom Moore gravatar image Tom Moore  ( 2015-01-29 07:57:56 -0500 )edit

What do you mean by "set position/orientation with two broadcasters"? Do you still have the P3 node broadcasting the odom->base_link transform as well?

Tom Moore gravatar image Tom Moore  ( 2015-02-08 11:49:41 -0500 )edit

I meant that ekf_localization is broadcasting odom->base_link and Gazebo_Bridge is broadcasting odom->base_link. I could not realize another way to provide odom data in my current setup. For now, I am fusing simulation odom data, with xsens data. Later planned to use wheel odometry data.

Orso gravatar image Orso  ( 2015-02-08 19:26:34 -0500 )edit

What do you mean by "provide odom data"? You simply can not have two different nodes publishing the same transform. If you are running ekf_localization_node, it is providing your odometry message and the odom->base_link transform.

Tom Moore gravatar image Tom Moore  ( 2015-02-09 07:43:50 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2015-01-27 09:34:29 -0500

Tom Moore gravatar image

updated 2015-02-04 08:38:47 -0500

One data source will work.

What message type is Pioneer3AT/pose? If it's a PoseWithCovarianceStamped, then your configuration is wrong. Change it to

<param name="pose0" value="Pioneer3AT/pose" />

When in doubt, start with one sensor at a time. Get your pose data working, then add the IMU, etc.

EDIT: Also, the best thing to do is paste your entire ekf_localization_node launch file, and post a sample message from each sensor.

EDIT 2: Your IMU data appears to be in the /imu frame. Are you providing a transform from /imu to /Pioneer3AT/base_link? Also, you want to make sure that your Pioneer code is not broadcasting a /Pioneer3AT/odom->/Pioneer3AT/base_link transform.

EDIT 3: So I ran your bag file with the launch file you posted. I am definitely getting publications on /odometry/filtered, though the only data going into it is IMU data, because your bag didn't have any Pioneer odometry messages in it. If you're still having trouble, do this for me:

  1. Set up your robot as before, without ekf_localization_node running. Bring everything up.
  2. Start bagging everything.
  3. Drive your robot in some pattern. A square works well. Make note of how you drove the robot (e.g., clockwise turns, leg lengths, etc.).
  4. Post the bag, and let me know what, if any, output you were getting from ekf_localization_node.

EDIT 4: If you download the bag file you posted, and then start playing it back, and do rostopic echo /Pioneer3AT/pose, you'll see that no messages are published to /Pioneer3AT/pose (or you can just do rosbag info on the bag and see that no odometry messages are in there). Now run your system live, and do rostopic echo /Pioneer3AT/pose. If nothing comes out, then you've got a problem with your odometry publishing.

EDIT 5 (in response to comment): here's a screenshot of the output of your bag after about 30 seconds. Is this what you mean by "light years"?

image description

This is exactly what I would expect to see from a single IMU being used for your state estimate. You're fusing linear acceleration, which is typically pretty noisy, and is clearly not zero-mean in this case, as you are creeping along in all three dimensions. If you have no other measurements from other sensors to help, small errors in your IMU acceleration will accumulate and lead to false velocity, which will continue to grow infinitely, and your position will as well.

Long story short: you can't get a good state estimate with a single IMU by itself.

Re: the odom->base_link transform, see REP-105. ROS assumes three principal frames: map, odom, and base_link (or whatever naming scheme you want). The purpose of packages like robot_localization is to provide two things:

  1. A nav_msgs/Odometry message with the robot's current pose estimate
  2. A transform from odom->base_link or a transform from map->odom. With your configuration, we are going from /Pioneer3/odom->/Pioneer3/base_link. This transform will contain effectively the same ...
(more)
edit flag offensive delete link more

Comments

I am not providing a transform from /imu frame to /Pioneer3AT/base_link. I believe that I had the broadcaster on from /Pioneer3AT_Gazebo, broadcasting a /Pioneer3AT/odom->/Pioneer3AT/base_link transform. Sorry about that.

Orso gravatar image Orso  ( 2015-01-28 17:53:34 -0500 )edit

No worries! Try providing the /Pioneer3AT/base_link->imu transform (you can probably just use tf's static_transform_publisher). Then disable the broadcast of the odom->base_link transform from Gazebo. Let me know if that helps.

Tom Moore gravatar image Tom Moore  ( 2015-01-29 08:00:30 -0500 )edit

I tried tf's static_transform_publisher for Pioneer3AT/base_link->imu while disabling broadcast of odom->base_link. /odometry/filtered publish's pose and orientation data. Both grow without bounds, and seem unaffected by the imu. Would you have any other suggestions? Greatly appreciated!

Orso gravatar image Orso  ( 2015-01-30 00:11:12 -0500 )edit

Can you run your system without running anything from robot_localization, and then post a bag? Thanks!

Tom Moore gravatar image Tom Moore  ( 2015-01-30 11:45:28 -0500 )edit

Unless you have a Kinect/camera/LIDAR, then yes.

Tom Moore gravatar image Tom Moore  ( 2015-01-30 14:05:15 -0500 )edit

Tom: Here is a link to the bag file: link text

Please let me know if I can provide anything else....Is it best to post in the question? Thanks!

Orso gravatar image Orso  ( 2015-01-30 14:12:48 -0500 )edit

Here is link to bag file: link text. I turned off Pioneer3AT/odom->Pioneer3AT/base_link. Just using imu sensor for now. I moved forward a few meters, turned CCW, forward a few meters, CCW, forward a few meters.

Orso gravatar image Orso  ( 2015-02-02 17:03:20 -0500 )edit

The bag file is in the previous comment. This time I surely had nothing running from robot_localization (Sorry about the previous bag file that did), and I got no output from robot_localization. Thanks!

Orso gravatar image Orso  ( 2015-02-03 08:18:42 -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: 2015-01-26 05:19:04 -0500

Seen: 760 times

Last updated: Feb 07 '15