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 new file mode 100644 index 0000000..361ae4b --- /dev/null +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -0,0 +1,218 @@ +#! /usr/bin/env python3 +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: + 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') + 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), + "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 + + # 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 + ) + + + + + + self.declare_parameter('velocity', 12) + if (self.get_namespace() == "/SC"): + self.buggy_name = "SC" + self.declare_parameter('pose', "Hill1_SC") + self.wheelbase = Simulator.WHEELBASE_SC + + if (self.get_namespace() == "/NAND"): + self.buggy_name = "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 = self.init_pose + 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() + + 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 = self.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 = float(260) + nsf.header.stamp = time_stamp + + 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 = 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 = float(velocity) + + odom.pose = PoseWithCovariance(pose=odom_pose) + odom.twist = TwistWithCovariance(twist=odom_twist) + + self.pose_publisher.publish(odom) + + def loop(self): + msg = Float64() + msg.data = float(self.i) + + self.number_publisher.publish(msg) + self.i += 1 + + self.step() + if self.pub_tick_count == self.pub_skip: + self.publish() + self.pub_tick_count = 0 + else: + self.pub_tick_count += 1 + + + + +def main(args=None): + rclpy.init(args=args) + sim = Simulator() + rclpy.spin(sim) + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/sim_2d_single.xml b/rb_ws/src/buggy/launch/sim_2d_single.xml index e0b5846..ca458b8 100755 --- a/rb_ws/src/buggy/launch/sim_2d_single.xml +++ b/rb_ws/src/buggy/launch/sim_2d_single.xml @@ -1,6 +1,16 @@ + - - - + + + + + + + + + + + \ No newline at end of file diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py index 0a9e40b..42318d0 100644 --- a/rb_ws/src/buggy/setup.py +++ b/rb_ws/src/buggy/setup.py @@ -25,6 +25,7 @@ entry_points={ 'console_scripts': [ 'hello_world = buggy.hello_world:main', + 'sim_single = buggy.simulator.engine:main', 'watchdog = buggy.watchdog.watchdog_node:main' ], },