diff --git a/README.md b/README.md index 67387a90..25033ee5 100755 --- a/README.md +++ b/README.md @@ -69,13 +69,13 @@ A complete re-write of the old RoboBuggy. ## 2D Simulation - Boot up the docker container -- Run `roslaunch buggy sim_2d_single.launch` to simulate 1 buggy -- See `rb_ws/src/buggy/launch/sim_2d_single.launch` to view all available launch options -- Run `roslaunch buggy sim_2d_2buggies.launch` to simulate 2 buggies +- Run `roslaunch buggy sim_single.launch` to simulate 1 buggy +- See `rb_ws/src/buggy/launch/sim_single.launch` to view all available launch options +- Run `roslaunch buggy sim_2buggies.launch` to simulate 2 buggies Screenshot 2023-11-13 at 3 18 30 PM -- See `rb_ws/src/buggy/launch/sim_2d_2buggies.launch` to view all available launch options +- See `rb_ws/src/buggy/launch/sim_2buggies.launch` to view all available launch options - The buggy starting positions can be changed using the `sc_start_pos` and `nand_start_pos` arguments (can pass as a key to a dictionary of preset start positions in engine.py, a single float for starting distance along planned trajectory, or 3 comma-separated floats (utm east, utm north, and heading)) - To prevent topic name collision, a topic named `t` associated with buggy named `x` have format `x/t`. The names are `SC` and `Nand` in the 2 buggy simulator. In the one buggy simulator, the name can be defined as a launch arg. - See [**Foxglove Visualization**](#foxglove-visualization) for visualizing the simulation. Beware that since topic names are user-defined, you will need to adjust the topic names in each panel. diff --git a/rb_ws/src/buggy/config/sim_2buggies.yaml b/rb_ws/src/buggy/config/sim_2buggies.yaml new file mode 100644 index 00000000..aba971f0 --- /dev/null +++ b/rb_ws/src/buggy/config/sim_2buggies.yaml @@ -0,0 +1,22 @@ +# SHORT CIRCUIT +SC_start_pos: "Hill1_SC" +SC_velocity: 15.0 + + +## Auton Arguments +SC_controller: "stanley" +SC_dist: 0.0 +SC_traj: "buggycourse_safe.json" +SC_self_name: "SC" +SC_other_name: "NAND" + +# NAND +NAND_start_pos: "Hill1_NAND" +NAND_velocity: 7.0 + +## Auton Arguments +NAND_controller: "stanley" +NAND_dist: 0.0 +NAND_traj: "buggycourse_safe.json" +NAND_self_name: "NAND" +NAND_other_name: NONE \ No newline at end of file diff --git a/rb_ws/src/buggy/config/sim_single.yaml b/rb_ws/src/buggy/config/sim_single.yaml index 57e1a2ea..bf3afc12 100644 --- a/rb_ws/src/buggy/config/sim_single.yaml +++ b/rb_ws/src/buggy/config/sim_single.yaml @@ -1,15 +1,11 @@ -start_pos: "Hill1_SC" -velocity: 15.0 -buggy_name: "SC" - -manual_vel: false -auto_vel: false - - -# Auton Arguments -controller: "stanley" -dist: 0.0 -traj: "buggycourse_safe.json" -self_name: "SC" -other_name: NONE +# SHORT CIRCUIT +SC_start_pos: "Hill1_SC" +SC_velocity: 7.0 + +## Auton Arguments +SC_controller: "stanley" +SC_dist: 0.0 +SC_traj: "buggycourse_safe.json" +SC_self_name: "SC" +SC_other_name: NONE diff --git a/rb_ws/src/buggy/launch/debug_filter.launch b/rb_ws/src/buggy/launch/debug_filter_unsupported.launch similarity index 100% rename from rb_ws/src/buggy/launch/debug_filter.launch rename to rb_ws/src/buggy/launch/debug_filter_unsupported.launch diff --git a/rb_ws/src/buggy/launch/ghost_auton.launch b/rb_ws/src/buggy/launch/ghost_auton_unsupported.launch similarity index 100% rename from rb_ws/src/buggy/launch/ghost_auton.launch rename to rb_ws/src/buggy/launch/ghost_auton_unsupported.launch diff --git a/rb_ws/src/buggy/launch/pure_pursuit.launch b/rb_ws/src/buggy/launch/pure_pursuit.launch deleted file mode 100644 index 041af270..00000000 --- a/rb_ws/src/buggy/launch/pure_pursuit.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/sim_2buggies.launch b/rb_ws/src/buggy/launch/sim_2buggies.launch index f94fb307..fffe2bb9 100755 --- a/rb_ws/src/buggy/launch/sim_2buggies.launch +++ b/rb_ws/src/buggy/launch/sim_2buggies.launch @@ -1,13 +1,7 @@ - - - + - - - - - + @@ -15,38 +9,30 @@ + output="screen" args="NAND"/> + output="screen" args="SC"/> + output="screen" args="SC"/> + output="screen" args="NAND"/> - - - + - - - + args="NAND"/> - - - + args="NAND"/> + - - - + args="SC"/> + + \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/sim_single.launch b/rb_ws/src/buggy/launch/sim_single.launch index 1c7d2c27..3f2d5200 100755 --- a/rb_ws/src/buggy/launch/sim_single.launch +++ b/rb_ws/src/buggy/launch/sim_single.launch @@ -3,25 +3,29 @@ - - + + - - - - - - - - + + + + + - + + + + + - + - + + + + \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py index 2caa89dd..437af64f 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py +++ b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py @@ -6,8 +6,8 @@ class Controller: def __init__(self, buggy_name): - self.steering_publisher = rospy.Publisher(buggy_name + "/input/steering", Float64, queue_size=10) - self.velocity_publisher = rospy.Publisher(buggy_name + "/velocity", Float64, queue_size=10) + self.steering_publisher = rospy.Publisher("/input/steering", Float64, queue_size=10) + self.velocity_publisher = rospy.Publisher("/velocity", Float64, queue_size=10) self.steering_angle = 0 self.velocity = 0 diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py index b11f54d9..3aceb81a 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/engine.py +++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py @@ -32,27 +32,27 @@ def __init__(self, start_pos: str, velocity: float, buggy_name: str): heading (float): degrees start heading of buggy """ # for X11 matplotlib (direction included) - self.plot_publisher = rospy.Publisher(buggy_name + "/sim_2d/utm", Pose, queue_size=1) + self.plot_publisher = rospy.Publisher("/sim_2d/utm", Pose, queue_size=1) # simulate the INS's outputs (noise included) - self.pose_publisher = rospy.Publisher(buggy_name + "/nav/odom", Odometry, queue_size=1) + self.pose_publisher = rospy.Publisher("/nav/odom", Odometry, queue_size=1) self.steering_subscriber = rospy.Subscriber( - buggy_name + "/buggy/input/steering", Float64, self.update_steering_angle + "/buggy/input/steering", Float64, self.update_steering_angle ) # To read from velocity self.velocity_subscriber = rospy.Subscriber( - buggy_name + "/velocity", Float64, self.update_velocity + "/velocity", Float64, self.update_velocity ) # to plot on Foxglove (no noise) self.navsatfix_publisher = rospy.Publisher( - buggy_name + "/state/pose_navsat", NavSatFix, queue_size=1 + "/state/pose_navsat", NavSatFix, queue_size=1 ) if Simulator.NOISE: # to plot on Foxglove (with noise) self.navsatfix_noisy_publisher = rospy.Publisher( - buggy_name + "/state/pose_navsat_noisy", NavSatFix, queue_size=1 + "/state/pose_navsat_noisy", NavSatFix, queue_size=1 ) # (UTM east, UTM north, HEADING(degs)) @@ -329,14 +329,13 @@ def loop(self): if __name__ == "__main__": rospy.init_node("sim_2d_engine") - print("sim 2d eng args:") - print(sys.argv) + print("sim 2d eng args: ", sys.argv) + self_name = sys.argv[1] - start_pos = rospy.get_param("/start_pos") - velocity = float(rospy.get_param("/velocity")) - buggy_name = rospy.get_param("/buggy_name") + start_pos = rospy.get_param("/" + self_name + "_start_pos") + velocity = float(rospy.get_param("/" + self_name + "_velocity")) - sim = Simulator(start_pos, velocity, buggy_name) + sim = Simulator(start_pos, velocity, self_name) for i in range(100): sim.publish() rospy.sleep(0.01) @@ -345,7 +344,7 @@ def loop(self): # so that auton stack has time to initialize # before buggy moves print("SIM PUBLISHED", rospy.get_time()) - if buggy_name == "SC": + if self_name == "SC": rospy.sleep(15.0) print("SIM STARTED STEPPING", rospy.get_time()) sim.loop() diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py index f6d8486f..47eaeaf9 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py +++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py @@ -35,9 +35,9 @@ def step(self): if __name__ == "__main__": rospy.init_node("velocity_ui") - init_vel = float(rospy.get_param("/velocity")) - buggy_name = rospy.get_param("/SC") - vel = VelocityUI(init_vel, buggy_name) + self_name = sys.argv[1] + init_vel = float(rospy.get_param("/" + self_name + "_velocity")) + vel = VelocityUI(init_vel, self_name) rate = rospy.Rate(100) while not rospy.is_shutdown(): vel.step() diff --git a/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py b/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py index fc26cb14..9a824647 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py +++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_updater.py @@ -63,10 +63,10 @@ def step(self): if __name__ == "__main__": rospy.init_node("vel_updater") + self_name = sys.argv[1] - init_vel = float(rospy.get_param("/velocity")) - buggy_name = rospy.get_param("/SC") - vel = VelocityUpdater(init_vel, buggy_name) + init_vel = float(rospy.get_param("/" + self_name + "_velocity")) + vel = VelocityUpdater(init_vel, self_name) rate = rospy.Rate(vel.RATE) while not rospy.is_shutdown(): diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index 25c06657..b2468fca 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -2,6 +2,7 @@ import argparse from threading import Lock +import sys import threading import rospy @@ -68,12 +69,12 @@ def __init__(self, self.other_odom_msg = None self.use_gps_pos = False - 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) + rospy.Subscriber("/nav/odom", Odometry, self.update_self_odom) + rospy.Subscriber("/gnss1/odom", Odometry, self.update_self_odom_backup) + rospy.Subscriber("/nav/traj", TrajectoryMsg, self.update_traj) # to report if the filter position has separated (so we need to use the antenna position) - rospy.Subscriber(self_name + "/debug/filter_gps_seperation_status", Bool, self.update_use_gps) + rospy.Subscriber("/debug/filter_gps_seperation_status", Bool, self.update_use_gps) if self.has_other_buggy: rospy.Subscriber(other_name + "/nav/odom", Odometry, self.update_other_odom) @@ -82,16 +83,16 @@ def __init__(self, ) self.init_check_publisher = rospy.Publisher( - self_name + "/debug/init_safety_check", Bool, queue_size=1 + "/debug/init_safety_check", Bool, queue_size=1 ) self.steer_publisher = rospy.Publisher( - self_name + "/buggy/input/steering", Float64, queue_size=1 + "/buggy/input/steering", Float64, queue_size=1 ) self.heading_publisher = rospy.Publisher( - self_name + "/auton/debug/heading", Float32, queue_size=1 + "/auton/debug/heading", Float32, queue_size=1 ) self.distance_publisher = rospy.Publisher( - self_name + "/auton/debug/distance", Float64, queue_size=1 + "/auton/debug/distance", Float64, queue_size=1 ) self.controller_rate = 100 @@ -264,23 +265,23 @@ def planner_tick(self): if __name__ == "__main__": rospy.init_node("auton_system") - ctrl = rospy.get_param("/controller") - start_dist = rospy.get_param("/dist") - traj = rospy.get_param("/traj") - self_name = rospy.get_param("/self_name") + self_name = sys.argv[1] + ctrl = rospy.get_param("/" + self_name + "_controller") + start_dist = rospy.get_param("/" + self_name + "_dist") + traj = rospy.get_param("/" + self_name + "_traj") - if rospy.has_param("/other_name"): - other_name = rospy.get_param("/other_name") + if rospy.has_param("/" + self_name + "_other_name"): + other_name = rospy.get_param("/" + self_name + "_other_name") else: other_name = None - if rospy.has_param("/profile"): - profile = rospy.get_param("/profile") + if rospy.has_param("/" + self_name + "_profile"): + profile = rospy.get_param("/" + self_name + "_profile") else: profile = None - if rospy.has_param("/left_curb"): - left_curb_file = rospy.get_param("/left_curb") + if rospy.has_param("/" + self_name + "_left_curb"): + left_curb_file = rospy.get_param("/" + self_name + "_left_curb") else: left_curb_file = "" diff --git a/rb_ws/src/buggy/scripts/auton/buggystate.py b/rb_ws/src/buggy/scripts/auton/buggystate.py index f8064768..931cb0b7 100644 --- a/rb_ws/src/buggy/scripts/auton/buggystate.py +++ b/rb_ws/src/buggy/scripts/auton/buggystate.py @@ -23,7 +23,7 @@ class BuggyState: """ - def __init__(self, name = "sc"): + def __init__(self, name = "SC"): self.filter_odom : ROSOdom = None self.gnss_1 : ROSOdom = None self.gnss_2 : ROSOdom = None @@ -31,9 +31,9 @@ def __init__(self, name = "sc"): # to report if the filter position has separated (so we need to break) - rospy.Subscriber(name + "/debug/filter_gps_seperation_status", Bool, self.update_use_gps) + rospy.Subscriber("/debug/filter_gps_seperation_status", Bool, self.update_use_gps) - rospy.Subscriber(name + "/nav/odom", ROSOdom, self.update_odom) + rospy.Subscriber("/nav/odom", ROSOdom, self.update_odom) rospy.Subscriber("/gnss1/odom", ROSOdom, self.update_gnss1) rospy.Subscriber("/gnss2/odom", ROSOdom, self.update_gnss2) rospy.Subscriber("/gnss1/fix_info", GNSSFixInfo, self.update_gnss1_fix) diff --git a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py index eb858a0b..37691fe2 100644 --- a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py +++ b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py @@ -98,10 +98,10 @@ def __init__(self, buggy_name : str, start_index=0, ref_trajectory=None, ROS=Fal self.solver: osqp.OSQP = None self.debug_reference_pos_publisher = rospy.Publisher( - buggy_name + "/auton/debug/reference_navsat", NavSatFix, queue_size=1 + "/auton/debug/reference_navsat", NavSatFix, queue_size=1 ) self.debug_error_publisher = rospy.Publisher( - buggy_name + "/auton/debug/error", ROSPose, queue_size=1 + "/auton/debug/error", ROSPose, queue_size=1 ) self.solver = osqp.OSQP() @@ -116,6 +116,7 @@ def __init__(self, buggy_name : str, start_index=0, ref_trajectory=None, ROS=Fal self.lock = Lock() self.ref_trajectory = ref_trajectory + # QUESTION: what do these topics do, and are they affected by namespace changes? if self.ROS: rospy.Subscriber('mpc/speed', Float64, self.update_speed) rospy.Subscriber('mpc/current_pose', Pose2D, self.update_trajectory) diff --git a/rb_ws/src/buggy/scripts/auton/path_planner.py b/rb_ws/src/buggy/scripts/auton/path_planner.py index 4b3a200d..82c92dbf 100755 --- a/rb_ws/src/buggy/scripts/auton/path_planner.py +++ b/rb_ws/src/buggy/scripts/auton/path_planner.py @@ -45,7 +45,7 @@ def __init__(self, nominal_traj:Trajectory, left_curb:Trajectory) -> None: "/auton/debug/other_buggy_xtrack", Float64, queue_size=1 ) - self.traj_publisher = rospy.Publisher("SC/nav/traj", TrajectoryMsg, queue_size=1) + self.traj_publisher = rospy.Publisher("nav/traj", TrajectoryMsg, queue_size=1) self.nominal_traj = nominal_traj self.left_curb = left_curb diff --git a/rb_ws/src/buggy/scripts/auton/rolling_sanity_check.py b/rb_ws/src/buggy/scripts/auton/rolling_sanity_check.py index 2caa7d8e..135d2be1 100755 --- a/rb_ws/src/buggy/scripts/auton/rolling_sanity_check.py +++ b/rb_ws/src/buggy/scripts/auton/rolling_sanity_check.py @@ -46,36 +46,36 @@ def __init__(self, self.warning_durations = [0] * 18 # keeps track of how long covariance, overrange warning, any of the filter status flags are active - rospy.Subscriber(self_name + "/imu/overrange_status", ImuOverrangeStatus, self.update_overrange_status) - rospy.Subscriber(self_name + "/nav/status.status_flags", FilterStatus, self.update_status_flags) - rospy.Subscriber(self_name + "/gnss1/fix_Pose/", PoseStamped, self.update_gps_location) - rospy.Subscriber(self_name + "/nav/odom", Odometry, self.update_filter_location) + rospy.Subscriber("/imu/overrange_status", ImuOverrangeStatus, self.update_overrange_status) + rospy.Subscriber("/nav/status.status_flags", FilterStatus, self.update_status_flags) + rospy.Subscriber("/gnss1/fix_Pose/", PoseStamped, self.update_gps_location) + rospy.Subscriber("/nav/odom", Odometry, self.update_filter_location) # these publishers are all bools as quick sanity checks (can display as indicators on foxglove for colors) # publishes IMU overrange status (checks if any warnings are tripped) - self.overrange_status_publisher = rospy.Publisher(self_name + "/debug/imu_overrange_status", Bool, queue_size=1) + self.overrange_status_publisher = rospy.Publisher("/debug/imu_overrange_status", Bool, queue_size=1) # publishes if the filter and gps data have diverged significantly - self.filter_gps_status_publisher = rospy.Publisher(self_name + "/debug/filter_gps_seperation_status", Bool, queue_size=1) + self.filter_gps_status_publisher = rospy.Publisher("/debug/filter_gps_seperation_status", Bool, queue_size=1) # publishes if the covariance is too high - self.covariance_status_publisher = rospy.Publisher(self_name + "/debug/covariance_status", Bool, queue_size=1) + self.covariance_status_publisher = rospy.Publisher("/debug/covariance_status", Bool, queue_size=1) # TODO WHAT DOES THIS MEAN self.error_message_publisher = rospy.Publisher( - self_name + "/nav/status/error_messages", String, queue_size=1 + "/nav/status/error_messages", String, queue_size=1 ) # publishes the IMU filter status messages self.status_flags_publisher = rospy.Publisher( - self_name + "/nav/status/tripped_status_flags", Int8MultiArray, queue_size=1 + "/nav/status/tripped_status_flags", Int8MultiArray, queue_size=1 ) # a compiled warning flag - outputs if any of the above have tripped self.overall_warning_publisher = rospy.Publisher( - self_name + "/debug/sanity_warning", Int8, queue_size=1 + "/debug/sanity_warning", Int8, queue_size=1 ) def update_overrange_status(self, msg : ImuOverrangeStatus): diff --git a/rb_ws/src/buggy/scripts/auton/stanley_controller.py b/rb_ws/src/buggy/scripts/auton/stanley_controller.py index dafe6a49..51c5a2ff 100755 --- a/rb_ws/src/buggy/scripts/auton/stanley_controller.py +++ b/rb_ws/src/buggy/scripts/auton/stanley_controller.py @@ -29,10 +29,10 @@ class StanleyController(Controller): def __init__(self, buggy_name, start_index=0) -> None: super(StanleyController, self).__init__(start_index, buggy_name) self.debug_reference_pos_publisher = rospy.Publisher( - buggy_name + "/auton/debug/reference_navsat", NavSatFix, queue_size=1 + "/auton/debug/reference_navsat", NavSatFix, queue_size=1 ) self.debug_error_publisher = rospy.Publisher( - buggy_name + "/auton/debug/error", ROSPose, queue_size=1 + "/auton/debug/error", ROSPose, queue_size=1 ) def compute_control( diff --git a/rb_ws/src/buggy/scripts/debug/debug_steer.py b/rb_ws/src/buggy/scripts/debug/debug_steer.py index 142920f2..9375be1c 100755 --- a/rb_ws/src/buggy/scripts/debug/debug_steer.py +++ b/rb_ws/src/buggy/scripts/debug/debug_steer.py @@ -19,7 +19,7 @@ class DebugController(): """ def __init__(self, self_name) -> None: self.steer_publisher = rospy.Publisher( - self_name + "/buggy/input/steering", Float64, queue_size=1) + "/buggy/input/steering", Float64, queue_size=1) self.rate = 1000 # Outputs a continuous sine wave ranging from -50 to 50, with a period of 500 ticks diff --git a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py index 800809df..4d2e176c 100755 --- a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py @@ -47,18 +47,18 @@ def __init__(self, self_name, other_name, teensy_name): self.lock = Lock() rospy.Subscriber( - self_name + "/buggy/input/steering", Float64, self.set_steering + "/buggy/input/steering", Float64, self.set_steering ) - rospy.Subscriber(self_name + "/debug/sanity_warning", Int8, self.set_alarm) + rospy.Subscriber("/debug/sanity_warning", Int8, self.set_alarm) # ISSUE: https://github.com/CMU-Robotics-Club/RoboBuggy2/issues/83 if other_name is None and self_name == "NAND": self.odom_publisher = rospy.Publisher( - self_name + "/nav/odom", ROSOdom, queue_size=1 + "/nav/odom", ROSOdom, queue_size=1 ) else: self.odom_publisher = rospy.Publisher( - other_name + "/nav/odom", ROSOdom, queue_size=1 + "/nav/odom", ROSOdom, queue_size=1 ) # upper bound of steering update rate, make sure auton sends slower than 500 Hz or update / 2ms @@ -68,31 +68,31 @@ def __init__(self, self_name, other_name, teensy_name): # ISSUE: https://github.com/CMU-Robotics-Club/RoboBuggy2/issues/90 self.rc_steering_angle_publisher = rospy.Publisher( - self_name + "/buggy/debug/rc_steering_angle", Float64, queue_size=1 + "/buggy/debug/rc_steering_angle", Float64, queue_size=1 ) self.steering_angle_publisher = rospy.Publisher( - self_name + "/buggy/debug/steering_angle", Float64, queue_size=1 + "/buggy/debug/steering_angle", Float64, queue_size=1 ) self.battery_voltage_publisher = rospy.Publisher( - self_name + "/buggy/debug/battery_voltage", Float64, queue_size=1 + "/buggy/debug/battery_voltage", Float64, queue_size=1 ) self.operator_ready_publisher = rospy.Publisher( - self_name + "/buggy/debug/operator_ready", Bool, queue_size=1 + "/buggy/debug/operator_ready", Bool, queue_size=1 ) self.steering_alarm_publisher = rospy.Publisher( - self_name + "/buggy/debug/steering_alarm", Bool, queue_size=1 + "/buggy/debug/steering_alarm", Bool, queue_size=1 ) self.brake_status_publisher = rospy.Publisher( - self_name + "/buggy/debug/brake_status", Bool, queue_size=1 + "/buggy/debug/brake_status", Bool, queue_size=1 ) self.use_auton_steer_publisher = rospy.Publisher( - self_name + "/buggy/debug/use_auton_steer", Bool, queue_size=1 + "/buggy/debug/use_auton_steer", Bool, queue_size=1 ) self.rc_uplink_qual_publisher = rospy.Publisher( - self_name + "/buggy/debug/rc_uplink_quality", UInt8, queue_size=1 + "/buggy/debug/rc_uplink_quality", UInt8, queue_size=1 ) self.nand_fix_publisher = rospy.Publisher( - self_name + "/buggy/debug/nand_fix", UInt8, queue_size=1 + "/buggy/debug/nand_fix", UInt8, queue_size=1 ) def set_alarm(self, msg): diff --git a/rb_ws/src/buggy/scripts/visualization/telematics.py b/rb_ws/src/buggy/scripts/visualization/telematics.py index 182b2743..a8b89461 100755 --- a/rb_ws/src/buggy/scripts/visualization/telematics.py +++ b/rb_ws/src/buggy/scripts/visualization/telematics.py @@ -18,7 +18,7 @@ class Telematics: def __init__(self): """Generate all the subscribers and publishers that need to be reformatted. """ - + # Should this be in a specific namespace? Simply because it doesn't make sense, as this seems to be a msg type translator self.odom_subscriber = rospy.Subscriber("/NAND/nav/odom", Odometry, self.convert_odometry_to_navsatfix) self.odom_publisher = rospy.Publisher("/NAND/nav/odom_NavSatFix", NavSatFix, queue_size=10)