IMU Data Collection #1000

Kazadhum · 2025-01-17T11:48:56Z

Hi! For my research, I need to save data from IMUs in the ATOM dataset.

The goal is to then be able to integrate this data to translate it into transformations. As such, a continuous stream of data should be saved. This implies a different system for data collection than used in ATOM thus far. IMU data cannot be saved in "collections" like camera images or LiDAR point clouds.

My current idea is to make use of the additional_sesor_data dataset field, changing it to accommodate the saving of continuous data. This change would be additive, so no functionality is lost and hopefully backwards compatibility is preserved. This should be tested when the PR is created.

Below is my TO-DO list for the implementation of this functionality, so it is documented. Development will take place in a separate branch.

TO-DO:

Create a new system under atom_examples called rihibot - which is a new version of rihbot with an IMU in the end-effector, for testing
Add support for the IMU sensor modality to configure_calibration_pkg so that the configure script doesn't return an error
Implement continuous option for the collection of sensor data
Implement continuous option for the collection of additional_sensor_data (Not implemented, check additional_data leads to configuration error #1005)
Write the documentation for rihibot

This list will be updated as I progress and possibly find more obstacles.

Tagging @miguelriemoliveira @rarrais for visibility.

The text was updated successfully, but these errors were encountered:

added an IMU to the rihibot system, using the libgazebo_ros_imu.so plugin

added the /imu topic to the list of topics to record in record.launch

Kazadhum · 2025-01-20T10:23:43Z

I'm running into a small issue...

I get this error when running rosrun rihibot_calibration configure:

ATOM Error: Sensor imu calibration config defined link imu_link but bagfile messages on topic /imu have header.frame_id wrist_3_link. These should be the same! Check #514.

This prompted my to check the messages being published under the /imu topic:

header: 
  seq: 16514
  stamp: 
    secs: 2674
    nsecs:  11000000
  frame_id: "wrist_3_link"
orientation: 
  x: 0.6987632116657506
  y: 0.1087786864452839
  z: -0.6940625160617692
  w: -0.13481244451389074
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -0.00018741384054262985
  y: -0.0052810690874827634
  z: -0.006005440687883528
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: -1.277917167846468e-11
  y: 1.4583417090238843e-11
  z: -1.522552128873299e-11
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

The frame_id in the message is set to "wrist_3_link". However, the IMU plugin in configured to consider the frame imu_link...

atom/atom_examples/rihibot/rihibot_description/urdf/imu.urdf.xacro

Lines 45 to 72 in ce1aa71

    
           <gazebo> 
        
           	<plugin name="imu_plugin" filename="libgazebo_ros_imu.so"> 
        
           	<alwaysOn>true</alwaysOn> 
        
           	<bodyName>imu_link</bodyName> 
        
           	<frameName>imu_link</frameName> 
        
           	<topicName>imu</topicName> 
        
           	<serviceName>imu_service</serviceName> 
        
           	<gaussianNoise>0.0</gaussianNoise> 
        
           	<updateRate>200</updateRate> 
        
           	<imu> 
        
           		<noise> 
        
           		<type>gaussian</type> 
        
           		<rate> 
        
           			<mean>0.0</mean> 
        
           			<stddev>2e-4</stddev> 
        
           			<bias_mean>0.0000075</bias_mean> 
        
           			<bias_stddev>0.0000008</bias_stddev> 
        
           		</rate> 
        
           		<accel> 
        
           			<mean>0.0</mean> 
        
           			<stddev>1.7e-2</stddev> 
        
           			<bias_mean>0.1</bias_mean> 
        
           			<bias_stddev>0.001</bias_stddev> 
        
           		</accel> 
        
           		</noise> 
        
           	</imu> 
        
           	</plugin> 
        
           </gazebo>

Looking at the TF tree using RQT:

The documentation for the plugin is scarce, so I couldn't find anything on this, but I believe what is happening is the IMU plugin ignoring the fixed TFs, such as the wrist-3-to-flange and flange-to-imu_link. I think I'll continue developing using softbot to test but I will circle back to this.

Kazadhum · 2025-01-20T10:36:55Z

I just checked the source code for the libgazebo_ros_imu plugin and my previous assertion doesn't make sense... There is no place in the code where either frameName or frame_id are changed w.r.t. fixed TF's

miguelriemoliveira · 2025-01-20T10:39:18Z

Hey @Kazadhum ,

not sure what is happening, but from what I gather the problem is not in atom right?

Can you point to the IMU plugin code?

Kazadhum · 2025-01-20T10:42:56Z

Hello @miguelriemoliveira! Yeah, the problem isn't in ATOM, but in the gazebo plugin, I think.

Here's the gazebo plugin source code, and here's my use of the plugin in the URDF:

atom/atom_examples/rihibot/rihibot_description/urdf/imu.urdf.xacro

Lines 45 to 72 in ce1aa71

    
           <gazebo> 
        
           	<plugin name="imu_plugin" filename="libgazebo_ros_imu.so"> 
        
           	<alwaysOn>true</alwaysOn> 
        
           	<bodyName>imu_link</bodyName> 
        
           	<frameName>imu_link</frameName> 
        
           	<topicName>imu</topicName> 
        
           	<serviceName>imu_service</serviceName> 
        
           	<gaussianNoise>0.0</gaussianNoise> 
        
           	<updateRate>200</updateRate> 
        
           	<imu> 
        
           		<noise> 
        
           		<type>gaussian</type> 
        
           		<rate> 
        
           			<mean>0.0</mean> 
        
           			<stddev>2e-4</stddev> 
        
           			<bias_mean>0.0000075</bias_mean> 
        
           			<bias_stddev>0.0000008</bias_stddev> 
        
           		</rate> 
        
           		<accel> 
        
           			<mean>0.0</mean> 
        
           			<stddev>1.7e-2</stddev> 
        
           			<bias_mean>0.1</bias_mean> 
        
           			<bias_stddev>0.001</bias_stddev> 
        
           		</accel> 
        
           		</noise> 
        
           	</imu> 
        
           	</plugin> 
        
           </gazebo>

miguelriemoliveira · 2025-01-20T10:48:47Z

Hum. This sounds like black magic ...

here the this->frame_name_ is used to set the frame in the message.

https://github.com/ros-simulation/gazebo_ros_pkgs/blob/959e7d02ecb8d0c8940623921220ce960542f6c7/gazebo_plugins/src/gazebo_ros_imu.cpp#L278-L281

So what happens if you just delete these two from the xacro?

		<bodyName>imu_link</bodyName>
		<frameName>imu_link</frameName>

Kazadhum · 2025-01-20T10:52:03Z

If I delete those two lines, I get this error in gazebo:

[FATAL] [1737370228.346497682, 3310.400000000]: imu plugin missing <bodyName>, cannot proceed

which makes sense, given this:

https://github.com/ros-simulation/gazebo_ros_pkgs/blob/959e7d02ecb8d0c8940623921220ce960542f6c7/gazebo_plugins/src/gazebo_ros_imu.cpp#L97-L100

Kazadhum · 2025-01-20T11:03:26Z

Using only <bodyName>, I get:

[ INFO] [1737370389.539868998, 2574.721000000]: imu plugin missing <frameName>, defaults to <bodyName>
which also makes sense given the source code.

The IMU is publishing data but it still has wrist_3_link as its frame_id.

Kazadhum · 2025-01-20T11:16:10Z

I just checked and the same thing happens with softbot... Maybe I have to use the alternative libgazebo_ros_imu_sensor plugin? I never got it to work properly before, but I can try again.

Kazadhum · 2025-01-20T11:31:05Z

Ok so I added the libgazebo_ros_imu_sensor plugin and I think it is working properly! The only thing to account for is that this plugin does measure gravity, so this must be considered in any algorithm that uses this data.

However, the IMU messages now have the proper header_id!

header: 
  seq: 9766
  stamp: 
    secs: 2722
    nsecs: 380000000
  frame_id: "imu_link"
orientation: 
  x: -0.0028736304093780474
  y: 0.03402877967051662
  z: -0.9956221813167769
  w: -0.08700722080625889
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.021119082716111827
  y: 0.003690111404028421
  z: 7.822583583077104e-05
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.11410727924185207
  y: -0.6591437516596792
  z: 9.777142752129746
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

So I think this roadblock is gone

replaced the libgazebo_ros_imu plugin with the libgazebo_ros_imu_sensor plugin to get the measurements relative to imu_link instead of wrist_3_link

miguelriemoliveira · 2025-01-20T14:45:25Z

If I delete those two lines, I get this error in gazebo:

[FATAL] [1737370228.346497682, 3310.400000000]: imu plugin missing <bodyName>, cannot proceed

which makes sense, given this:

https://github.com/ros-simulation/gazebo_ros_pkgs/blob/959e7d02ecb8d0c8940623921220ce960542f6c7/gazebo_plugins/src/gazebo_ros_imu.cpp#L97-L100

right, but it proves those code lines are actually being used.

miguelriemoliveira · 2025-01-20T14:46:15Z

Ok so I added the libgazebo_ros_imu_sensor plugin and I think it is working properly! The only thing to account for is that this plugin does measure gravity, so this must be considered in any algorithm that uses this data.

However, the IMU messages now have the proper header_id!
header: 
  seq: 9766
  stamp: 
    secs: 2722
    nsecs: 380000000
  frame_id: "imu_link"
orientation: 
  x: -0.0028736304093780474
  y: 0.03402877967051662
  z: -0.9956221813167769
  w: -0.08700722080625889
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.021119082716111827
  y: 0.003690111404028421
  z: 7.822583583077104e-05
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.11410727924185207
  y: -0.6591437516596792
  z: 9.777142752129746
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
So I think this roadblock is gone

Great news!

miguelriemoliveira · 2025-01-20T14:46:45Z

So I think this roadblock is gone

on to the next 10 million ...

Added the option of using the 'imu' sensor modality in the configuration yaml file, so the configure script does not return an error;

Kazadhum · 2025-01-20T14:54:50Z

Changed the configure_calibration_pkg script so no errors are found when it sees a sensor with the imu modality... A small note on this. I did not add RViz configurations for visualizing IMu data. The arrow that results from the use of that plugin can be very large depending on the linear acceleration. Keeping in mind that the simulated IMU measures gravity, the resulting arrow is very large and does not, in my mind, facilitate the visualization of data.

Moving onto the collection of IMU data...

miguelriemoliveira · 2025-01-20T14:57:43Z

Changed the configure_calibration_pkg script so no errors are found when it sees a sensor with the imu modality... A small note on this. I did not add RViz configurations for visualizing IMu data. The arrow that results from the use of that plugin can be very large depending on the linear acceleration. Keeping in mind that the simulated IMU measures gravity, the resulting arrow is very large and does not, in my mind, facilitate the visualization of data.

Moving onto the collection of IMU data...

Hey @Kazadhum ,

I think there should be an rviz configuration even if its empty. What you meant was that there is no specific marker for imus configured automatically?

Kazadhum · 2025-01-20T15:01:51Z

Hi @miguelriemoliveira! Yes, that was what I meant!

When iterating through the sensor list, I added a continue when the modality is imu.

atom/atom_calibration/scripts/configure_calibration_pkg

Lines 860 to 908 in 448bc70

    
           # Generate rviz displays according to the sensor types 
        
           # Iterate alphabetically through the sensors dict keys https://github.com/lardemua/atom/issues/409 
        
           for idx, sensor_key in enumerate(sorted(list(config['sensors'].keys()))): 
        
               color = atom_core.drawing.colormapToRVizColor(cm_sensors[idx, :]) 
        
               topic = config['sensors'][sensor_key]['topic_name'] 
        
               topic_compressed = topic + '/compressed' 
        
               modality = config['sensors'][sensor_key]['modality'] 
        
               print('\tGenerating rviz displays for sensor ' + Fore.BLUE + sensor_key + Style.RESET_ALL + 
        
                     ' with topic ' + Fore.BLUE + topic + Style.RESET_ALL + ' (' + Fore.CYAN + modality + Style.RESET_ALL + ')') 
        
               if modality == 'rgb' or modality == 'depth': 
        
                   # Raw image 
        
                   rendered = env.get_template('/rviz/Image.rviz.j2').render(c={'Name': sensor_key + '-Image', 
        
                                                                             'Image_Topic': topic}) 
        
                   displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) 
        
                   # Camera 
        
                   rendered = env.get_template('/rviz/Camera.rviz.j2').render(c={'Name': sensor_key + '-Camera', 
        
                                                                              'Image_Topic': topic}) 
        
                   displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) 
        
               elif modality == 'lidar2d': 
        
                   # Raw data 
        
                   rendered = env.get_template('/rviz/LaserScan.rviz.j2').render(c={'Name': sensor_key + '-LaserScan', 
        
                                                                                 'Topic': topic, 
        
                                                                                    'Color': color}) 
        
                   displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) 
        
               # elif msg_type == 'sensor_msgs/PointCloud2': 
        
               elif modality == 'lidar3d': 
        
                   # Raw data 
        
                   rendered = env.get_template('/rviz/PointCloud2.rviz.j2').render(c={'Name': sensor_key + '-PointCloud2', 
        
                                                                                   'Topic': topic, 
        
                                                                                      'Color': color, 
        
                                                                                      'Color_Transformer': 'FlatColor', 
        
                                                                                      'Style': 'Spheres', 
        
                                                                                      'Size__m_': 0.02, 
        
                                                                                      'Alpha': 1}) 
        
                   displays.append(yaml.load(rendered, Loader=yaml.SafeLoader)) 
        
               elif modality == 'imu': 
        
                   continue 
        
               else: 
        
                   atomError('Warning: Cannot generate rviz configuration for sensor ' + sensor_key + ' with topic ' 
        
                             + topic + ' (' + modality + ')' + Style.RESET_ALL)

