From 931c7a8c39f04de3b8c7dd8847b998ae3c64927a Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Mon, 5 Feb 2024 00:35:49 -0500 Subject: [PATCH] minor cleanup --- rb_ws/src/buggy/scripts/2d_sim/controller_2d.py | 1 - rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py | 9 ++++----- rb_ws/src/buggy/scripts/auton/autonsystem.py | 3 ++- 3 files changed, 6 insertions(+), 7 deletions(-) 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 f5443114..4ca18a61 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py +++ b/rb_ws/src/buggy/scripts/2d_sim/controller_2d.py @@ -51,6 +51,5 @@ def set_velocity(self, vel: float): rate = rospy.Rate(5) i = 0 while not rospy.is_shutdown(): - controller.set_velocity(15) rate.sleep() 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 e4544b43..bed311f9 100644 --- a/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py +++ b/rb_ws/src/buggy/scripts/2d_sim/velocity_ui.py @@ -1,13 +1,9 @@ #! /usr/bin/env python3 -import numpy as np import rospy import sys from controller_2d import Controller -from std_msgs.msg import Float64 import threading -import math import tkinter as tk -import keyboard class VelocityUI: @@ -34,10 +30,13 @@ def __init__(self, init_vel: float, buggy_name: str): def step(self): + """sets velocity of buggy to the current scale value + called once every tick + """ self.root.update_idletasks() self.root.update() '''Update velocity of buggy''' - self.buggy_vel = self.scale.get()/10 + self.buggy_vel = self.scale.get()/10 # so we can set velocity with 0.1 precision self.controller.set_velocity(self.buggy_vel) diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index 22039b0b..e8d783c0 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -53,7 +53,8 @@ def __init__(self, trajectory, controller, brake_controller, buggy_name, is_sim) rospy.Subscriber(buggy_name + "/nav/odom", Odometry, self.update_msg) - rospy.Subscriber("nav/odom", Odometry, self.update_msg) + rospy.Subscriber(buggy_name + "nav/odom", Odometry, self.update_msg) + self.covariance_warning_publisher = rospy.Publisher( buggy_name + "/debug/is_high_covariance", Bool, queue_size=1 )