0
$\begingroup$

I would like to know more about the service provided by the package robot_localization /set_pose. I'm working in ROS Melodic for this task.

In my case, I'm running a dual ekf filter, so the local one will publish the local odometry (pose between odom and base_link) and is fed with wheel odometry, as well as IMU. For the second one (global) I'm adding GPS Pose. Here is my configuration file:

ekf_local:
frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.5
transform_timeout: 0.0
print_diagnostics: false
debug: false

map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom

odom0: odom_setter/odom
odom0_config: [false, false, false,
              false, false, false, 
              true,  true,  false, 
              false, false, true, 
              false, false, false] 
odom0_queue_size: 10
odom0_differential: false
odom0_relative: false

imu0: imu/data
imu0_config: [false, false, false,
             false,  false,  true,
             false, false, false,
             false,  false,  true,
             true,  false,  false]

imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true

use_control: false

process_noise_covariance: [1e-3, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                          0,    1e-3, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                          0,    0,    1e-3, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0.3,  0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0.3,  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.5,   0,     0,    0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0.5,   0,    0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0.1,  0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0.3,  0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.3,  0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.3,  0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.3,  0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.3,  0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.3]

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,    0,    0,     0,     0,     0,    0,    0,
                             0,    0,    0,    1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                             0,    0,    0,    0,    1.0,  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,    1.0,  0,    0,    0,     0,     0,     0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,    1.0,  0,    0,     0,     0,     0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,    0,    1.0,  0,     0,     0,     0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,    0,    0,    1.0,   0,     0,     0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1.0,   0,     0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1.0,   0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1.0,  0,    0,
                             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1.0,  0,
                             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1.0]

ekf_global:
frequency: 10
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.5
transform_timeout: 0.0
print_diagnostics: false
debug: false

map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: map

odom0: odom_setter/odom
odom0_config: [false, false, false,
              false, false, false,
              true,  true,  false,
              false, false, true,
              false, false, false]
odom0_queue_size: 10
odom0_differential: false
odom0_relative: false

imu0: imu/data
imu0_config: [false, false, false,
             false,  false, true,
             false, false, false,
             false,  false,  false,
             true,  true,  false]

imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true

use_control: false

odom1: /navsat/odometry/filtered
odom1_config: [true,  true,  false,
               false, false, false,
               false, false, false,
               false, false, false,
               false, false, false]
               
odom1_queue_size: 10
odom1_differential: false
odom1_relative: false

process_noise_covariance: [1.0,  0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                          0,    1.0,  0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                          0,    0,    1e-3, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0.3,  0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0.3,  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.5,   0,     0,    0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0.5,   0,    0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0.1,  0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0.3,  0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.3,  0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.3,  0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.3,  0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.3,  0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.3]

initial_estimate_covariance: [1.0,  0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                             0,    1.0,  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,    1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                             0,    0,    0,    0,    1.0,  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,    1.0,  0,    0,    0,     0,     0,     0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,    1.0,  0,    0,     0,     0,     0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,    0,    1.0,  0,     0,     0,     0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,    0,    0,    1.0,   0,     0,     0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1.0,   0,     0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1.0,   0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1.0,  0,    0,
                             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1.0,  0,
                             0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1.0]

navsat_transform:
frequency: 10
delay: 0.2
magnetic_declination_radians: 0.00576  # For lat/long 37.147830, 0.33º E
yaw_offset: 1.570796  # IMU reads 0 facing magnetic north, not east
zero_altitude: true
broadcast_cartesian_transform: false
broadcast_cartesian_transform_as_parent_frame: true
publish_filtered_gps: true
use_odometry_yaw: false
wait_for_datum: false
transform_timeout: 0.3
debug: false

I'm using navsat_node as well, but in this case, I will publish the utm -> map tf myself, so there's no reason to comment on this.

I would like to update the state of the filter so that for example, odom and map frames are not created one above the other at the start of a simulation. If this is not the correct use of updating the state of a filter, please correct me.