But maybe this is not the proper way to do this. What do you think?

miguelriemoliveira · 2025-01-20T18:10:47Z

But maybe this is not the proper way to do this. What do you think?

Yep. Makes sense.

added a warning for when a user tries configuring a system with an IMU, explaining it is still under development and not all calibration scripts are functional

#1000 Implemented the ability to collect data in a continuous manner, i.e., collect all sensor messages and record them all in the dataset during data collection. To do this, a new 'continuous' field in the configuration for a given sensor was created and included in the template. To maintain backwards compatibility with older datasets, if there is no 'continuous' field, it defaults to False and works as it did previously. The 'continuous' field was also added to the configuration YAML template. Additionally, support for the IMU sensor modality in data collection was also added.

Kazadhum · 2025-01-22T11:07:18Z

Hi @miguelriemoliveira and @rarrais! Continuous data collection was implemented! Check this the message for commit 7122ad1 for details.

I believe backwards compatibility was preserved with every change I made. I will test this further, however, before making the pull request.

I also added a warning (in commit c1d505c) for any user who includes an IMU to know that the scripts created during configuration might not be working properly if an IMU is included in the configuration.

The only two things missing is to add the continuous option to the additional_sensor_data field and to write the documentation for rihibot.

miguelriemoliveira · 2025-01-22T16:58:18Z

