diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py index bcc4652..ee1bb2f 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/buggy/controller/controller_node.py @@ -24,22 +24,23 @@ def __init__(self): """ super().__init__('controller') + self.get_logger().info('INITIALIZED.') #Parameters self.declare_parameter("dist", 0.0) #Starting Distance along path - start_dist = self.get_parameter("dist") + start_dist = self.get_parameter("dist").value - self.declare_parameter("traj_name", "buggycourse_path.json") - traj_name = self.get_parameter("traj_name") + self.declare_parameter("traj_name", "buggycourse_safe.json") + traj_name = self.get_parameter("traj_name").value self.cur_traj = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + traj_name) #TODO: Fixed filepath, not good start_index = self.cur_traj.get_index_from_distance(start_dist) self.declare_parameter("controller_name", "stanley") - match self.get_parameter("controller_name"): + match self.get_parameter("controller_name").value: case "stanley": - self.controller = StanleyController(start_index = start_index, buggy_name = self.get_namespace(), node=self) #IMPORT STANLEY + self.controller = StanleyController(start_index = start_index, namespace = self.get_namespace(), node=self) #IMPORT STANLEY case _: self.get_logger().error("Invalid Controller Name!") raise Exception("Invalid Controller Argument") @@ -49,14 +50,14 @@ def __init__(self): "debug/init_safety_check", 1 ) self.steer_publisher = self.create_publisher( - Float64, "/buggy/steering", 1 + Float64, "input/steering", 1 ) self.heading_publisher = self.create_publisher( - Float32, "/auton/debug/heading", 1 + Float32, "auton/debug/heading", 1 ) # Subscribers - self.odom_subscriber = self.create_subscription(Odometry, 'self/buggy/state', self.odom_listener, 1) + self.odom_subscriber = self.create_subscription(Odometry, 'self/state', self.odom_listener, 1) self.traj_subscriber = self.create_subscription(Odometry, 'self/cur_traj', self.traj_listener, 1) self.lock = threading.Lock() @@ -106,7 +107,9 @@ def init_check(self): closest_heading = self.cur_traj.get_heading_by_index(self.cur_traj.get_closest_index_on_path(self.odom.pose.pose.position.x, self.odom.pose.pose.position.y)) self.get_logger().info("current heading: " + str(np.rad2deg(current_heading))) - self.heading_publisher.publish(Float32(np.rad2deg(current_heading))) + msg = Float32() + msg.data = np.rad2deg(current_heading) + self.heading_publisher.publish(msg) #Converting headings from [-pi, pi] to [0, 2pi] if (current_heading < 0): @@ -123,30 +126,32 @@ def init_check(self): def loop(self): if not self.passed_init: self.passed_init = self.init_check() - self.init_check_publisher.publish(self.passed_init) + msg = Bool() + msg.data = self.passed_init + self.init_check_publisher.publish(msg) if self.passed_init: - self.get_logger.info("Passed Initialization Check") + self.get_logger().info("Passed Initialization Check") else: return - self.heading_publisher.publish(Float32(np.rad2deg(self.odom.pose.pose.orientation.z))) + self.heading_publisher.publish(Float32(data=np.rad2deg(self.odom.pose.pose.orientation.z))) steering_angle = self.controller.compute_control(self.odom, self.cur_traj) steering_angle_deg = np.rad2deg(steering_angle) - self.steer_publisher.publish(Float64(steering_angle_deg)) + self.steer_publisher.publish(Float64(data=float(steering_angle_deg))) def main(args=None): rclpy.init(args=args) - watchdog = Controller() + controller = Controller() - rclpy.spin(watchdog) + rclpy.spin(controller) # Destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) - watchdog.destroy_node() + controller.destroy_node() rclpy.shutdown() diff --git a/rb_ws/src/buggy/buggy/controller/stanley_controller.py b/rb_ws/src/buggy/buggy/controller/stanley_controller.py index d8be46b..932538c 100644 --- a/rb_ws/src/buggy/buggy/controller/stanley_controller.py +++ b/rb_ws/src/buggy/buggy/controller/stanley_controller.py @@ -6,7 +6,7 @@ from nav_msgs.msg import Odometry sys.path.append("/rb_ws/src/buggy/buggy") -from rb_ws.src.buggy.buggy.util.trajectory import Trajectory +from util.trajectory import Trajectory from controller.controller_superclass import Controller from util.pose import Pose @@ -30,10 +30,10 @@ class StanleyController(Controller): def __init__(self, start_index, namespace, node): super(StanleyController, self).__init__(start_index, namespace, node) self.debug_reference_pos_publisher = self.node.create_publisher( - "/auton/debug/reference_navsat", NavSatFix, queue_size=1 + NavSatFix, "auton/debug/reference_navsat", 1 ) self.debug_error_publisher = self.node.create_publisher( - "/auton/debug/error", ROSPose, queue_size=1 + ROSPose, "auton/debug/error", 1 ) def compute_control(self, state_msg : Odometry, trajectory : Trajectory): @@ -115,7 +115,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory): #Calculate error, where x is in orientation of buggy, y is cross track error - current_pose = Pose(current_rospose.pose.x, current_rospose.pose.y, heading) + current_pose = Pose(current_rospose.position.x, current_rospose.position.y, heading) reference_error = current_pose.convert_point_from_global_to_local_frame(closest_position) reference_error -= np.array([StanleyController.WHEELBASE, 0]) #Translae back to back wheel to get accurate error diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index efaf6db..e03c2c6 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -10,7 +10,7 @@ import numpy as np import utm sys.path.append("/rb_ws/src/buggy/buggy") -from util import Constants +from util.util import Constants class Simulator(Node): @@ -63,12 +63,12 @@ def __init__(self): self.timer = self.create_timer(timer_period, self.loop) self.steering_subscriber = self.create_subscription( - Float64, "buggy/input/steering", self.update_steering_angle, 1 + Float64, "input/steering", self.update_steering_angle, 1 ) # To read from velocity self.velocity_subscriber = self.create_subscription( - Float64, "velocity", self.update_velocity, 1 + Float64, "sim/velocity", self.update_velocity, 1 ) # for X11 matplotlib (direction included) @@ -76,10 +76,10 @@ def __init__(self): # 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.pose_publisher = self.create_publisher(Odometry, "self/state", 1) self.navsatfix_noisy_publisher = self.create_publisher( - NavSatFix, "state/pose_navsat_noisy", 1 + NavSatFix, "self/pose_navsat_noisy", 1 ) def update_steering_angle(self, data: Float64): @@ -155,13 +155,12 @@ def publish(self): odom.header.stamp = time_stamp odom_pose = Pose() - east, north = utm.from_latlon(lat_noisy, long_noisy) + 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) + odom_pose.orientation.z = np.deg2rad(self.heading) # NOTE: autonsystem only cares about magnitude of velocity, so we don't need to split into components odom_twist = Twist() @@ -183,6 +182,8 @@ def main(args=None): rclpy.init(args=args) sim = Simulator() rclpy.spin(sim) + + sim.destroy_node() rclpy.shutdown() if __name__ == "__main__": diff --git a/rb_ws/src/buggy/buggy/util/pose.py b/rb_ws/src/buggy/buggy/util/pose.py index 985755a..b39e03d 100644 --- a/rb_ws/src/buggy/buggy/util/pose.py +++ b/rb_ws/src/buggy/buggy/util/pose.py @@ -1,8 +1,7 @@ import numpy as np from geometry_msgs.msg import Pose as ROSPose -from tf.transformations import euler_from_quaternion -from tf.transformations import quaternion_from_euler +from scipy.spatial.transform import Rotation class Pose: @@ -28,20 +27,20 @@ def rospose_to_pose(rospose: ROSPose): Returns: Pose: converted pose """ - (_, _, yaw) = euler_from_quaternion( + (_, _, yaw) = Rotation.from_quat( [ rospose.orientation.x, rospose.orientation.y, rospose.orientation.z, rospose.orientation.w, ] - ) + ).as_euler('xyz') p = Pose(rospose.position.x, rospose.position.y, yaw) return p def heading_to_quaternion(heading): - q = quaternion_from_euler(0, 0, heading) + q = Rotation.from_euler([0, 0, heading]).as_quat() return q def __init__(self, x, y, theta): diff --git a/rb_ws/src/buggy/buggy/util/trajectory.py b/rb_ws/src/buggy/buggy/util/trajectory.py index a7da7df..85b996f 100644 --- a/rb_ws/src/buggy/buggy/util/trajectory.py +++ b/rb_ws/src/buggy/buggy/util/trajectory.py @@ -3,7 +3,7 @@ import uuid import matplotlib.pyplot as plt -from buggy.msg import TrajectoryMsg +# from buggy.msg import TrajectoryMsg import numpy as np from scipy.interpolate import Akima1DInterpolator, CubicSpline @@ -58,7 +58,7 @@ def __init__(self, json_filepath=None, positions = None, interpolator="CubicSpli lon = waypoint["lon"] # Convert to world coordinates - x, y = utm.from_latlon(lat, lon) #TODO: Before Merging, make sure that we can do this nonlocalized + x, y, _, _ = utm.from_latlon(lat, lon) #TODO: Before Merging, make sure that we can do this nonlocalized positions.append([x, y]) positions = np.array(positions) @@ -350,7 +350,7 @@ def get_closest_index_on_path( + start_index ) - def pack(self, x, y) -> TrajectoryMsg: + """ def pack(self, x, y) -> TrajectoryMsg: traj = TrajectoryMsg() traj.easting = self.positions[:, 0] traj.northing = self.positions[:, 1] @@ -361,5 +361,5 @@ def pack(self, x, y) -> TrajectoryMsg: def unpack(trajMsg : TrajectoryMsg): pos = np.array([trajMsg.easting, trajMsg.northing]).transpose(1, 0) cur_idx = trajMsg.cur_idx - return Trajectory(positions=pos), cur_idx + return Trajectory(positions=pos), cur_idx """ diff --git a/rb_ws/src/buggy/buggy/util.py b/rb_ws/src/buggy/buggy/util/util.py similarity index 100% rename from rb_ws/src/buggy/buggy/util.py rename to rb_ws/src/buggy/buggy/util/util.py diff --git a/rb_ws/src/buggy/launch/sim_2d_single.xml b/rb_ws/src/buggy/launch/sim_2d_single.xml index 0398279..bc455d6 100755 --- a/rb_ws/src/buggy/launch/sim_2d_single.xml +++ b/rb_ws/src/buggy/launch/sim_2d_single.xml @@ -1,17 +1,23 @@ - + + + + + + + + + - - diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py index a44f005..a8a022e 100644 --- a/rb_ws/src/buggy/setup.py +++ b/rb_ws/src/buggy/setup.py @@ -26,7 +26,7 @@ 'console_scripts': [ 'hello_world = buggy.hello_world:main', 'sim_single = buggy.simulator.engine:main', - 'controller = buggy.controller.controller_node:main' + 'controller = buggy.controller.controller_node:main', 'buggy_state_converter = buggy.buggy_state_converter:main', 'watchdog = buggy.watchdog.watchdog_node:main', ],