Imagine the next scenario: We run a simulation using Gazebo where the robot spawns at (3,3).

  • The control node won't publish the tf from odom -> base_link since I want robot_localization to do this taking into account the IMU information.

  • Also, I would like to state the Gazebo World frame equal to my map frame, in order to make the odometry published in the ekf_global would be the same as the position of the model inside Gazebo.

Only running the simulation will create my TF tree using base_link as the parent of the frames, and all the static transforms to other frames (except for the wheels).

Now when I run the EKF filter, if I don't make any changes, both odom and map frames will be published at the same position as my base_link, but I said before that I would like the MAP frame to match the Gazebo reference frame.

That's when I thought I could use this service to reinitialize the state of the ekf_global filter depending of the pose in Gazebo. The idea I have in mind is creating a service to get the position in Gazebo World, and then calling the /set_pose service with the pose equivalent.

I have checked the code from ros.filters.cpp as well as the wiki here, and confirms that the idea is not take into account the previous meassures and set the initial state of the filter.

Expected behaviour: enter image description here

What actually happens: video here

The fact here is that the Expected behaviour is obtained if I toggle the filter off. If I toggle on again the filter, it republishes everything above each other, but that wouldn't make sense if I've just reinitialized the filter state. Does it take into account the previous state that it had before being toggled off?

Hope someone can clarify me the correct way of using this service

Edit1: I will explain each topic before attaching the launch and msgs requested

Both filters will start out at the origin. As your robot drives around, they will diverge.

I understand the concept of robot_localization but I wanted to go a bit further and recreate this situation: Imagine we are inside a building where we have a blueprint of it and I have a point in the middle of it which represents my "local point of reference" (let's suppose that the frame UTM would be my global point of reference, and the coordinates using the point in the middle of the blueprint my local reference(despite the fact that normally, global is map->base and local is odom -> base)). Imagine that I recreate the building inside Gazebo, and I spawn the model such that the middle point would be at the center of Gazebo (in fact, it doesn't need to be the middle of the model)

The idea is that, I could spawn the robot in different rooms of the building, and I do know the local positions of each room according to the blueprint. As I want the map frame to represent the local position of my robot, it wouldn't make sense to spawn odom just above map frame since the robot is not starting in the middle of the building.

If I don't use this idea, map would be dependent of the pose where the robot is spawned, which is not ideal since the map frame is well defined and shouldn't change relative to the robot's pose.

I'm inclined to ask why this is necessary. Gazebo has its own world frame that is independent from your coordinate frames. What do you gain from the map-frame EKF output matching the Gazebo pose?

As I mentioned in the last comment, I would like map->base_link to represent my pose inside my blueprint. I could define several poses where I could spawn and as I'm working in the same environment, the utm -> map is defined and always the same.

Regardless, if you want your robot's pose to match the Gazebo world frame pose, then I believe the Gazebo pose is published as a message, right? Can you just make the Gazebo pose an input to your map-frame EKF?

This could be a solution but the idea is to be as close to the real situation, so there shouldn't be an "absolute topic" where I know my exact position inside the building.

You set the EKF pose to some arbitrary location using the service call, but then it gets a GPS measurement, and that sends it right back to where it was when navsat_transform_node was initialized

Interesting, I might be misunderstanding something. When you mention "to where it was", isn't set_pose service supposed to not take into account any meassures from before? I imagined this services as a reinitialization of the ekf filter.

Edit of this section: I totally forgot about the real weight of the gps inside the EKF. The GPS is publishing an Odometry message, as the GPS fix msg is the same, that odometry isn't modified, and that's the reason it is redirecting the state to the first place?

Before attaching things, it's been a while since I posted the question, and I did solve this, but I don't think this is the best solution to this problem.

