Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Engine rewrite #6

Merged
merged 4 commits into from
Dec 29, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
113 changes: 42 additions & 71 deletions rb_ws/src/buggy/buggy/simulator/engine.py
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -8,25 +9,20 @@
from nav_msgs.msg import Odometry
import numpy as np
import utm
sys.path.append("/rb_ws/src/buggy/buggy")

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is a hardcoded constant path that works because in the docker container, rb_ws is in the root directory, however since we are running outside docker in production, we need a way to not do this. Any suggestions?

from util import Constants

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 = {
mehulgoel873 marked this conversation as resolved.
Show resolved Hide resolved
"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),
"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),
Expand All @@ -39,57 +35,53 @@ def __init__(self):
"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
self.wheelbase = Constants.WHEELBASE_SC

if (self.get_namespace() == "/NAND"):
self.buggy_name = "NAND"
self.declare_parameter('pose', "Hill1_NAND")
self.wheelbase = Simulator.WHEELBASE_NAND
self.wheelbase = Constants.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.rate = 100 # Hz
self.tick_count = 0
self.interval = 2 # how frequently to publish

self.lock = threading.Lock()

freq = 10
timer_period = 1/freq # seconds
timer_period = 1/self.rate # seconds
self.timer = self.create_timer(timer_period, self.loop)

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
)

# for X11 matplotlib (direction included)
self.plot_publisher = self.create_publisher(Pose, "sim_2d/utm", 1)

# simulate the INS's outputs (noise included)
# this is published as a BuggyState (UTM and radians)
self.pose_publisher = self.create_publisher(Odometry, "nav/odom", 1)

self.navsatfix_noisy_publisher = self.create_publisher(
NavSatFix, "state/pose_navsat_noisy", 1
)

def update_steering_angle(self, data: Float64):
with self.lock:
self.steering_angle = data.data
Expand All @@ -98,14 +90,6 @@ 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
Expand Down Expand Up @@ -154,18 +138,13 @@ def publish(self):
(lat, long) = utm.to_latlon(
p.position.x,
p.position.y,
Simulator.UTM_ZONE_NUM,
Simulator.UTM_ZONE_LETTER,
Constants.UTM_ZONE_NUM,
Constants.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
Expand All @@ -176,13 +155,15 @@ def publish(self):
odom.header.stamp = time_stamp

odom_pose = Pose()
odom_pose.position.x = float(long)
odom_pose.position.y = float(lat)
east, north = utm.from_latlon(lat_noisy, long_noisy)
odom_pose.position.x = float(east)
odom_pose.position.y = float(north)
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)

# NOTE: autonsystem only cares about magnitude of velocity, so we don't need to split into components
odom_twist = Twist()
odom_twist.linear.x = float(velocity)

Expand All @@ -192,20 +173,10 @@ def publish(self):
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:
if self.tick_count % self.interval == 0:
self.publish()
self.pub_tick_count = 0
else:
self.pub_tick_count += 1


self.tick_count += 1


def main(args=None):
Expand Down
8 changes: 8 additions & 0 deletions rb_ws/src/buggy/buggy/util.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
class 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
Loading