From 79879a8d696c7143b78cda12e106659c8829418e Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Mon, 21 Oct 2024 15:37:39 -0400 Subject: [PATCH 1/9] basic node setup --- rb_ws/src/buggy/launch/sim_2d_single.xml | 3 +++ rb_ws/src/buggy/scripts/simulator/engine.py | 27 +++++++++++++++++++++ rb_ws/src/buggy/setup.py | 2 ++ 3 files changed, 32 insertions(+) create mode 100644 rb_ws/src/buggy/scripts/simulator/engine.py diff --git a/rb_ws/src/buggy/launch/sim_2d_single.xml b/rb_ws/src/buggy/launch/sim_2d_single.xml index e0b5846..0aed958 100755 --- a/rb_ws/src/buggy/launch/sim_2d_single.xml +++ b/rb_ws/src/buggy/launch/sim_2d_single.xml @@ -1,6 +1,9 @@ + + + \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py new file mode 100644 index 0000000..f91efda --- /dev/null +++ b/rb_ws/src/buggy/scripts/simulator/engine.py @@ -0,0 +1,27 @@ +#! /usr/bin/env python3 +import sys +import time +import rclpy +from rclpy.node import Node + +class Simulator(Node): + # simulator constants: + + def __init__(self): + if (self.get_namespace() == "SC"): + self.buggy_name = "SC" + if (self.get_namespace() == "NAND"): + self.buggy_name = "NAND" + + if __name__ == "__main__": + rclpy.init() + sim = Simulator() + rclpy.spin(sim) + + # publish initial position, then sleep + # so that auton stack has time to initialize + # before buggy moves + time.sleep(15.0) + sim.loop() + + diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py index 4373639..b8edf92 100644 --- a/rb_ws/src/buggy/setup.py +++ b/rb_ws/src/buggy/setup.py @@ -25,6 +25,8 @@ entry_points={ 'console_scripts': [ 'hello_world = buggy.hello_world:main' + 'sim-single = buggy.engine:main' + ], }, ) From 1043ab8ee413589111fb1db65854c6de58f0318d Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Tue, 22 Oct 2024 15:08:39 -0400 Subject: [PATCH 2/9] added publisher --- rb_ws/src/buggy/scripts/simulator/engine.py | 27 ++++++++++++--------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/rb_ws/src/buggy/scripts/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py index f91efda..d0bab70 100644 --- a/rb_ws/src/buggy/scripts/simulator/engine.py +++ b/rb_ws/src/buggy/scripts/simulator/engine.py @@ -2,26 +2,31 @@ import sys import time import rclpy -from rclpy.node import Node +from std_msgs.msg import String -class Simulator(Node): +class Simulator(): # simulator constants: def __init__(self): + self.test_publisher = self.create_publisher(String, 'test', 10) if (self.get_namespace() == "SC"): self.buggy_name = "SC" + if (self.get_namespace() == "NAND"): self.buggy_name = "NAND" - if __name__ == "__main__": - rclpy.init() - sim = Simulator() - rclpy.spin(sim) + def loop(self): + print("hello") + self.test_publisher.publish(self.buggy_name) + +if __name__ == "__main__": + rclpy.init() + sim = Simulator() - # publish initial position, then sleep - # so that auton stack has time to initialize - # before buggy moves - time.sleep(15.0) - sim.loop() + # publish initial position, then sleep + # so that auton stack has time to initialize + # before buggy moves + time.sleep(15.0) + sim.loop() From 858196f001f1864c4bbce49f8381f835122490e2 Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Tue, 22 Oct 2024 21:11:31 -0400 Subject: [PATCH 3/9] implemented callback in loop with timer --- rb_ws/src/buggy/buggy/simulator/engine.py | 49 +++++++++++++++++++++ rb_ws/src/buggy/launch/sim_2d_single.xml | 10 ++--- rb_ws/src/buggy/scripts/simulator/engine.py | 32 -------------- rb_ws/src/buggy/setup.py | 7 ++- 4 files changed, 57 insertions(+), 41 deletions(-) create mode 100644 rb_ws/src/buggy/buggy/simulator/engine.py delete mode 100644 rb_ws/src/buggy/scripts/simulator/engine.py diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py new file mode 100644 index 0000000..346ae3a --- /dev/null +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -0,0 +1,49 @@ +#! /usr/bin/env python3 +import sys +import time +import rclpy +from rclpy.node import Node + +from std_msgs.msg import String, Float64 + +class Simulator(Node): + # simulator constants: + + def __init__(self): + super().__init__('SC_sim_single') + self.get_logger().info("INITIALIZING") + self.number_publisher = self.create_publisher(Float64, 'numbers', 1) + self.test_publisher = self.create_publisher(String, 'test', 1) + self.i = 0 + + self.buggy_name = "NONE" + + if (self.get_namespace() == "/SC"): + self.buggy_name = "SC" + + if (self.get_namespace() == "/NAND"): + self.buggy_name = "NAND" + + freq = 10 + timer_period = 1/freq # seconds + self.timer = self.create_timer(timer_period, self.loop) + + def loop(self): + + self.get_logger().info("LOOPING") + + msg = String() + msg.data = self.buggy_name + self.test_publisher.publish(msg) + msg2 = Float64() + msg2.data = float(self.i) + self.number_publisher.publish(msg2) + self.i += 1 + +def main(args=None): + rclpy.init(args=args) + sim = Simulator() + sim.get_logger().info("CREATED NODE") + rclpy.spin(sim) + + diff --git a/rb_ws/src/buggy/launch/sim_2d_single.xml b/rb_ws/src/buggy/launch/sim_2d_single.xml index 0aed958..550c259 100755 --- a/rb_ws/src/buggy/launch/sim_2d_single.xml +++ b/rb_ws/src/buggy/launch/sim_2d_single.xml @@ -1,9 +1,9 @@ - - - + + + - - + \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py deleted file mode 100644 index d0bab70..0000000 --- a/rb_ws/src/buggy/scripts/simulator/engine.py +++ /dev/null @@ -1,32 +0,0 @@ -#! /usr/bin/env python3 -import sys -import time -import rclpy -from std_msgs.msg import String - -class Simulator(): - # simulator constants: - - def __init__(self): - self.test_publisher = self.create_publisher(String, 'test', 10) - if (self.get_namespace() == "SC"): - self.buggy_name = "SC" - - if (self.get_namespace() == "NAND"): - self.buggy_name = "NAND" - - def loop(self): - print("hello") - self.test_publisher.publish(self.buggy_name) - -if __name__ == "__main__": - rclpy.init() - sim = Simulator() - - # publish initial position, then sleep - # so that auton stack has time to initialize - # before buggy moves - time.sleep(15.0) - sim.loop() - - diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py index b8edf92..42b86b1 100644 --- a/rb_ws/src/buggy/setup.py +++ b/rb_ws/src/buggy/setup.py @@ -13,7 +13,7 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - (os.path.join("share", package_name), glob("launch/*.xml")), + (os.path.join("share", package_name), glob("launch/*.xml")) ], install_requires=['setuptools'], zip_safe=True, @@ -24,9 +24,8 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'hello_world = buggy.hello_world:main' - 'sim-single = buggy.engine:main' - + 'hello_world = buggy.hello_world:main', + 'sim_single = buggy.simulator.engine:main' ], }, ) From b9c8231614f9f4153e7d00908260bf192c69b6c4 Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Tue, 22 Oct 2024 21:17:26 -0400 Subject: [PATCH 4/9] publish/subscriber base code --- rb_ws/src/buggy/buggy/simulator/engine.py | 18 +++++------------- 1 file changed, 5 insertions(+), 13 deletions(-) diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index 346ae3a..4873035 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -4,20 +4,17 @@ import rclpy from rclpy.node import Node -from std_msgs.msg import String, Float64 +from std_msgs.msg import Float64 class Simulator(Node): # simulator constants: def __init__(self): - super().__init__('SC_sim_single') - self.get_logger().info("INITIALIZING") + super().__init__('sim_single') self.number_publisher = self.create_publisher(Float64, 'numbers', 1) - self.test_publisher = self.create_publisher(String, 'test', 1) self.i = 0 self.buggy_name = "NONE" - if (self.get_namespace() == "/SC"): self.buggy_name = "SC" @@ -29,12 +26,6 @@ def __init__(self): self.timer = self.create_timer(timer_period, self.loop) def loop(self): - - self.get_logger().info("LOOPING") - - msg = String() - msg.data = self.buggy_name - self.test_publisher.publish(msg) msg2 = Float64() msg2.data = float(self.i) self.number_publisher.publish(msg2) @@ -43,7 +34,8 @@ def loop(self): def main(args=None): rclpy.init(args=args) sim = Simulator() - sim.get_logger().info("CREATED NODE") rclpy.spin(sim) + rclpy.shutdown() - +if __name__ == "__main__": + main() \ No newline at end of file From a8e41519585a678c2a79d5c14b2bf0f2313e7e7f Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 23 Oct 2024 16:47:30 -0400 Subject: [PATCH 5/9] naive translation (ros1 -> ros2) --- rb_ws/src/buggy/buggy/simulator/engine.py | 180 +++++++++++++++++++++- 1 file changed, 173 insertions(+), 7 deletions(-) diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index 4873035..c62e39d 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -1,35 +1,201 @@ #! /usr/bin/env python3 +import re import sys -import time +import threading import rclpy from rclpy.node import Node - +from geometry_msgs.msg import Pose, Twist, PoseWithCovariance, TwistWithCovariance from std_msgs.msg import Float64 +from sensor_msgs.msg import NavSatFix +from nav_msgs.msg import Odometry +import numpy as np +import utm class Simulator(Node): # simulator constants: def __init__(self): super().__init__('sim_single') + UTM_EAST_ZERO = 589702.87 + UTM_NORTH_ZERO = 4477172.947 + UTM_ZONE_NUM = 17 + UTM_ZONE_LETTER = "T" + #TODO: make these values accurate + WHEELBASE_SC = 1.17 + WHEELBASE_NAND= 1.17 + + self.starting_poses = { + "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 0, Simulator.UTM_NORTH_ZERO + 0, -110), + "Hill1_SC": (Simulator.UTM_EAST_ZERO + 20, Simulator.UTM_NORTH_ZERO + 30, -110), + "WESTINGHOUSE": (589647, 4477143, -150), + "UC_TO_PURNELL": (589635, 4477468, 160), + "UC": (589681, 4477457, 160), + "TRACK_EAST_END": (589953, 4477465, 90), + "TRACK_RESNICK": (589906, 4477437, -20), + "GARAGE": (589846, 4477580, 180), + "PASS_PT": (589491, 4477003, -160), + "FREW_ST": (589646, 4477359, -20), + "FREW_ST_PASS": (589644, 4477368, -20), + "RACELINE_PASS": (589468.02, 4476993.07, -160), + } + self.number_publisher = self.create_publisher(Float64, 'numbers', 1) self.i = 0 - self.buggy_name = "NONE" + # for X11 matplotlib (direction included) + self.plot_publisher = self.create_publisher(Pose, "sim_2d/utm", 1) + + # simulate the INS's outputs (noise included) + self.pose_publisher = self.create_publisher(Odometry, "nav/odom", 1) + + self.steering_subscriber = self.create_subscription( + Float64, "buggy/input/steering", self.update_steering_angle, 1 + ) + # To read from velocity + self.velocity_subscriber = self.create_subscription( + Float64, "velocity", self.update_velocity, 1 + ) + self.navsatfix_noisy_publisher = self.create_publisher( + NavSatFix, "state/pose_navsat_noisy", 1 + ) if (self.get_namespace() == "/SC"): self.buggy_name = "SC" + init_pose = self.starting_poses["Hill_1SC"] + if (self.get_namespace() == "/NAND"): self.buggy_name = "NAND" + init_pose = self.starting_poses["Hill_1NAND"] + + self.e_utm, self.n_utm, self.heading = init_pose + # TODO: do we want to not hard code this + self.velocity = 12 # m/s + self.steering_angle = 0 # degrees + self.rate = 200 # Hz + self.pub_skip = 2 # publish every pub_skip ticks + + self.lock = threading.Lock() freq = 10 timer_period = 1/freq # seconds self.timer = self.create_timer(timer_period, self.loop) + def update_steering_angle(self, data: Float64): + with self.lock: + self.steering_angle = data.data + + def update_velocity(self, data: Float64): + with self.lock: + self.velocity = data.data + + def get_steering_arc(self): + with self.lock: + steering_angle = self.steering_angle + if steering_angle == 0.0: + return np.inf + + return Simulator.WHEELBASE / np.tan(np.deg2rad(steering_angle)) + + def dynamics(self, state, v): + l = Simulator.WHEELBASE + _, _, theta, delta = state + + return np.array([v * np.cos(theta), + v * np.sin(theta), + v / l * np.tan(delta), + 0]) + + def step(self): + with self.lock: + heading = self.heading + e_utm = self.e_utm + n_utm = self.n_utm + velocity = self.velocity + steering_angle = self.steering_angle + + h = 1/self.rate + state = np.array([e_utm, n_utm, np.deg2rad(heading), np.deg2rad(steering_angle)]) + k1 = self.dynamics(state, velocity) + k2 = self.dynamics(state + h/2 * k1, velocity) + k3 = self.dynamics(state + h/2 * k2, velocity) + k4 = self.dynamics(state + h/2 * k3, velocity) + + final_state = state + h/6 * (k1 + 2 * k2 + 2 * k3 + k4) + + e_utm_new, n_utm_new, heading_new, _ = final_state + heading_new = np.rad2deg(heading_new) + + with self.lock: + self.e_utm = e_utm_new + self.n_utm = n_utm_new + self.heading = heading_new + + def publish(self): + p = Pose() + time_stamp = self.get_clock().now().to_msg() + with self.lock: + p.position.x = self.e_utm + p.position.y = self.n_utm + p.position.z = self.heading + velocity = self.velocity + + self.plot_publisher.publish(p) + + (lat, long) = utm.to_latlon( + p.position.x, + p.position.y, + Simulator.UTM_ZONE_NUM, + Simulator.UTM_ZONE_LETTER, + ) + + nsf = NavSatFix() + nsf.latitude = lat + nsf.longitude = long + nsf.altitude = 260 + nsf.header.stamp = time_stamp + self.navsatfix_publisher.publish(nsf) + + if Simulator.NOISE: + lat_noisy = lat + np.random.normal(0, 1e-6) + long_noisy = long + np.random.normal(0, 1e-6) + nsf_noisy = NavSatFix() + nsf_noisy.latitude = lat_noisy + nsf_noisy.longitude = long_noisy + nsf_noisy.header.stamp = time_stamp + self.navsatfix_noisy_publisher.publish(nsf_noisy) + + odom = Odometry() + odom.header.stamp = time_stamp + + odom_pose = Pose() + odom_pose.position.x = long + odom_pose.position.y = lat + odom_pose.position.z = 260 + + odom_pose.orientation.z = np.sin(np.deg2rad(self.heading) / 2) + odom_pose.orientation.w = np.cos(np.deg2rad(self.heading) / 2) + + odom_twist = Twist() + odom_twist.linear.x = velocity + + odom.pose = PoseWithCovariance(pose=odom_pose) + odom.twist = TwistWithCovariance(twist=odom_twist) + + self.pose_publisher.publish(odom) + def loop(self): - msg2 = Float64() - msg2.data = float(self.i) - self.number_publisher.publish(msg2) - self.i += 1 + rate = self.create_rate(self.rate) + pub_tick_count = 0 + + self.step() + + if pub_tick_count == self.pub_skip: + self.publish() + pub_tick_count = 0 + else: + pub_tick_count += 1 + + def main(args=None): rclpy.init(args=args) From 97191d00cbfe4700055e43b36d592388b0b11964 Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Tue, 29 Oct 2024 19:21:15 -0400 Subject: [PATCH 6/9] the dots move now --- docker_tester | 13 ++++ rb_ws/src/buggy/buggy/simulator/engine.py | 67 +++++++++++---------- rb_ws/src/buggy/scripts/simulator/engine.py | 32 ++++++++++ 3 files changed, 81 insertions(+), 31 deletions(-) create mode 100644 docker_tester create mode 100644 rb_ws/src/buggy/scripts/simulator/engine.py diff --git a/docker_tester b/docker_tester new file mode 100644 index 0000000..a7b9fe2 --- /dev/null +++ b/docker_tester @@ -0,0 +1,13 @@ +matplotlib==3.1.2 +NavPy==1.0 +numba==0.58.0 +numpy<1.21.0 +osqp==0.6.3 +pandas==2.0.3 +pyembree==0.2.11 +pymap3d==3.0.1 +scipy==1.10.1 +trimesh==3.23.5 +utm==0.7.0 +keyboard==0.13.5 +tk==0.1.0 \ No newline at end of file diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index c62e39d..55599f0 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -13,16 +13,17 @@ class Simulator(Node): # simulator constants: + UTM_EAST_ZERO = 589702.87 + UTM_NORTH_ZERO = 4477172.947 + UTM_ZONE_NUM = 17 + UTM_ZONE_LETTER = "T" + #TODO: make these values accurate + WHEELBASE_SC = 1.17 + WHEELBASE_NAND= 1.17 def __init__(self): super().__init__('sim_single') - UTM_EAST_ZERO = 589702.87 - UTM_NORTH_ZERO = 4477172.947 - UTM_ZONE_NUM = 17 - UTM_ZONE_LETTER = "T" - #TODO: make these values accurate - WHEELBASE_SC = 1.17 - WHEELBASE_NAND= 1.17 + self.get_logger().info('INITIALIZED.') self.starting_poses = { "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 0, Simulator.UTM_NORTH_ZERO + 0, -110), @@ -60,12 +61,14 @@ def __init__(self): ) if (self.get_namespace() == "/SC"): self.buggy_name = "SC" - init_pose = self.starting_poses["Hill_1SC"] - + init_pose = self.starting_poses["Hill1_SC"] + self.wheelbase = Simulator.WHEELBASE_SC if (self.get_namespace() == "/NAND"): self.buggy_name = "NAND" - init_pose = self.starting_poses["Hill_1NAND"] + init_pose = self.starting_poses["Hill1_NAND"] + self.wheelbase = Simulator.WHEELBASE_NAND + self.e_utm, self.n_utm, self.heading = init_pose # TODO: do we want to not hard code this @@ -73,6 +76,7 @@ def __init__(self): self.steering_angle = 0 # degrees self.rate = 200 # Hz self.pub_skip = 2 # publish every pub_skip ticks + self.pub_tick_count = 0 self.lock = threading.Lock() @@ -97,7 +101,7 @@ def get_steering_arc(self): return Simulator.WHEELBASE / np.tan(np.deg2rad(steering_angle)) def dynamics(self, state, v): - l = Simulator.WHEELBASE + l = self.wheelbase _, _, theta, delta = state return np.array([v * np.cos(theta), @@ -151,32 +155,30 @@ def publish(self): nsf = NavSatFix() nsf.latitude = lat nsf.longitude = long - nsf.altitude = 260 + nsf.altitude = float(260) nsf.header.stamp = time_stamp - self.navsatfix_publisher.publish(nsf) - if Simulator.NOISE: - lat_noisy = lat + np.random.normal(0, 1e-6) - long_noisy = long + np.random.normal(0, 1e-6) - nsf_noisy = NavSatFix() - nsf_noisy.latitude = lat_noisy - nsf_noisy.longitude = long_noisy - nsf_noisy.header.stamp = time_stamp - self.navsatfix_noisy_publisher.publish(nsf_noisy) + lat_noisy = lat + np.random.normal(0, 1e-6) + long_noisy = long + np.random.normal(0, 1e-6) + nsf_noisy = NavSatFix() + nsf_noisy.latitude = lat_noisy + nsf_noisy.longitude = long_noisy + nsf_noisy.header.stamp = time_stamp + self.navsatfix_noisy_publisher.publish(nsf_noisy) odom = Odometry() odom.header.stamp = time_stamp odom_pose = Pose() - odom_pose.position.x = long - odom_pose.position.y = lat - odom_pose.position.z = 260 + odom_pose.position.x = float(long) + odom_pose.position.y = float(lat) + odom_pose.position.z = float(260) odom_pose.orientation.z = np.sin(np.deg2rad(self.heading) / 2) odom_pose.orientation.w = np.cos(np.deg2rad(self.heading) / 2) odom_twist = Twist() - odom_twist.linear.x = velocity + odom_twist.linear.x = float(velocity) odom.pose = PoseWithCovariance(pose=odom_pose) odom.twist = TwistWithCovariance(twist=odom_twist) @@ -184,16 +186,19 @@ def publish(self): self.pose_publisher.publish(odom) def loop(self): - rate = self.create_rate(self.rate) - pub_tick_count = 0 + msg = Float64() + msg.data = float(self.i) - self.step() + self.number_publisher.publish(msg) + self.i += 1 - if pub_tick_count == self.pub_skip: + self.step() + if self.pub_tick_count == self.pub_skip: self.publish() - pub_tick_count = 0 + self.pub_tick_count = 0 else: - pub_tick_count += 1 + self.pub_tick_count += 1 + diff --git a/rb_ws/src/buggy/scripts/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py new file mode 100644 index 0000000..d0bab70 --- /dev/null +++ b/rb_ws/src/buggy/scripts/simulator/engine.py @@ -0,0 +1,32 @@ +#! /usr/bin/env python3 +import sys +import time +import rclpy +from std_msgs.msg import String + +class Simulator(): + # simulator constants: + + def __init__(self): + self.test_publisher = self.create_publisher(String, 'test', 10) + if (self.get_namespace() == "SC"): + self.buggy_name = "SC" + + if (self.get_namespace() == "NAND"): + self.buggy_name = "NAND" + + def loop(self): + print("hello") + self.test_publisher.publish(self.buggy_name) + +if __name__ == "__main__": + rclpy.init() + sim = Simulator() + + # publish initial position, then sleep + # so that auton stack has time to initialize + # before buggy moves + time.sleep(15.0) + sim.loop() + + From a62da31d1097e1286f29f60c252290161f54bdc3 Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Tue, 29 Oct 2024 19:59:01 -0400 Subject: [PATCH 7/9] added command line args --- rb_ws/src/buggy/buggy/simulator/engine.py | 18 +++++++++++++----- rb_ws/src/buggy/launch/sim_2d_single.xml | 11 +++++++++-- 2 files changed, 22 insertions(+), 7 deletions(-) diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index 55599f0..67c2473 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -25,6 +25,7 @@ def __init__(self): super().__init__('sim_single') self.get_logger().info('INITIALIZED.') + self.starting_poses = { "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 0, Simulator.UTM_NORTH_ZERO + 0, -110), "Hill1_SC": (Simulator.UTM_EAST_ZERO + 20, Simulator.UTM_NORTH_ZERO + 30, -110), @@ -59,20 +60,27 @@ def __init__(self): self.navsatfix_noisy_publisher = self.create_publisher( NavSatFix, "state/pose_navsat_noisy", 1 ) + + + + + + self.declare_parameter('velocity', 12) if (self.get_namespace() == "/SC"): self.buggy_name = "SC" - init_pose = self.starting_poses["Hill1_SC"] + self.declare_parameter('pose', "Hill1_SC") self.wheelbase = Simulator.WHEELBASE_SC if (self.get_namespace() == "/NAND"): self.buggy_name = "NAND" - init_pose = self.starting_poses["Hill1_NAND"] + self.declare_parameter('pose', "Hill1_NAND") self.wheelbase = Simulator.WHEELBASE_NAND + self.velocity = self.get_parameter("velocity").value + init_pose_name = self.get_parameter("pose").value + self.init_pose = self.starting_poses[init_pose_name] - self.e_utm, self.n_utm, self.heading = init_pose - # TODO: do we want to not hard code this - self.velocity = 12 # m/s + self.e_utm, self.n_utm, self.heading = self.init_pose self.steering_angle = 0 # degrees self.rate = 200 # Hz self.pub_skip = 2 # publish every pub_skip ticks diff --git a/rb_ws/src/buggy/launch/sim_2d_single.xml b/rb_ws/src/buggy/launch/sim_2d_single.xml index 550c259..ca458b8 100755 --- a/rb_ws/src/buggy/launch/sim_2d_single.xml +++ b/rb_ws/src/buggy/launch/sim_2d_single.xml @@ -1,8 +1,15 @@ + - - + + + + + + + + From c256d1218e3976dd24d0f6979ba0379a56b4490b Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 30 Oct 2024 17:17:14 -0400 Subject: [PATCH 8/9] pylint --- rb_ws/src/buggy/buggy/simulator/engine.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index 67c2473..361ae4b 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -1,6 +1,4 @@ #! /usr/bin/env python3 -import re -import sys import threading import rclpy from rclpy.node import Node From fd52cbd98bc8d5f3c73539bb8d56c7536da66700 Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 30 Oct 2024 17:21:34 -0400 Subject: [PATCH 9/9] got rid of duplicate --- rb_ws/src/buggy/scripts/simulator/engine.py | 32 --------------------- 1 file changed, 32 deletions(-) delete mode 100644 rb_ws/src/buggy/scripts/simulator/engine.py diff --git a/rb_ws/src/buggy/scripts/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py deleted file mode 100644 index d0bab70..0000000 --- a/rb_ws/src/buggy/scripts/simulator/engine.py +++ /dev/null @@ -1,32 +0,0 @@ -#! /usr/bin/env python3 -import sys -import time -import rclpy -from std_msgs.msg import String - -class Simulator(): - # simulator constants: - - def __init__(self): - self.test_publisher = self.create_publisher(String, 'test', 10) - if (self.get_namespace() == "SC"): - self.buggy_name = "SC" - - if (self.get_namespace() == "NAND"): - self.buggy_name = "NAND" - - def loop(self): - print("hello") - self.test_publisher.publish(self.buggy_name) - -if __name__ == "__main__": - rclpy.init() - sim = Simulator() - - # publish initial position, then sleep - # so that auton stack has time to initialize - # before buggy moves - time.sleep(15.0) - sim.loop() - -