Changes made during mock rolls, debugging sessions that need to be pr…

…eserved (#47) * updated path planner and autonsystem, added option for building trajectory from __init__ parameter of trajectory.py * [BETA] obstacle avoidance * added halfplane constraints to MPC * Switched path planning to use offsets along the path normal * Cleaned up unused code, added comments * CI (#19) * pylint fixes more pylint fixes pylint fixes for 2d sim and other existing modules pylint fixes yet again pylint fixes yet again #2 * removed ROSbags * added changes requested * Updated main.launch Co-authored-by: Mehul Goel <[email protected]> Co-authored-by: TiaSinghania <[email protected]> Co-authored-by: PatXue <[email protected]> * fixed main.launch typo * Fixed topic remapping * fixed topic remp, added dummy subscriber for debug * Fixed heading gain, added ghost NAND feature * removed dummy node (was used for debug) * Commented out MPC halfplane constraints, Updated sim launch file * Added exception for path run out for MPC, stanley Added more mock roll paths and starting poses expanded MPC traj index search window * Added debug steer routine * updated the cut path * changed controller type * Added debug topic for filter <-> gnss distance * Turned on debug logging for INS * set debug to true in INS_params.yml * removed duplicate publish_rtk_err.py fixed typo in publish_rtk_err.py variable naming * Fixed pylint warnings * removed duplicate publish_rtk_err.py * removed unusued vars in MPC --------- Co-authored-by: Christian Luu <[email protected]> Co-authored-by: Mehul Goel <[email protected]> Co-authored-by: TiaSinghania <[email protected]> Co-authored-by: PatXue <[email protected]> Co-authored-by: Buggy <[email protected]>
CMU-Robotics-Club · Mar 13, 2024 · 26935a2 · 26935a2
1 parent 3af4032
commit 26935a2
Show file tree

Hide file tree

Showing 15 changed files with 403 additions and 64 deletions.
diff --git a/rb_ws/src/buggy/INS_params.yml b/rb_ws/src/buggy/INS_params.yml
@@ -3,22 +3,22 @@
 # Note: Feature support is device-dependent and some of the following settings may have no affect on your device.
 # Please consult your device's documentation for supported features
 
-# ****************************************************************** 
+# ******************************************************************
 # NOTE: This file is formatted to work with ROS and will not work if specified as the params_file argument in ROS2.
 #       If you want to override parameters for ROS2, start with https://github.com/LORD-MicroStrain/microstrain_inertial/blob/ros2/microstrain_inertial_driver/config/empty.yml
-# ****************************************************************** 
+# ******************************************************************
 
-# ****************************************************************** 
-# General Settings 
-# ****************************************************************** 
+# ******************************************************************
+# General Settings
+# ******************************************************************
 
 # port is the main port that the device will communicate over. For all devices except the GQ7, this is the only available port.
 # aux_port is only available for the GQ7 and is only needed when streaming RTCM corrections to the device from ROS, or if you want to publish NMEA sentences from this node
 port        : "/dev/ttyACM0"
 aux_port    : "/dev/null"
 baudrate    : 115200
-debug       : False
-diagnostics : False
+debug       : True
+diagnostics : True
 
 # If set to true, this will configure the requested baudrate on the device.
 # Note that this will be set on both USB and serial, but will only actually affect the baudrate of a serial connection.
@@ -83,18 +83,18 @@ raw_file_enable : False
 #      true  - Request the additional channels (please see notes below!)
 #
 #      Notes: **We recommend only enabling this feature when specifically requested by Microstrain.**
-#      
+#
 #      Including this feature increases communication bandwidth requirements significantly... for serial data connections
-#      please ensure the baudrate is sufficient for the added data channels.     
+#      please ensure the baudrate is sufficient for the added data channels.
 raw_file_include_support_data : False
 
 # The directory to store the raw data file (no trailing '/')
 raw_file_directory : "/home/your_name"
 
 
-# ****************************************************************** 
-# IMU Settings 
-# ****************************************************************** 
+# ******************************************************************
+# IMU Settings
+# ******************************************************************
 imu_data_rate : 100
 
 # The speed at which the individual IMU publishers will publish at.
@@ -106,15 +106,15 @@ imu_gps_corr_data_rate         : -1  # Rate of gps_corr topic
                                      # Note that for philo devices (GX5, CV5, CX5), this will be published at the highest IMU data rate if "use_device_timestamp" is set to true
 imu_overrange_status_data_rate : -1  # Rate of imu/overrange_status topic
 
-# Static IMU message covariance values (the device does not generate these) 
-# Since internally these are std::vector we need to use the rosparam tags 
+# Static IMU message covariance values (the device does not generate these)
+# Since internally these are std::vector we need to use the rosparam tags
 imu_orientation_cov : [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
 imu_linear_cov      : [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
 imu_angular_cov     : [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
 
-# ****************************************************************** 
-# GNSS Settings (only applicable for devices with GNSS) 
-# ****************************************************************** 
+# ******************************************************************
+# GNSS Settings (only applicable for devices with GNSS)
+# ******************************************************************
 gnss1_data_rate : 100
 
 # The speed at which the individual GNSS1 publishers will publish at.
@@ -137,7 +137,7 @@ gnss1_rf_error_detection_data_rate : -1  # Rate of gnss1/rf_error_detection topi
 #     Note: Make this as accurate as possible for good performance
 gnss1_antenna_offset : [-0.6007, 0.0, 0.2225]
 
-# GNSS2 settings are only applicable for multi-GNSS systems (e.g. GQ7) 
+# GNSS2 settings are only applicable for multi-GNSS systems (e.g. GQ7)
 gnss2_data_rate : 100
 
 # The speed at which the individual GNSS2 publishers will publish at.
@@ -154,7 +154,7 @@ gnss2_sbas_info_data_rate          : -1  # Rate of gnss2/sbas_info topic
 gnss2_rf_error_detection_data_rate : -1  # Rate of gnss1/rf_error_detection topic
 
 # Antenna #2 lever arm offset vector (In the vehicle frame wrt IMU origin (meters) )
-# Note: Make this as accurate as possible for good performance 
+# Note: Make this as accurate as possible for good performance
 gnss2_antenna_offset : [0.4968, 0.0, 0.3268]
 
 # (GQ7 Only) Enable RTK dongle interface
@@ -181,9 +181,9 @@ rtcm_topic     : "/rtcm"
 #       this will publish NMEA sentences from both the main and aux port on the same topic.
 publish_nmea   : False
 
-# ****************************************************************** 
-# Kalman Filter Settings (only applicable for devices with a Kalman Filter) 
-# ****************************************************************** 
+# ******************************************************************
+# Kalman Filter Settings (only applicable for devices with a Kalman Filter)
+# ******************************************************************
 filter_data_rate : 50
 
 # The speed at which the individual Filter publishers will publish at.
@@ -221,35 +221,35 @@ filter_reset_after_config : True
 # Controls if the Kalman filter will auto-init or requires manual initialization
 filter_auto_init : True
 
-# (All, except -10, and -15 products) Declination Source 1 = None, 2 = magnetic model, 3 = manual 
+# (All, except -10, and -15 products) Declination Source 1 = None, 2 = magnetic model, 3 = manual
 # Note: When using a CV7, this MUST be changed to either 1, or 3 or the node will not start
 filter_declination_source : 2
 filter_declination        : 0.23
 
-# (All, except GQ7, CV7, -10, and -15 products) Heading Source 0 = None, 1 = magnetic, 2 = GNSS velocity (note: see manual for limitations)  
+# (All, except GQ7, CV7, -10, and -15 products) Heading Source 0 = None, 1 = magnetic, 2 = GNSS velocity (note: see manual for limitations)
 # Note: When using a -10/-AR product. This MUST be set to 0 or the node will not start
 filter_heading_source : 1
 
-# (GX5 and previous,-45 models only) Dynamics Mode 1 = Portable (default), 2 = Automotive, 3 = Airborne (<2Gs), 4 = Airborne High G (<4Gs) 
+# (GX5 and previous,-45 models only) Dynamics Mode 1 = Portable (default), 2 = Automotive, 3 = Airborne (<2Gs), 4 = Airborne High G (<4Gs)
 filter_dynamics_mode : 1
 
 # Controls what kind of linear acceleration data is used in the Filter IMU message.
 #      If this is set to true, the acceleration will not factor out gravity, if set to false gravity will be filtered out of the linear acceleration.
 filter_use_compensated_accel : True
 
-# ZUPT control 
+# ZUPT control
 filter_velocity_zupt_topic : "/moving_vel"
 filter_angular_zupt_topic  : "/moving_ang"
 filter_velocity_zupt       : True
 filter_angular_zupt        : True
 
 # (GQ7/CV7 full support, GX5-45 limited support) Adaptive filter settings
 #      Adaptive level: 0 - off, 1 - Conservative, 2 = Moderate (default), 3 = agressive
-#      Time limit: Max duration of measurement rejection prior to recovery, in milliseconds - default = 15000 
+#      Time limit: Max duration of measurement rejection prior to recovery, in milliseconds - default = 15000
 filter_adaptive_level         : 2
 filter_adaptive_time_limit_ms : 15000
 
-# (GQ7/CV7 only) Aiding measurement control 
+# (GQ7/CV7 only) Aiding measurement control
 filter_enable_gnss_pos_vel_aiding     : True
 filter_enable_gnss_heading_aiding     : True
 filter_enable_altimeter_aiding        : False

diff --git a/rb_ws/src/buggy/launch/debug_filter.launch b/rb_ws/src/buggy/launch/debug_filter.launch
@@ -0,0 +1,33 @@
+<!-- roslaunch buggy main.launch -->
+
+<launch>
+    <arg name="controller"      default="stanley" />
+    <arg name="start_dist"      default="0.0" />
+    <arg name="path"      default="buggycourse_safe_1.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"/>
+
+    <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="publish_rtk_err" pkg="buggy" type="publish_rtk_err.py" output="screen"/>
+
+
+    <!-- ENABLE AUTON -->
+    <!-- autonsystem args: controller start_dist path buggy_name is_sim -->
+    <!-- Conditional Launch Files, depending on if NAND Exists or not -->
+    <arg name="autonsystem_args" default="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC" />
+    <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" args="$(arg autonsystem_args)"/>
+</launch>
diff --git a/rb_ws/src/buggy/launch/debug_steer.launch b/rb_ws/src/buggy/launch/debug_steer.launch
@@ -0,0 +1,14 @@
+<!-- roslaunch buggy debug_steer.launch -->
+
+<launch>
+    <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>
+    <!-- ENABLE AUTON -->
+    <node name="debug_steer" pkg="buggy" type="debug_steer.py" output="screen"/>
+</launch>
diff --git a/rb_ws/src/buggy/launch/ghost_auton.launch b/rb_ws/src/buggy/launch/ghost_auton.launch
@@ -0,0 +1,37 @@
+<!-- roslaunch buggy main.launch -->
+
+<launch>
+    <arg name="controller"      default="stanley" />
+    <arg name="start_dist"      default="0.0" />
+    <arg name="path"      default="buggycourse_safe_1.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="UC_TO_PURNELL" />
+    <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"/>
+
+    <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" />
+
+    <!-- Run the simulation with NAND -->
+    <arg name="autonsystem_args" default="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC --other_name NAND" />
+    <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" args="$(arg autonsystem_args)"/>
+</launch>
diff --git a/rb_ws/src/buggy/launch/main.launch b/rb_ws/src/buggy/launch/main.launch
@@ -1,12 +1,17 @@
 <!-- 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="buggycourse_safe_1.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"/>
+
     <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"/>
@@ -18,7 +23,20 @@
     <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
     <node name="telematics" pkg="buggy" type="telematics.py" />
 
+
+
+
     <!-- ENABLE AUTON -->
     <!-- autonsystem args: controller start_dist path buggy_name is_sim -->
-    <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" args="$(arg controller) $(arg start_dist) $(arg path) SC False"/>
-</launch>
+    <!-- Conditional Launch Files, depending on if NAND Exists or not -->
+    <arg name="NAND_exist" default="true"/>
+    <group if="$(arg NAND_exist)">
+        <!-- Run the simulation with NAND -->
+        <arg name="autonsystem_args" default="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC --other_name NAND" />
+        <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" args="$(arg autonsystem_args)"/>
+    </group>
+    <group unless="$(arg NAND_exist)">
+        <arg name="autonsystem_args" default="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC" />
+        <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" args="$(arg autonsystem_args)"/>
+    </group>
+</launch>
diff --git a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
@@ -1,13 +1,16 @@
 <launch>
-    <arg name="sc_start_pos"       default="Hill1_SC" />
+    <arg name="sc_controller"     default="stanley" />
+    <arg name="sc_path"           default="buggycourse_safe_1.json" />
+
+    <arg name="sc_start_pos"    default="Hill1_SC" />
     <arg name="sc_velocity"         default="15.0" />
 
     <arg name="nand_controller"     default="stanley" />
     <arg name="nand_path"           default="buggycourse_safe_1.json" />
     <arg name="nand_start_pos"     default="Hill1_NAND" />
     <arg name="nand_velocity"       default="10.0" />
 
-    <arg name="manual_vel"          default="true" />
+    <arg name="manual_vel"          default="false" />
     <arg name="auto_vel"            default="false" />
 
     <group if="$(arg auto_vel)">
@@ -33,14 +36,14 @@
         args="$(arg nand_start_pos) $(arg nand_velocity) NAND"/>
 
     <!-- NAND is not aware of SC -->
-    <arg name="nand_autonsystem_args" default="--controller stanley --dist 0.0 --traj buggycourse_safe_1.json --self_name NAND" />
+    <arg name="nand_autonsystem_args" default="--controller $(arg nand_controller) --dist 0.0 --traj $(arg nand_path) --self_name NAND" />
     <node name="nand_auton_system" pkg="buggy" type="autonsystem.py" output="screen"
         args="$(arg nand_autonsystem_args)"/>
 
     <node name="sc_sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
         args="$(arg sc_start_pos) $(arg sc_velocity) SC"/>
 
-    <arg name="sc_autonsystem_args" default="--controller stanley --dist 0.0 --traj buggycourse_safe_1.json --self_name SC --other_name NAND" />
+    <arg name="sc_autonsystem_args" default="--controller $(arg sc_controller) --dist 0.0 --traj $(arg sc_path) --self_name SC --other_name NAND" />
     <node name="sc_auton_system" pkg="buggy" type="autonsystem.py" output="screen"
         args="$(arg sc_autonsystem_args)"/>
 

diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch
@@ -6,7 +6,7 @@
 
     <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
 
-    <arg name="manual_vel"          default="true" />
+    <arg name="manual_vel"          default="false" />
     <arg name="auto_vel"            default="false" />
 
     <group if="$(arg auto_vel)">

diff --git a/rb_ws/src/buggy/paths/UC_to_purnell.json b/rb_ws/src/buggy/paths/UC_to_purnell.json
@@ -0,0 +1,26 @@
+[
+  {
+    "key": "df250f90-3cc4-4eb8-991c-cb88fa03f774",
+    "lat": 40.44295800415301,
+    "lon": -79.9426950522631,
+    "active": false
+  },
+  {
+    "key": "93e8d2e2-43e1-460f-9ad9-1bf272dd6b4d",
+    "lat": 40.4430150847491,
+    "lon": -79.94294473319194,
+    "active": false
+  },
+  {
+    "key": "7fd4962e-8c0a-4780-bf19-0d3fcc597e1f",
+    "lat": 40.443081717291996,
+    "lon": -79.94324300478857,
+    "active": false
+  },
+  {
+    "key": "00677a33-8f23-41f5-8287-222d2d1efd40",
+    "lat": 40.44311914802749,
+    "lon": -79.9434202506794,
+    "active": false
+  }
+]