I had to disable the global filter at startup, and use a custom node to get the "local pose" of my robot inside the simulation, through Gazebo services (in real situation the idea is that somehow a server or previous status files could send the robot it's last registered pose). This pose represents the tf from map -> odom so I need to calculate the transformation from it, and use the set_pose service with that result before I activate it. In this way, map frame was always at the same place, and I could spawn my robot in whichever position and orientation, that the tfs were set correctly.

Please if something wasn't clear don't hesitate to ask again, since it is a large answer and perhaps I could have explained better some aspect hehe.

This is the situation from where I got the messages: Robot spawned at 2.0,3.0

dual_ekf.launch

<launch>

  <rosparam command="load" file="$(find localization_outdoor)/params/dual_ekf_navsat.yaml" />

  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true" respawn="true">
    <remap from="/imu/data" to="/imu/data" />
    <remap from="/gps/fix" to="/gps/fix" />
    <remap from="/odometry/filtered" to="ekf_global/odometry/filtered"/>

    <remap from="/odometry/gps" to="/navsat/odometry/filtered" />
    <remap from="/gps/filtered" to="navsat/gps/filtered"/>
  </node>

    <!-- Esta trasformada funciona regular, debido al control malo  -->
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_local" clear_params="true" respawn="true">
    <remap from="odometry/filtered" to="ekf_local/odometry/filtered"/>

    <remap from="set_pose" to="/ekf_local/set_pose"/>
  </node>
  
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_global" clear_params="true" respawn="true">
    <remap from="odometry/filtered" to="ekf_global/odometry/filtered"/>

    <remap from="set_pose" to="/ekf_global/set_pose"/>
  </node>

</launch>

imu/data (Imu)

rostopic echo -n1 /imu/data
header: 
  seq: 1251
  stamp: 
    secs: 25
    nsecs: 200000000
  frame_id: "base_link"
orientation: 
  x: 1.81671642062e-05
  y: -0.000284356047451
  z: -6.4493040437e-07
  w: 0.999999959406
orientation_covariance: [2.6030820491461885e-07, 0.0, 0.0, 0.0, 2.6030820491461885e-07, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 1.45719417112e-05
  y: -0.00858249676551
  z: -0.00326851133086
angular_velocity_covariance: [2.5e-05, 0.0, 0.0, 0.0, 2.5e-05, 0.0, 0.0, 0.0, 2.5e-05]
linear_acceleration: 
  x: 0.0151732557259
  y: 0.000133832683127
  z: 9.79843866127
linear_acceleration_covariance: [2.5e-05, 0.0, 0.0, 0.0, 2.5e-05, 0.0, 0.0, 0.0, 2.5e-05]

gps/fix (NavSatFix)

rostopic echo -n1 /gps/fix
header: 
  seq: 1672
  stamp: 
    secs: 42
    nsecs:         0
  frame_id: "base_link"
status: 
  status: 0
  service: 0
latitude: 36.7157024582
longitude: -4.49090215601
altitude: 0.132276267621
position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 2

husky_velocity_controller/odom (Odometry)

rostopic echo /husky_velocity_controller/odom -n1
header: 
  seq: 3096
  stamp: 
    secs: 62
    nsecs: 248000000
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 7.00959522238e-05
      y: -4.3015274195e-11
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -3.38496439936e-07
      w: 1.0
  covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.03]
twist: 
  twist: 
    linear: 
      x: 9.05422244247e-07
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 4.46662676883e-11
  covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.03]

navsat/odometry/filtered

rostopic echo /navsat/odometry/filtered -n1
header: 
  seq: 954
  stamp: 
    secs: 96
    nsecs: 250000000
  frame_id: "map"
child_frame_id: ''
pose: 
  pose: 
    position: 
      x: 2.00008654641
      y: 2.99999951501
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance: [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, 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]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [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, 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]

Edit2: I get the point, the state of the filter isn't able to reset since the odom being published by the navsat node hasn't changed. As I mentioned, the idea was to stick at most to the reality inside the simulation, as @automatom pointed out, perhaps I'm struggling in something that it won't be possible to get working, so I left it for the moment. Right now, in simulation I'm able to set the initial state of the filter to match Gazebo's pose while in real tests I just spawn the map frame on top of the robot.