Hey @Kazadhum ,

great work.

Congratullations!

Removed the commented additional_data field that was added for testing after deciding to not implement it.

Kazadhum · 2025-01-22T17:21:25Z

Ran into a bug where, when saving a collection, IMU data for that collection (and not for the continuous data collection) would result in a data blob. Probably a data processing bug.

Working on a fix...

A bug was causing the IMU data recorded in the collections (and not in the continuous_sensor_data field) to be recorded as blob. Added message processing to turn it into a dictionary.

Kazadhum · 2025-01-22T17:30:44Z

... Fixed!

Renamed 'imu' sensor to 'imu_hand', to be more explicit.

Kazadhum added the enhancement New feature or request label Jan 17, 2025

Kazadhum self-assigned this Jan 17, 2025

Kazadhum added this to Ongoing work on General Calibration Jan 17, 2025

github-project-automation bot moved this to To do in Ongoing work on General Calibration Jan 17, 2025

Kazadhum moved this from To do to In progress in Ongoing work on General Calibration Jan 17, 2025

Kazadhum added a commit that referenced this issue Jan 17, 2025

feat(rihibot): added imu and respective plugin #1000

ce904f1

added an IMU to the rihibot system, using the libgazebo_ros_imu.so plugin

Kazadhum added a commit that referenced this issue Jan 17, 2025

