Add vectornav imu

.gitmodules

@@ -23,6 +23,9 @@
 [submodule "mil_common/perception/yolov7-ros"]
 	path = mil_common/perception/yolov7-ros
 	url = https://github.com/uf-mil/yolov7-ros.git
+[submodule "SubjuGator/drivers/vectornav"]
+	path = SubjuGator/drivers/vectornav
+	url = https://github.com/uf-mil/vectornav
 [submodule "mil_common/perception/vision_stack"]
 	path = mil_common/perception/vision_stack
 	url = https://github.com/uf-mil/vision_stack.git

SubjuGator/command/subjugator_launch/launch/subsystems/nav_box.launch

@@ -27,28 +27,25 @@
 
   <!-- IMU -->
   <group if="$(arg imu)" >
-    <node if="$(eval environment == 'real')" pkg="subjugator_launch" type="imu_conn" name="imu_conn" respawn="true" respawn_delay = "10"/>
-    <node if="$(eval environment == 'real')" pkg="nodelet" type="nodelet" name="imu_driver"
-          args="standalone adis16400_imu/nodelet">
-      <param name="port" type="string" value="/tmp/imu"/>
-      <param name="frame_id" type="string" value="/imu"/>
-    </node>
-
-    <node if="$(eval environment == 'real')"
-          pkg="nodelet" type="nodelet" name="magnetic_dynamic_compensation" args="standalone magnetic_dynamic_compensation/nodelet">
-      <remap from="/imu/mag" to="/imu/mag_lessraw"/>
-    </node>
-    <node if="$(eval environment == 'real')" pkg="nodelet" type="nodelet" name="magnetic_hardsoft_compensation" args="standalone magnetic_hardsoft_compensation/nodelet">
-      <remap from="/imu/mag_raw" to="/imu/mag_lessraw"/>
-      <param name="frame_id" type="string" value="/imu"/>
-      <rosparam>
-        scale:
-        - [0.9991765357958566, 0.006242798579443988, -0.008472478269327878]
-        - [0.006242798579443957, 1.0016179705091928, 0.0059841151097914345]
-        - [-0.008472478269327836, 0.0059841151097913, 0.999354597532967]
-        shift: [7.889247409445414e-06, 4.879179471165382e-06, 7.46017199298374e-06]
-      </rosparam>
-    </node>
+      <group if="$(eval environment == 'real')">
+          <include file="$(find vectornav)/launch/vn100_imu.launch">
+              <arg name="serial_port" value="/dev/serial/by-id/usb-FTDI_USB-RS232_Cable_AV0K7MTC-if00-port0" />
+          </include>
+        <node pkg="nodelet" type="nodelet" name="magnetic_dynamic_compensation" args="standalone magnetic_dynamic_compensation/nodelet">
+          <remap from="/imu/mag" to="/imu/mag_lessraw"/>
+        </node>
+        <node pkg="nodelet" type="nodelet" name="magnetic_hardsoft_compensation" args="standalone magnetic_hardsoft_compensation/nodelet">
+          <remap from="/imu/mag_raw" to="/imu/mag_lessraw"/>
+          <param name="frame_id" type="string" value="/imu"/>
+          <rosparam>
+            scale:
+            - [0.9991765357958566, 0.006242798579443988, -0.008472478269327878]
+            - [0.006242798579443957, 1.0016179705091928, 0.0059841151097914345]
+            - [-0.008472478269327836, 0.0059841151097913, 0.999354597532967]
+            shift: [7.889247409445414e-06, 4.879179471165382e-06, 7.46017199298374e-06]
+          </rosparam>
+          </node>
+      </group>
 
     <node pkg="mil_tools" type="mag_to_marker.py" name="magnetometer_vis"
           args="/imu/mag /imu/marker -l3" />

SubjuGator/command/subjugator_launch/package.xml

@@ -32,6 +32,7 @@
   <run_depend>mil_pneumatic_actuator</run_depend>
   <run_depend>mil_blueview_driver</run_depend>
   <run_depend>nav_tube_driver</run_depend>
+  <run_depend>vectornav</run_depend>
   <run_depend>subjugator_perception</run_depend>
   <run_depend>yolov7_ros</run_depend>
   <buildtool_depend>catkin</buildtool_depend>

scripts/install.sh

@@ -173,7 +173,10 @@ mil_system_install \
 	ros-noetic-teleop-twist-keyboard \
 	ros-noetic-ros-control \
 	ros-noetic-ros-controllers \
-	ros-noetic-tf2-tools
+	ros-noetic-tf2-tools \
+	ros-noetic-rviz-imu-plugin \
+	ros-noetic-imu-filter-madgwick \
+	ros-noetic-imu-transformer
 
 cat <<EOF
 $(color "$Pur")