$\endgroup$
3
  • $\begingroup$ Please post a sample message from each sensor input. $\endgroup$
    – automatom
    Commented Feb 15 at 16:09
  • $\begingroup$ Also, please include your launch file, as you apparently have some remapping going on. $\endgroup$
    – automatom
    Commented Feb 15 at 16:19
  • $\begingroup$ Thank you @automatom for the response, please see my edited response. $\endgroup$
    – ÁngeLoGa
    Commented Feb 16 at 8:19

1 Answer 1

0
$\begingroup$

I would like to update the state of the filter so that for example, odom and map frames are not created one above the other at the start of a simulation.

This expected behaviour in general. Both filters will start out at the origin. As your robot drives around, they will diverge. If the robot starts getting pose messages from a pose sensor source in the map-frame EKF instance, it will move the robot to that pose. But navsat_transform_node intentionally transforms the GPS data to be consistent with your initial map-frame pose, so if the robot hasn't moved, I'd expect the map- and odom-frame EKF instances to remain at the origin.

Also, I would like to state the Gazebo World frame equal to my map frame, in order to make the odometry published in the ekf_global would be the same as the position of the model inside Gazebo.

I'm inclined to ask why this is necessary. Gazebo has its own world frame that is independent from your coordinate frames. What do you gain from the map-frame EKF output matching the Gazebo pose?

Regardless, if you want your robot's pose to match the Gazebo world frame pose, then I believe the Gazebo pose is published as a message, right? Can you just make the Gazebo pose an input to your map-frame EKF? Note that this will not work well unless you turn off the GPS: your pose may "jump" back and forth between the GPS-derived position and the Gazebo pose.

If I had to guess, the reason the pose "jumps" back to the origin is because you are using navsat_transform_node. You set the EKF pose to some arbitrary location using the service call, but then it gets a GPS measurement, and that sends it right back to where it was when navsat_transform_node was initialized, which is the origin.

Edit in response to further questions:

Interesting, I might be misunderstanding something. When you mention "to where it was", isn't set_pose service supposed to not take into account any meassures from before? I imagined this services as a reinitialization of the ekf filter.

set_pose just resets the current state of the filter. But navsat_transform_node is a separate process producing measurements. So you will see this:

  • Time t0: EKF and navsat_transform_node initialise. navsat_transform_node uses your current EKF pose (at the origin) to compute a transform from the UTM frame to your map frame.
  • Time t1: you get a GPS measurement. It gets transformed into a pose, but since you haven't moved, that pose is just (0, 0) with 0 yaw. That gets fused into your state estimate by the EKF (which was already at that pose).
  • Time t2: you call set_pose on the EKF. That moves the EKF to some other pose.
  • Time t3: you get another GPS measurement from navsat_transform_node. You haven't moved (in GPS coordinates), so navsat_transform_node produces another pose of (0, 0) with 0 yaw. The EKF receives that measurement, and your state jumps back to the origin.

I would like map->base_link to represent my pose inside my blueprint

What you are trying to do is reconcile two different global coordinate frames, the GPS (UTM) frame and your robot's pose in the Gazebo world. In other words, what you want is for the datum in navsat_transform_node to match your map origin in Gazebo. And that's going to be difficult in both the real world and in sim. There's a parameter for it, but you're going to have to figure out how to set that parameter such that the resulting measurements align with your Gazebo world frame. That won't be trivial, and it also won't necessarily work if you moved to some other Gazebo model or tried this in the real world.

If it were me, I might revisit the overall design and determine why I need the Gazebo world frame to match my EKF coordinates. If it's purely for convenience/debugging, then maybe write a small node that produces a transform between the Gazebo frame and your EKF frame so that you can get the pose in the frame you want.

$\endgroup$
1
  • $\begingroup$ I updated my answer, sorry for the delay but I got no notification when you edited your answer, and thanks again for the time and effort $\endgroup$
    – ÁngeLoGa
    Commented Feb 20 at 15:47

Your Answer

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

Not the answer you're looking for? Browse other questions tagged or ask your own question.