- <node pkg="razor_imu_9dof" type="imu_node.py" name="imu_node" output="screen" if="$(arg use_imu)">
- <rosparam file="$(find wild_thumper)/config/razor.yaml" command="load"/>
+ <node pkg="tinkerforge_imu_ros" type="tinkerforge_imu_ros" name="tinkerforge_imu_brick2" output="screen" respawn="true" if="$(arg use_imu)">
+ <param name="uid" value="6DdNSn"/>
+ <param name="frame_id" value="base_imu_link"/>
+ <param name="period_ms" value="20"/>
+ <!-- Observed orientation variance: 0.0 (10k samples)
+ Magnometer heading accuracy is +-2.5 deg => 0.088 rad
+ With heading accuracy as std dev, variance = 0.088^2 = 0.008 -->
+ <param name="variance_orientation" value="0.008"/>
+ <remap from="/tinkerforge_imu_brick2/imu" to="imu"/>
+ </node>
+
+ <include file="$(find yocs_cmd_vel_mux)/launch/cmd_vel_mux.launch">
+ <arg name="config_file" value="$(find wild_thumper)/config/cmd_vel_mux.yaml" />
+ <arg name="nodelet_manager_name" value="nodelet_manager" />
+ </include>
+
+ <node pkg="robot_localization" type="ekf_localization_node" name="ekf" clear_params="true" output="screen" if="$(arg use_imu)">
+ <rosparam command="load" file="$(find wild_thumper)/config/robot_localization.yaml"/>
+ <remap from="odometry/filtered" to="odom_combined"/>