switched INS heading init mode

added visualization for live NAND pos
added lat long conversion for live NAND pos
added passing location between westinhouse and stop sign
smoothed out outside curb
added debug topic for path planner

rb_ws/src/buggy/INS_params.yml

@@ -135,7 +135,7 @@ gnss1_rf_error_detection_data_rate : -1  # Rate of gnss1/rf_error_detection topi
 #     For GQ7 - in the vehicle frame wrt IMU origin (meters)
 #     For all other models - in the IMU frame wrt IMU origin (meters)
 #     Note: Make this as accurate as possible for good performance
-gnss1_antenna_offset : [-0.6007, 0.0, 0.2225]
+gnss1_antenna_offset : [-0.6007, 0.0, -0.2225]
 
 # GNSS2 settings are only applicable for multi-GNSS systems (e.g. GQ7)
 gnss2_data_rate : 100
@@ -155,7 +155,7 @@ gnss2_rf_error_detection_data_rate : -1  # Rate of gnss1/rf_error_detection topi
 
 # Antenna #2 lever arm offset vector (In the vehicle frame wrt IMU origin (meters) )
 # Note: Make this as accurate as possible for good performance
-gnss2_antenna_offset : [0.4968, 0.0, 0.3268]
+gnss2_antenna_offset : [0.4968, 0.0, -0.3268]
 
 # (GQ7 Only) Enable RTK dongle interface
 # Note: Enabling this will cause the node to publish on one of two topics depending
@@ -362,7 +362,7 @@ gpio4_pin_mode : 0
 #   1 - WGS84 Earth-fixed, earth centered (ECEF) position, velocity, attitude
 #   2 - WGS84 Latitude, Longitude, height above ellipsoid position, NED velocity and attitude
 filter_init_condition_src              : 0
-filter_auto_heading_alignment_selector : 1
+filter_auto_heading_alignment_selector : 3
 filter_init_reference_frame            : 2
 filter_init_position : [0.0, 0.0, 0.0]
 filter_init_velocity : [0.0, 0.0, 0.0]

rb_ws/src/buggy/launch/debug_steer.launch

@@ -9,6 +9,7 @@
     <!-- ENABLE AUTON -->
     <node name="debug_steer" pkg="buggy" type="debug_steer.py" output="screen"/>
 
+    <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
     <node name="bnyahaj" pkg="buggy" type="ros_to_bnyahaj.py" output="screen" args="--self_name SC --other_name NAND"/>
 
 </launch>

rb_ws/src/buggy/launch/ghost_auton.launch

@@ -1,32 +1,25 @@
 <!-- roslaunch buggy main.launch -->
 
 <launch>
-    <arg name="controller"      default="stanley" />
+    <arg name="controller"      default="mpc" />
     <arg name="start_dist"      default="0.0" />
-    <arg name="path"      default="garage.json" />
+    <arg name="path"      default="buggycourse_raceline.json" />
 
     <include file="$(find microstrain_inertial_driver)/launch/microstrain.launch">
         <arg name="params_file" value="/rb_ws/src/buggy/INS_params.yml"/>
     </include>
 
     <!-- run a sim with name = NAND at velocity ~= 0 -->
-    <arg name="starting_pose"    default="GARAGE" />
+    <arg name="starting_pose"    default="PASS_PT" />
     <arg name="velocity"         default="0.0001" />
     <arg name="buggy_name"       default="NAND" />
 
     <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
         args="$(arg starting_pose) $(arg velocity) $(arg buggy_name)"/>
 
-    <!-- <remap from="/SC/nav/odom" to="/nav/odom"/>
-    <remap from="/buggy/input/steering" to="/SC/buggy/input/steering"/> -->
+    <remap from="/SC/nav/odom" to="/nav/odom"/>
 
-    <!-- <node name="serial_node" pkg="rosserial_python" type="serial_node.py">
-        <param name="port" type="string" value="/dev/ttyUSB0"/>
-        <param name="baud" type="int" value="1000000"/>
-    </node> -->
-
-
-    <node name="bnyahaj" pkg="buggy" type="bnyahaj_to_ros.py" output="screen" args="--self_name SC --other_name NAND"/>
+    <node name="bnyahaj" pkg="buggy" type="ros_to_bnyahaj.py" output="screen" args="--self_name SC --other_name NAND"/>
 
     <node name="serial_node2" pkg="rosserial_python" type="serial_node.py">
         <param name="port" type="string" value="/dev/ttyACM1"/>

rb_ws/src/buggy/launch/main.launch

@@ -3,30 +3,22 @@
 <launch>
     <arg name="controller"      default="mpc" />
     <arg name="start_dist"      default="0.0" />
-    <arg name="path"      default="buggycourse_safe_1.json" />
+    <arg name="path"      default="buggycourse_raceline.json" />
 
     <include file="$(find microstrain_inertial_driver)/launch/microstrain.launch">
         <arg name="params_file" value="/rb_ws/src/buggy/INS_params.yml"/>
     </include>
 
-    <!-- <remap from="/SC/nav/odom" to="/nav/odom"/>
-    <remap from="/buggy/input/steering" to="/SC/buggy/input/steering"/> -->
+    <remap from="/SC/nav/odom" to="/nav/odom"/>
 
-    <!-- <node name="serial_node" pkg="rosserial_python" type="serial_node.py">
-        <param name="port" type="string" value="/dev/ttyUSB0"/>
-        <param name="baud" type="int" value="1000000"/> -->
-    <!-- </node> -->
     <node name="serial_node2" pkg="rosserial_python" type="serial_node.py">
         <param name="port" type="string" value="/dev/ttyACM1"/>
         <param name="baud" type="int" value="115200"/>
     </node>
     <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
     <node name="telematics" pkg="buggy" type="telematics.py" />
 
-
-    <node name="bnyahaj" pkg="buggy" type="bnyahaj_to_ros.py" output="screen" args="--self_name SC --other_name NAND"/>
-
-
+    <node name="bnyahaj" pkg="buggy" type="ros_to_bnyahaj.py" output="screen" args="--self_name SC --other_name NAND"/>
 
     <!-- ENABLE AUTON -->
     <!-- autonsystem args: controller start_dist path buggy_name is_sim -->