Added the Madgwick filter to estimate the orientation data from the IMU inertial data.

launch/bringup.launch

@@ -20,7 +20,7 @@
   <arg name="side_left_us_topic_name"   default="/$(arg car_name)/sensors/range/side_left_sonar"/>
 
   <arg name="egomotion_config_file"     default="$(find iri_model_car_bringup)/config/egomotion_config.yaml"/>
-  <arg name="imu_topic_name"            default="/$(arg car_name)/sensors/imu_data"/>
+  <arg name="imu_topic_name"            default="/$(arg car_name)/sensors/imu"/>
   <arg name="magnetic_field_topic_name" default="/$(arg car_name)/sensors/magnetic_field"/>
   <arg name="encoders_topic_name"       default="/$(arg car_name)/encoders"/>
 
@@ -31,8 +31,8 @@
   <arg name="rplidar_config_file"       default="$(find iri_model_car_bringup)/config/rplidar_config.yaml" />
   <arg name="scan_topic_name"           default="/$(arg car_name)/sensors/scan"/>
 
-  <arg name="control_config_file"       default="$(find iri_model_car_control)/config/params.yaml"/>
-  <arg name="odometry_config_file"      default="$(find iri_model_car_odometry)/config/params_no_tf.yaml"/>
+  <arg name="control_config_file"       default="$(find iri_model_bringup)/config/control_config.yaml"/>
+  <arg name="odometry_config_file"      default="$(find iri_model_bringup)/config/odometry_config.yaml"/>
   <arg name="cmd_vel_mux_config_file"   default="$(find iri_model_car_launch)/config/default/mux.yaml"/>
   <arg name="local_ekf_config_file"     default="$(find iri_model_car_launch)/config/default/local_ekf_odom_imu.yaml"/>
 
@@ -97,11 +97,9 @@
     <arg name="launch_prefix"         value="$(arg launch_prefix)" />
   </include>
 
-<!--
   <include file="$(find iri_model_car_launch)/launch/local_ekf_loc.launch">
     <arg name="name"                   value="$(arg car_name)"/>
     <arg name="local_ekf_config_file"  value="$(arg local_ekf_config_file)"/>
   </include>
--->
 
 </launch>

launch/include/sensors.launch

@@ -17,7 +17,7 @@
   <arg name="side_left_us_topic_name"   default="/$(arg car_name)/sensors/range/side_left_sonar"/>
 
   <arg name="egomotion_config_file"     default="$(find iri_model_car_bringup)/config/egomotion_config.yaml"/>
-  <arg name="imu_topic_name"            default="$(arg car_name)/sensors/imu_data"/>
+  <arg name="imu_topic_name"            default="$(arg car_name)/sensors/imu"/>
   <arg name="magnetic_field_topic_name" default="$(arg car_name)/sensors/magnetic_field"/>
   <arg name="encoders_topic_name"       default="$(arg car_name)/encoders"/>
 
@@ -60,6 +60,32 @@
       <arg name="encoders_topic_name"          value="$(arg encoders_topic_name)"/>
     </include>
 
+    <node name="imu_filter"
+          pkg ="imu_filter_madgwick"
+          type="imu_filter_node"
+          output="screen">
+      <remap from="/$(arg car_name)/imu/data_raw"
+               to="/$(arg imu_topic_name)"/>
+      <remap from="/$(arg car_name)/imu/data"
+               to="/$(arg imu_topic_name)_data"/>
+      <param name="use_mag" value="false"/>
+      <param name="gain" value="0.1"/>
+      <param name="zeta" value="0.0"/>
+      <param name="mag_bias_x" value="0.0"/>
+      <param name="mag_bias_y" value="0.0"/>
+      <param name="mag_bias_z" value="0.0"/>
+      <param name="orientation_stddev" value="0.1"/>
+      <param name="world_frame" value="nwu"/>
+      <param name="use_magnetic_field_msg" value="false"/>
+      <param name="fixed_frame" value="/$(arg car_name)/imu_mpu9250"/>
+      <param name="publish_tf" value="false"/>
+      <param name="reverse_tf" value="false"/>
+      <param name="constant_dt" value="0.0"/>
+      <param name="publish_debug_topics" value="false"/>
+      <param name="stateless" value="false"/>
+      <param name="remove_gravity_vector" value="true"/>
+    </node>
+
     <group ns="sensors">
       <include file="$(find pylon_camera)/launch/pylon_camera_node.launch">
         <arg name="respawn"                         value="false" />