diff --git a/python-requirements.txt b/python-requirements.txt
index 66b300e..d75cc06 100644
--- a/python-requirements.txt
+++ b/python-requirements.txt
@@ -5,6 +5,7 @@ numpy
osqp
pandas
pymap3d
+pyproj
pyserial
scipy
setuptools==58.2.0
diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py
new file mode 100644
index 0000000..48b1086
--- /dev/null
+++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py
@@ -0,0 +1,198 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Odometry
+import numpy as np
+import pyproj
+from scipy.spatial.transform import Rotation
+
+class BuggyStateConverter(Node):
+ def __init__(self):
+ super().__init__("buggy_state_converter")
+
+ namespace = self.get_namespace()
+
+ # Create publisher and subscriber for "self" namespace
+ self.self_raw_state_subscriber = self.create_subscription(
+ Odometry, "self/raw_state", self.self_raw_state_callback, 10
+ )
+ self.self_state_publisher = self.create_publisher(Odometry, "self/state", 10)
+
+ # Check if namespace is "SC" to add an "other" subscriber/publisher
+ if namespace == "/SC":
+ self.other_raw_state_subscriber = self.create_subscription(
+ Odometry, "other/raw_state", self.other_raw_state_callback, 10
+ )
+ self.other_state_publisher = self.create_publisher(Odometry, "other/state", 10)
+
+ # Initialize pyproj Transformer for ECEF -> UTM conversion for /SC
+ self.ecef_to_utm_transformer = pyproj.Transformer.from_crs(
+ "epsg:4978", "epsg:32617", always_xy=True
+ ) # TODO: Confirm UTM EPSG code, using EPSG:32617 for UTM Zone 17N
+
+ def self_raw_state_callback(self, msg):
+ """ Callback for processing self/raw_state messages and publishing to self/state """
+ namespace = self.get_namespace()
+
+ if namespace == "/SC":
+ converted_msg = self.convert_SC_state(msg)
+ elif namespace == "/NAND":
+ converted_msg = self.convert_NAND_state(msg)
+ else:
+ self.get_logger().warn(f"Namespace not recognized for buggy state conversion: {namespace}")
+ return
+
+ self.self_state_publisher.publish(converted_msg)
+
+ def other_raw_state_callback(self, msg):
+ """ Callback for processing other/raw_state messages and publishing to other/state """
+ # Convert the SC message and publish to other/state
+ converted_msg = self.convert_NAND_other_state(msg)
+ self.other_state_publisher.publish(converted_msg)
+
+ def convert_SC_state(self, msg):
+ """
+ Converts self/raw_state in SC namespace to clean state units and structure
+
+ Takes in ROS message in nav_msgs/Odometry format
+ Assumes that the SC namespace is using ECEF coordinates and quaternion orientation
+ """
+
+ converted_msg = Odometry()
+ converted_msg.header = msg.header
+
+ # ---- 1. Convert ECEF Position to UTM Coordinates ----
+ ecef_x = msg.pose.pose.position.x
+ ecef_y = msg.pose.pose.position.y
+ ecef_z = msg.pose.pose.position.z
+
+ # Convert ECEF to UTM
+ utm_x, utm_y, utm_z = self.ecef_to_utm_transformer.transform(ecef_x, ecef_y, ecef_z)
+ converted_msg.pose.pose.position.x = utm_x # UTM Easting
+ converted_msg.pose.pose.position.y = utm_y # UTM Northing
+ converted_msg.pose.pose.position.z = utm_z # UTM Altitude
+
+ # ---- 2. Convert Quaternion to Heading (Radians) ----
+ qx, qy, qz, qw = msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
+
+ # Use Rotation.from_quat to get roll, pitch, yaw
+ roll, pitch, yaw = Rotation.from_quat([qx, qy, qz, qw]).as_euler('xyz')
+ # roll, pitch, yaw = euler_from_quaternion([qx, qy, qz, qw]) # tf_transformations bad
+
+ # Store the heading in the x component of the orientation
+ converted_msg.pose.pose.orientation.x = roll
+ converted_msg.pose.pose.orientation.y = pitch
+ converted_msg.pose.pose.orientation.z = yaw
+ converted_msg.pose.pose.orientation.w = 0.0 # fourth (quaternion) term irrelevant for euler angles
+
+ # ---- 3. Copy Covariances (Unchanged) ----
+ converted_msg.pose.covariance = msg.pose.covariance
+ converted_msg.twist.covariance = msg.twist.covariance
+
+ # ---- 4. Copy Linear Velocities ----
+ converted_msg.twist.twist.linear.x = msg.twist.twist.linear.x # m/s in x-direction
+ converted_msg.twist.twist.linear.y = msg.twist.twist.linear.y # m/s in x-direction
+ converted_msg.twist.twist.linear.z = msg.twist.twist.linear.z # keep original Z velocity
+
+ # ---- 5. Copy Angular Velocities ----
+ converted_msg.twist.twist.angular.x = msg.twist.twist.angular.x # copying over
+ converted_msg.twist.twist.angular.y = msg.twist.twist.angular.y # copying over
+ converted_msg.twist.twist.angular.z = msg.twist.twist.angular.z # rad/s, heading change rate
+
+ return converted_msg
+
+ def convert_NAND_state(self, msg):
+ """
+ Converts self/raw_state in NAND namespace to clean state units and structure
+ Takes in ROS message in nav_msgs/Odometry format
+ """
+
+ converted_msg = Odometry()
+ converted_msg.header = msg.header
+
+ # ---- 1. Directly use UTM Coordinates ----
+ converted_msg.pose.pose.position.x = msg.pose.pose.position.x # UTM Easting
+ converted_msg.pose.pose.position.y = msg.pose.pose.position.y # UTM Northing
+ converted_msg.pose.pose.position.z = msg.pose.pose.position.z # UTM Altitude
+
+ # ---- 2. Orientation in Radians ----
+ converted_msg.pose.pose.orientation.x = msg.pose.pose.orientation.x
+ converted_msg.pose.pose.orientation.y = msg.pose.pose.orientation.y
+ converted_msg.pose.pose.orientation.z = msg.pose.pose.orientation.z
+ converted_msg.pose.pose.orientation.w = 0.0 # fourth (quaternion) term irrelevant for euler angles
+
+ # ---- 3. Copy Covariances (Unchanged) ----
+ converted_msg.pose.covariance = msg.pose.covariance
+ converted_msg.twist.covariance = msg.twist.covariance
+
+ # ---- 4. Linear Velocities in m/s ----
+ # TODO: Check if scalar velocity is coming in from msg.twist.twist.linear.x
+ # Convert scalar speed to velocity x/y components using heading (orientation.z)
+ speed = msg.twist.twist.linear.x # m/s scalar velocity
+ heading = msg.pose.pose.orientation.z # heading in radians
+
+ # Calculate velocity components
+ converted_msg.twist.twist.linear.x = speed * np.cos(heading) # m/s in x-direction
+ converted_msg.twist.twist.linear.y = speed * np.sin(heading) # m/s in y-direction
+ converted_msg.twist.twist.linear.z = 0.0
+
+ # ---- 5. Copy Angular Velocities ----
+ converted_msg.twist.twist.angular.x = msg.twist.twist.angular.x # copying over
+ converted_msg.twist.twist.angular.y = msg.twist.twist.angular.y # copying over
+ converted_msg.twist.twist.angular.z = msg.twist.twist.angular.z # rad/s, heading change rate
+
+ return converted_msg
+
+ def convert_NAND_other_state(self, msg):
+ """ Converts other/raw_state in SC namespace (NAND data) to clean state units and structure """
+ converted_msg = Odometry()
+ converted_msg.header = msg.header
+
+ # ---- 1. Directly use UTM Coordinates ----
+ converted_msg.pose.pose.position.x = msg.x # UTM Easting
+ converted_msg.pose.pose.position.y = msg.y # UTM Northing
+ converted_msg.pose.pose.position.z = msg.z # UTM Altitude (not provided in other/raw_state, defaults to 0.0)
+
+ # ---- 2. Orientation in Radians ----
+ converted_msg.pose.pose.orientation.x = msg.roll # (roll not provided in other/raw_state, defaults to 0.0)
+ converted_msg.pose.pose.orientation.y = msg.pitch # (pitch not provided in other/raw_state, defaults to 0.0)
+ converted_msg.pose.pose.orientation.z = msg.heading # heading in radians
+ converted_msg.pose.pose.orientation.w = 0.0 # fourth quaternion term irrelevant for euler angles
+
+ # ---- 3. Copy Covariances (Unchanged) ----
+ converted_msg.pose.covariance = msg.pose_covariance # (not provided in other/raw_state)
+ converted_msg.twist.covariance = msg.twist_covariance # (not provided in other/raw_state)
+
+ # ---- 4. Linear Velocities in m/s ----
+ # Convert scalar speed to velocity x/y components using heading (msg.heading)
+ speed = msg.speed # m/s scalar velocity
+ heading = msg.heading # heading in radians
+
+ # Calculate velocity components
+ converted_msg.twist.twist.linear.x = speed * np.cos(heading) # m/s in x-direction
+ converted_msg.twist.twist.linear.y = speed * np.sin(heading) # m/s in y-direction
+ converted_msg.twist.twist.linear.z = 0.0
+
+ # ---- 5. Angular Velocities ----
+ converted_msg.twist.twist.angular.x = msg.roll_rate # (roll rate not provided in other/raw_state, defaults to 0.0)
+ converted_msg.twist.twist.angular.y = msg.pitch_rate # (pitch rate not provided in other/raw_state, defaults to 0.0)
+ converted_msg.twist.twist.angular.z = msg.heading_rate # rad/s, heading change rate
+
+ return converted_msg
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ # Create the BuggyStateConverter node and spin it
+ state_converter = BuggyStateConverter()
+ rclpy.spin(state_converter)
+
+ # Shutdown when done
+ state_converter.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py
index 740c7dd..d57b92a 100644
--- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py
+++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py
@@ -3,6 +3,9 @@
import threading
import rclpy
from rclpy import Node
+from scipy.spatial.transform import Rotation
+import utm
+
from std_msgs.msg import Float64, Int8, UInt8, Bool
from host_comm import *
from nav_msgs.msg import Odometry
diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py
index 361ae4b..efaf6db 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,16 +9,11 @@
from nav_msgs.msg import Odometry
import numpy as np
import utm
+sys.path.append("/rb_ws/src/buggy/buggy")
+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')
@@ -25,8 +21,8 @@ def __init__(self):
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),
+ "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),
@@ -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
@@ -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
@@ -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
@@ -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)
@@ -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):
diff --git a/rb_ws/src/buggy/buggy/util.py b/rb_ws/src/buggy/buggy/util.py
new file mode 100644
index 0000000..0bdfc8e
--- /dev/null
+++ b/rb_ws/src/buggy/buggy/util.py
@@ -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
\ 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 ca458b8..0398279 100755
--- a/rb_ws/src/buggy/launch/sim_2d_single.xml
+++ b/rb_ws/src/buggy/launch/sim_2d_single.xml
@@ -10,6 +10,8 @@
+
+
diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py
index 42318d0..08a05de 100644
--- a/rb_ws/src/buggy/setup.py
+++ b/rb_ws/src/buggy/setup.py
@@ -26,7 +26,8 @@
'console_scripts': [
'hello_world = buggy.hello_world:main',
'sim_single = buggy.simulator.engine:main',
- 'watchdog = buggy.watchdog.watchdog_node:main'
+ 'buggy_state_converter = buggy.buggy_state_converter:main',
+ 'watchdog = buggy.watchdog.watchdog_node:main',
],
},
)
\ No newline at end of file