0
$\begingroup$

Rosanswers logo

Hi,

I have the objective of collecting laser data using a tripod and moving the tripod and I am using a Mavros ArduPilot board on the top of that tripod so that I can fuse the GPS and IMU. Initially, I have created a simple .urdf file, where I have the laser (laser_base) approximately 1m above the base_link.

To fuse the IMU and GPS, my launch file is as follows:

<arg name="fcu_url" default="/dev/ttyUSB1:57600" />
(...)
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_local" clear_params="true">
  <param name="frequency" value="10 "/>
  <param name="sensor_timeout" value="0.1"/>
  <param name="two_d_mode" value="false"/>

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

  <param name="transform_time_offset" value="0.0"/>

  <param name="odom0" value="/odom/out"/>
  <param name="imu0" value="/mavros/imu/data"/>

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

  <rosparam param="imu0_config">[false, false, false,
                                 false, false, false,       <!-- roll, pitch, yaw -->
                                 true, true, true,           <!-- dx, dy, dz2 -->
                                 true, true, true,           <!-- droll, dpitch, dyaw -->
                                 false, false, false]</rosparam> <!-- dx2, dy2, dz2 -->

  <param name="odom0_differential" value="false"/> <!-- this was false -->
  <param name="imu0_differential" value="false"/>
  <param name="odom0_relative" value="false"/>
  <param name="imu0_relative" value="false"/>
  <param name="imu0_remove_gravitational_acceleration" value="true"/>
  <param name="print_diagnostics" value="true"/>

  <param name="debug"           value="false"/>
  <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

  <remap from="/odometry/filtered" to="/odom/out"/>
</node>

<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">

    <param name="frequency" value="20"/>
    <param name="delay" value="3"/>
    <param name="magnetic_declination_radians" value="0"/>
    <param name="yaw_offset" value="0"/>
    <param name="zero_altitude" value="false"/>
    <param name="broadcast_utm_transform" value="false"/>
    <param name="publish_filtered_gps" value="false"/>
    <param name="use_odometry_yaw" value="false"/>
    <param name="wait_for_datum" value="false"/>

    <remap from="/gps/fix" to="/mavros/global_position/raw/fix"/>
    <remap from="/imu/data" to="/mavros/imu/data"/>
    <remap from="/odometry/filtered" to="/odom/out" />

</node>

<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
</node>
<param name="robot_description" textfile="$(find ODpkg)/description/tripod.urdf" />
<node name="modelvisualization" pkg="rviz" type="rviz" output="screen"/>

So, navsat_transform_node receives the IMU from the ekf_localization_node and produces the odometry that will be used by ekf_localization_node, which results in a closed loop, as stated here.

Also, I have defined the world frame the same as the odom frame, as stated in the Wiki.

As the odom is being produced by the gps, in odom_config, I have only defined x, y and z as true. Also, if on imu_config, I define either roll, Pitch, yaw, dx2, dy2 or dz2 as true, the base_link frame starts moving away from odom_frame infinitely and I don't know why.

So, to sum up, I wan't to move the tripod and keep recording laser data, so I want to notice any inclination or change in the direction of the tripod, as well as x, y, z changes. As it is right now, the distance between odom and base_link remains the same, no matter if I move the tripod. On the other hand, the joint, between laser_base and base_link (the "tripod") changes its orientation, but it starts getting an offset and sometimes gets completely wrong for no apparent reason.

A snippet of the IMU topic from the ArduPilot (/mavros/imu/data) is shown below:

header: 
  seq: 61
  stamp: 
    secs: 1533685811
    nsecs: 564266624
  frame_id: "base_link"
orientation: 
x: -0.00348900489561
y: -0.019581376355
z: -0.758538467474
w: 0.651324642484
orientation_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
angular_velocity: 
  x: 0.00158514082432
  y: -0.00135391950607
  z: 0.00221287831664
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration: 
  x: 0.0
  y: 0.0392266
  z: 9.83606995
linear_acceleration_covariance: [8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08]

And the GPS topic (mavros/global_position/raw/fix):

header: (...)
frame_id: "base_link"
status: 
  status: -1
  service: 1
latitude: 38.6639703
longitude: -9.2055322
altitude: 132.121468393
position_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 0

Any idea of what I might be doing wrong, namely on the imu_config? Shouldn't I be able to also set roll pitch yaw and the linear accelerations to true? Also, why is the base_link not moving when I move the tripod, considering it is taking the odom from the gps into consideration?

Thank you in advance.


Originally posted by jpde.lopes on ROS Answers with karma: 64 on 2018-08-07

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

In your EKF node config, your gps odometry topic name looks incorrect. It should be the output coming from your navsat_transform_node (which by default is odometry/gps). Also, not critical, but in your imu config you are fusing x/y/z velocities which aren't published by your imu.


Originally posted by stevejp with karma: 929 on 2018-08-07

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by jpde.lopes on 2018-08-08:
So, in my IMU config, should I be fusing the first two rows; the angular velocity and acceleration?

Comment by jpde.lopes on 2018-08-08:
According to this, doesnt the output of navsat defaults to odom/filtered (which I am then remapping to /odom/out)?

Comment by stevejp on 2018-08-08:
For your IMU, the only valid rows to fuse are [2,4,5] which correspond to roll-pitch-yaw, roll dot-pitch dot-yaw dot, and accel x, accel y, accel z. My advice would be to start with just fusing roll-pitch-yaw and then add the others and compare performance.

Comment by stevejp on 2018-08-08:
The statement Tom made "The odometry input to...output (it defaults to odometry/filtered)." is saying that the default input odom topic for navsat_transform is odometry/filtered, which is the default output of the EKF. But the default output of navsat_transform is odometry/gps

Comment by jpde.lopes on 2018-08-09:
Thank you very much for your answer and tips. It seems to be stable so now, but if I move the ArduPilot board, nothing happens, which I assume is because the gps coordinates are not changing. Tomorrow I'll go outside to test it better. Nevertheless, I will accept your answer.

Comment by jpde.lopes on 2018-08-09:
One thing that is bothering me now, is that with rqt_graph, the navsat_transform_node is not receiving any IMU topic, as it should.

Comment by jpde.lopes on 2018-08-09:
Also, if I set accel x, accel y, accel z on IMU config to true, the base_link starts getting away from odom infinitely.

Comment by stevejp on 2018-08-09:
navsat_transform_node only needs one imu message to initiate the utm->map frame TF, so it could be dropping the connection once it's publishing that TF. Regarding accelerations: does this happen even when you're receiving good gps?

Comment by jpde.lopes on 2018-08-09:
Thank you. Might be. I will test outdoor in a couple of hours and will give feedback. Although it is not behaving bad, just a little slow, which is related to the Mavros topics' publish rate and not with the robot localization package.

Comment by jpde.lopes on 2018-08-11:
Sorry for the delay. Sill not working properly. Went outside to test today, but the x,y,z are not changing, although the GPS is presenting different values in its topic.

Comment by stevejp on 2018-08-11:
Can you record & upload a bag file? Would make it easier to troubleshoot.

Comment by jpde.lopes on 2018-08-11:
Here it is. I created it when walking around with the ArduPilot board. The recorded topics are the gps and IMU ones (/mavros/imu/data and /mavros/global_position/raw/fix)

Comment by jpde.lopes on 2018-08-11:
I have also created a new question with the current error it is showing.

Comment by jpde.lopes on 2018-08-11:
And I have created a ros bag, where I am running the ekf and navsat_transform_node, and recorded all topics.

$\endgroup$

Your Answer

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