fix: rihibot's record.launch now records the /imu messages #1000

ce1aa71

added the /imu topic to the list of topics to record in record.launch

Kazadhum added a commit that referenced this issue Jan 20, 2025

fix(rihibot): replaced gazebo imu plugin #1000

9dfda13

replaced the libgazebo_ros_imu plugin with the libgazebo_ros_imu_sensor plugin to get the measurements relative to imu_link instead of wrist_3_link

Kazadhum added a commit that referenced this issue Jan 20, 2025

style(rihibot): changed indentation for the imu plugin #1000

3b70043

Kazadhum added a commit that referenced this issue Jan 20, 2025

fix(rihibot): fixed IMU pose on the end-effector #1000

9243589

Kazadhum added a commit that referenced this issue Jan 20, 2025

feat(configuration): imu now supported in configuration #1000

448bc70

Added the option of using the 'imu' sensor modality in the configuration yaml file, so the configure script does not return an error;

Kazadhum added a commit that referenced this issue Jan 21, 2025

chore(config): added a warning for systems with imus #1000

c1d505c

added a warning for when a user tries configuring a system with an IMU, explaining it is still under development and not all calibration scripts are functional

Kazadhum mentioned this issue Jan 22, 2025

additional_data leads to configuration error #1005

Closed

Kazadhum added a commit that referenced this issue Jan 22, 2025

