Skip to content

Commit

Permalink
Mock Rolls Sept 19
Browse files Browse the repository at this point in the history
  • Loading branch information
Buggy committed Sep 20, 2024
1 parent 4eb240c commit d1d2970
Show file tree
Hide file tree
Showing 6 changed files with 35 additions and 20 deletions.
0 docker_tester/Dockerfile → docker_tester_outofdate/Dockerfile
View file
File renamed without changes.
1 change: 1 addition & 0 deletions python-requirements.txt
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
matplotlib==3.1.2
NavPy==1.0
numba==0.58.0
numpy<1.21.0
osqp==0.6.3
Expand Down
30 changes: 15 additions & 15 deletions rb_ws/src/buggy/INS_params_v2.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -445,55 +445,55 @@ 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.

# The speed at which the individual MIP publishers will publish at.
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
Expand Down
8 changes: 4 additions & 4 deletions rb_ws/src/buggy/launch/sc-main.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,9 @@
<launch>
<arg name="controller" default="stanley" />
<arg name="start_dist" default="0.0" />
<arg name="path" default="buggycourse_safe_1.json" />
<arg name="path" default="buggycourse_raceline.json" />

<remap from="/SC/nav/odom" to="/ekf/odometry_earth"/>
<!-- <remap from="/SC/nav/odom" to="/ekf/odometry_earth"/> -->
<node name="sc_rolling_sanity_check" pkg="buggy" type="rolling_sanity_check.py" output="screen" args="SC"/>

<!-- ENABLE AUTON -->
Expand All @@ -14,11 +14,11 @@
<arg name="NAND_exist" default="true"/>
<group if="$(arg NAND_exist)">
<!-- Run the simulation with NAND -->
<arg name="autonsystem_args" value="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC --other_name NAND --left_curb curb.json" />
<arg name="autonsystem_args" value="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC --other_name NAND --left_curb buggycourse_curb.json" />
<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" value="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC --left_curb curb.json" />
<arg name="autonsystem_args" value="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC --left_curb buggycourse_curb.json" />
<node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" args="$(arg autonsystem_args)"/>
</group>
</launch>
2 changes: 1 addition & 1 deletion rb_ws/src/buggy/launch/sc-system.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

<launch>
<include file="$(find microstrain_inertial_driver)/launch/microstrain.launch">
<arg name="params_file" value="/rb_ws/src/buggy/INS_params.yml"/>
<arg name="params_file" value="/rb_ws/src/buggy/INS_params_v2.yml"/>
</include>

<node name="bnyahaj" pkg="buggy" type="ros_to_bnyahaj.py" respawn="true" output="screen" args="--self_name SC --other_name NAND --teensy_name ttyUSB0"/>
Expand Down
14 changes: 14 additions & 0 deletions rb_ws/src/buggy/scripts/auton/autonsystem.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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):
Expand Down

0 comments on commit d1d2970

Please sign in to comment.