From 295913880f50b6449dfdbc8b1c983914e356e1fb Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 18 Dec 2024 23:02:03 -0800 Subject: [PATCH] cleaned up constants class --- rb_ws/src/buggy/buggy/simulator/engine.py | 18 +++++++++++------- rb_ws/src/buggy/buggy/util.py | 2 +- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index 912ec56..0d68462 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -1,5 +1,6 @@ #! /usr/bin/env python3 import threading +import sys import rclpy from rclpy.node import Node from geometry_msgs.msg import Pose, Twist, PoseWithCovariance, TwistWithCovariance @@ -8,17 +9,20 @@ from nav_msgs.msg import Odometry import numpy as np import utm -from util import Utils +sys.path.append("/rb_ws/src/buggy/buggy") +from util import Constants class Simulator(Node): + def __init__(self): super().__init__('sim_single') self.get_logger().info('INITIALIZED.') + self.starting_poses = { - "Hill1_NAND": (Utils.UTM_EAST_ZERO + 0, Utils.UTM_NORTH_ZERO + 0, -110), - "Hill1_SC": (Utils.UTM_EAST_ZERO + 20, Utils.UTM_NORTH_ZERO + 30, -110), + "Hill1_NAND": (Constants.UTM_EAST_ZERO + 0, Constants.UTM_NORTH_ZERO + 0, -110), + "Hill1_SC": (Constants.UTM_EAST_ZERO + 20, Constants.UTM_NORTH_ZERO + 30, -110), "WESTINGHOUSE": (589647, 4477143, -150), "UC_TO_PURNELL": (589635, 4477468, 160), "UC": (589681, 4477457, 160), @@ -56,12 +60,12 @@ def __init__(self): if (self.get_namespace() == "/SC"): self.buggy_name = "SC" self.declare_parameter('pose', "Hill1_SC") - self.wheelbase = Utils.WHEELBASE_SC + self.wheelbase = Constants.WHEELBASE_SC if (self.get_namespace() == "/NAND"): self.buggy_name = "NAND" self.declare_parameter('pose', "Hill1_NAND") - self.wheelbase = Utils.WHEELBASE_NAND + self.wheelbase = Constants.WHEELBASE_NAND self.velocity = self.get_parameter("velocity").value init_pose_name = self.get_parameter("pose").value @@ -135,8 +139,8 @@ def publish(self): (lat, long) = utm.to_latlon( p.position.x, p.position.y, - Utils.UTM_ZONE_NUM, - Utils.UTM_ZONE_LETTER, + Constants.UTM_ZONE_NUM, + Constants.UTM_ZONE_LETTER, ) lat_noisy = lat + np.random.normal(0, 1e-6) diff --git a/rb_ws/src/buggy/buggy/util.py b/rb_ws/src/buggy/buggy/util.py index b5e78e1..0bdfc8e 100644 --- a/rb_ws/src/buggy/buggy/util.py +++ b/rb_ws/src/buggy/buggy/util.py @@ -1,4 +1,4 @@ -class Utils: +class Constants: UTM_EAST_ZERO = 589702.87 UTM_NORTH_ZERO = 4477172.947 UTM_ZONE_NUM = 17