chore(rihibot): removed commented additional_data field #1000 #1005

4090257

Removed the commented additional_data field that was added for testing after deciding to not implement it.

Kazadhum added a commit that referenced this issue Jan 23, 2025

chore(rihibot): renamed imu sensor name #1000 #1005

1a39235

Renamed 'imu' sensor to 'imu_hand', to be more explicit.

Kazadhum mentioned this issue Jan 23, 2025

Inclusion of rihibot in the ATOM repo #1007

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

IMU Data Collection #1000

IMU Data Collection #1000

Kazadhum commented Jan 17, 2025 •

edited

Loading

Kazadhum commented Jan 20, 2025

Kazadhum commented Jan 20, 2025

miguelriemoliveira commented Jan 20, 2025

Kazadhum commented Jan 20, 2025

miguelriemoliveira commented Jan 20, 2025

Kazadhum commented Jan 20, 2025

Kazadhum commented Jan 20, 2025

Kazadhum commented Jan 20, 2025

Kazadhum commented Jan 20, 2025 •

edited

Loading

miguelriemoliveira commented Jan 20, 2025

miguelriemoliveira commented Jan 20, 2025

miguelriemoliveira commented Jan 20, 2025

Kazadhum commented Jan 20, 2025

miguelriemoliveira commented Jan 20, 2025

Kazadhum commented Jan 20, 2025

miguelriemoliveira commented Jan 20, 2025

Kazadhum commented Jan 22, 2025

miguelriemoliveira commented Jan 22, 2025

Kazadhum commented Jan 22, 2025

Kazadhum commented Jan 22, 2025

IMU Data Collection #1000

IMU Data Collection #1000

Comments

Kazadhum commented Jan 17, 2025 • edited Loading

TO-DO:

Kazadhum commented Jan 20, 2025

Kazadhum commented Jan 20, 2025

miguelriemoliveira commented Jan 20, 2025

Kazadhum commented Jan 20, 2025

miguelriemoliveira commented Jan 20, 2025

Kazadhum commented Jan 20, 2025

Kazadhum commented Jan 20, 2025

Kazadhum commented Jan 20, 2025

Kazadhum commented Jan 20, 2025 • edited Loading

miguelriemoliveira commented Jan 20, 2025

miguelriemoliveira commented Jan 20, 2025

miguelriemoliveira commented Jan 20, 2025

Kazadhum commented Jan 20, 2025

miguelriemoliveira commented Jan 20, 2025

Kazadhum commented Jan 20, 2025

miguelriemoliveira commented Jan 20, 2025

Kazadhum commented Jan 22, 2025

miguelriemoliveira commented Jan 22, 2025

Kazadhum commented Jan 22, 2025

Kazadhum commented Jan 22, 2025

Kazadhum commented Jan 17, 2025 •

edited

Loading

Kazadhum commented Jan 20, 2025 •

edited

Loading