From d1d297026548f3fe7c12fd9239a48d182de7d7e1 Mon Sep 17 00:00:00 2001 From: Buggy Date: Fri, 20 Sep 2024 19:53:59 -0400 Subject: [PATCH] Mock Rolls Sept 19 --- .../Dockerfile | 0 python-requirements.txt | 1 + rb_ws/src/buggy/INS_params_v2.yml | 30 +++++++++---------- rb_ws/src/buggy/launch/sc-main.launch | 8 ++--- rb_ws/src/buggy/launch/sc-system.launch | 2 +- rb_ws/src/buggy/scripts/auton/autonsystem.py | 14 +++++++++ 6 files changed, 35 insertions(+), 20 deletions(-) rename {docker_tester => docker_tester_outofdate}/Dockerfile (100%) diff --git a/docker_tester/Dockerfile b/docker_tester_outofdate/Dockerfile similarity index 100% rename from docker_tester/Dockerfile rename to docker_tester_outofdate/Dockerfile diff --git a/python-requirements.txt b/python-requirements.txt index 254c5dac..a7b9fe22 100755 --- a/python-requirements.txt +++ b/python-requirements.txt @@ -1,4 +1,5 @@ matplotlib==3.1.2 +NavPy==1.0 numba==0.58.0 numpy<1.21.0 osqp==0.6.3 diff --git a/rb_ws/src/buggy/INS_params_v2.yml b/rb_ws/src/buggy/INS_params_v2.yml index a4640986..d0f0fe92 100644 --- a/rb_ws/src/buggy/INS_params_v2.yml +++ b/rb_ws/src/buggy/INS_params_v2.yml @@ -445,38 +445,38 @@ imu_pressure_data_rate : 0 # Rate of imu/pressure topic imu_wheel_speed_data_rate : 0 # Rate of imu/wheel_Speed topic # The speed at which the individual GNSS1 publishers will publish at. -gnss1_llh_position_data_rate : 2 # Rate of gnss_1/llh_position topic -gnss1_velocity_data_rate : 2 # Rate of gnss_1/velocity topic -gnss1_velocity_ecef_data_rate : 0 # Rate of gnss_1/velocity_ecef topic +gnss1_llh_position_data_rate : 10 # Rate of gnss_1/llh_position topic +gnss1_velocity_data_rate : 10 # Rate of gnss_1/velocity topic +gnss1_velocity_ecef_data_rate : 10 # Rate of gnss_1/velocity_ecef topic # Note: gnss1_odometry_earth_data_rate depends on the contents of this message. # If it is set to a higher value, this message will be published at that rate. -gnss1_odometry_earth_data_rate : 0 # Rate of gnss_1/odometry_earth topic +gnss1_odometry_earth_data_rate : 10 # Rate of gnss_1/odometry_earth topic gnss1_time_data_rate : 0 # Rate of gnss_1/time topic # The speed at which the individual GNSS2 publishers will publish at. -gnss2_llh_position_data_rate : 2 # Rate of gnss_2/llh_position topic +gnss2_llh_position_data_rate : 10 # Rate of gnss_2/llh_position topic gnss2_velocity_data_rate : 2 # Rate of gnss_2/velocity topic gnss2_velocity_ecef_data_rate : 0 # Rate of gnss_2/velocity_ecef topic # Note: gnss2_odometry_earth_data_rate depends on the contents of this message. # If it is set to a higher value, this message will be published at that rate. -gnss2_odometry_earth_data_rate : 0 # Rate of gnss_2/odometry_earth topic +gnss2_odometry_earth_data_rate : 10 # Rate of gnss_2/odometry_earth topic gnss2_time_data_rate : 0 # Rate of gnss_2/time topic # The speed at which the individual Filter publishers will publish at. -filter_human_readable_status_data_rate : 1 # Rate of ekf/status +filter_human_readable_status_data_rate : 100 # Rate of ekf/status filter_imu_data_rate : 0 # Rate of ekf/imu/data topic # Note: Both filter_odometry_earth_data_rate and filter_odometry_map_data_rate depend on the contents of this message. # If either are set to a higher value, this message will be published at that rate. -filter_llh_position_data_rate : 0 # Rate of ekf/llh_position topic +filter_llh_position_data_rate : 100 # Rate of ekf/llh_position topic filter_velocity_data_rate : 0 # Rate of ekf/velocity topic # Note: filter_odometry_map_data_rate depends on the contents of this message. # If either are set to a higher value, this message will be published at that rate. filter_velocity_ecef_data_rate : 0 # Rate of ekf/velocity_ecef topic # Note: filter_odometry_earth_data_rate depends on the contents of this message. # If either are set to a higher value, this message will be published at that rate. -filter_odometry_earth_data_rate : 25 # Rate of ekf/odometry_earth topic -filter_odometry_map_data_rate : 25 # Rate of ekf/odometry_map topic -filter_dual_antenna_heading_data_rate : 0 # Rate of ekf/dual_antenna_heading topic +filter_odometry_earth_data_rate : 100 # Rate of ekf/odometry_earth topic +filter_odometry_map_data_rate : 100 # Rate of ekf/odometry_map topic +filter_dual_antenna_heading_data_rate : 10 # Rate of ekf/dual_antenna_heading topic # Note: mip_filter_gnss_position_aiding_status_data_rate depends on the contents of this message. # If either are set to a higher value, this message will be published at that rate. @@ -484,16 +484,16 @@ filter_dual_antenna_heading_data_rate : 0 # Rate of ekf/dual_antenna_heading t mip_sensor_overrange_status_data_rate : 0 # Rate of mip/sensor/overrange_status topic mip_sensor_temperature_statistics_data_rate : 0 # Rate of mip/sensor/temperature_statistics topic -mip_gnss1_fix_info_data_rate : 0 # Rate of mip/gnss_1/fix_info topic +mip_gnss1_fix_info_data_rate : 10 # Rate of mip/gnss_1/fix_info topic mip_gnss1_sbas_info_data_rate : 0 # Rate of mip/gnss_1/sbas_info topic mip_gnss1_rf_error_detection_data_rate : 0 # Rate of mip/gnss_1/rf_error_detection -mip_gnss2_fix_info_data_rate : 0 # Rate of mip/gnss_1/fix_info topic +mip_gnss2_fix_info_data_rate : 10 # Rate of mip/gnss_1/fix_info topic mip_gnss2_sbas_info_data_rate : 0 # Rate of mip/gnss_2/sbas_info topic mip_gnss2_rf_error_detection_data_rate : 0 # Rate of mip/gnss_2/rf_error_detection -mip_filter_status_data_rate : 0 # Rate of mip/filter/status topic -mip_filter_gnss_position_aiding_status_data_rate : 0 # Rate of mip/filter/gnss_aiding_status +mip_filter_status_data_rate : 10 # Rate of mip/filter/status topic +mip_filter_gnss_position_aiding_status_data_rate : 10 # Rate of mip/filter/gnss_aiding_status # Note: filter_llh_position_data_rate depends on the contents of this message. # If it is set to a higher value, this message will be published at that rate. mip_filter_multi_antenna_offset_correction_data_rate : 0 # Rate of mip/filter/multi_antenna_offset_correction diff --git a/rb_ws/src/buggy/launch/sc-main.launch b/rb_ws/src/buggy/launch/sc-main.launch index a948dd09..5b3316a9 100755 --- a/rb_ws/src/buggy/launch/sc-main.launch +++ b/rb_ws/src/buggy/launch/sc-main.launch @@ -3,9 +3,9 @@ - + - + @@ -14,11 +14,11 @@ - + - + \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/sc-system.launch b/rb_ws/src/buggy/launch/sc-system.launch index de560f14..219c44b4 100644 --- a/rb_ws/src/buggy/launch/sc-system.launch +++ b/rb_ws/src/buggy/launch/sc-system.launch @@ -2,7 +2,7 @@ - + diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index 14a3c6cc..d3015a2c 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -21,6 +21,8 @@ from path_planner import PathPlanner from pose import Pose +import navpy + class AutonSystem: """ Top-level class for the RoboBuggy autonomous system @@ -68,6 +70,10 @@ def __init__(self, self.other_odom_msg = None self.use_gps_pos = False + #TODO: DOUBLE CONVERTING HERE, NOT A GOOD IDEA + rospy.Subscriber("/ekf/odometry_earth", Odometry, self.change_utm_latlon) + self.latlonodom = rospy.Publisher(self_name + "/nav/odom", Odometry, queue_size=1) + rospy.Subscriber(self_name + "/nav/odom", Odometry, self.update_self_odom) rospy.Subscriber(self_name + "/gnss1/odom", Odometry, self.update_self_odom_backup) rospy.Subscriber(self_name + "/nav/traj", TrajectoryMsg, self.update_traj) @@ -103,6 +109,14 @@ def __init__(self, self.profile = profile self.tick_caller() + def change_utm_latlon(self, msg): + new_msg = msg + northing = msg.pose.pose.position.x + easting = msg.pose.pose.position.y + lat, lon, alt = navpy.ecef2lla([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z]) + new_msg.pose.pose.position.x, new_msg.pose.pose.position.y = lon, lat + self.latlonodom.publish(new_msg) + # functions to read data from ROS nodes def update_use_gps(self, msg):