From 1a2eb709719186bf1da01176511e1efb3f401d7d Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Mon, 6 Jan 2025 23:28:09 -0800 Subject: [PATCH] added Trajectory Msg and finished off path planner --- rb_ws/src/buggy/CMakeLists.txt | 10 ++++++- rb_ws/src/buggy/launch/sim_2d_double.xml | 29 ++++++++++--------- rb_ws/src/buggy/msg/TrajectoryMsg.msg | 4 +++ rb_ws/src/buggy/package.xml | 9 +++++- .../scripts/controller/controller_node.py | 3 +- .../scripts/path_planner/path_planner.py | 18 +++++++----- rb_ws/src/buggy/scripts/util/trajectory.py | 11 +++---- 7 files changed, 54 insertions(+), 30 deletions(-) create mode 100644 rb_ws/src/buggy/msg/TrajectoryMsg.msg diff --git a/rb_ws/src/buggy/CMakeLists.txt b/rb_ws/src/buggy/CMakeLists.txt index 3fc99ba..a8a5142 100644 --- a/rb_ws/src/buggy/CMakeLists.txt +++ b/rb_ws/src/buggy/CMakeLists.txt @@ -23,7 +23,7 @@ install(DIRECTORY ) # Install Python modules -ament_python_install_package(${PROJECT_NAME}) +# ament_python_install_package(${PROJECT_NAME}) # Install Python executables install(PROGRAMS @@ -37,4 +37,12 @@ install(PROGRAMS DESTINATION lib/${PROJECT_NAME} ) + +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/TrajectoryMsg.msg" +) +ament_export_dependencies(rosidl_default_runtime) + ament_package() \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/sim_2d_double.xml b/rb_ws/src/buggy/launch/sim_2d_double.xml index 1fdaecd..17eb93d 100644 --- a/rb_ws/src/buggy/launch/sim_2d_double.xml +++ b/rb_ws/src/buggy/launch/sim_2d_double.xml @@ -1,30 +1,31 @@ - - - - - - - + + + - - - - + - + + + + + + + + + + + \ No newline at end of file diff --git a/rb_ws/src/buggy/msg/TrajectoryMsg.msg b/rb_ws/src/buggy/msg/TrajectoryMsg.msg new file mode 100644 index 0000000..9c952cc --- /dev/null +++ b/rb_ws/src/buggy/msg/TrajectoryMsg.msg @@ -0,0 +1,4 @@ +float64 time +float64 cur_idx +float64[] easting +float64[] northing \ No newline at end of file diff --git a/rb_ws/src/buggy/package.xml b/rb_ws/src/buggy/package.xml index 68bf6c0..26879e3 100644 --- a/rb_ws/src/buggy/package.xml +++ b/rb_ws/src/buggy/package.xml @@ -10,13 +10,20 @@ ament_cmake ament_cmake_python + rclcpp rclpy - + + ros2launch ament_lint_auto ament_lint_common + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + ament_cmake diff --git a/rb_ws/src/buggy/scripts/controller/controller_node.py b/rb_ws/src/buggy/scripts/controller/controller_node.py index 7d31213..cad007e 100755 --- a/rb_ws/src/buggy/scripts/controller/controller_node.py +++ b/rb_ws/src/buggy/scripts/controller/controller_node.py @@ -10,6 +10,7 @@ from std_msgs.msg import Float32, Float64, Bool from nav_msgs.msg import Odometry +from buggy.msg import TrajectoryMsg sys.path.append("/rb_ws/src/buggy/scripts") from util.trajectory import Trajectory @@ -62,7 +63,7 @@ def __init__(self): # Subscribers 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.traj_subscriber = self.create_subscription(TrajectoryMsg, 'self/cur_traj', self.traj_listener, 1) self.lock = threading.Lock() diff --git a/rb_ws/src/buggy/scripts/path_planner/path_planner.py b/rb_ws/src/buggy/scripts/path_planner/path_planner.py index 3357eee..272f81d 100755 --- a/rb_ws/src/buggy/scripts/path_planner/path_planner.py +++ b/rb_ws/src/buggy/scripts/path_planner/path_planner.py @@ -9,7 +9,7 @@ from nav_msgs.msg import Odometry from std_msgs.msg import Float64 -from TrajectoryMsg.msg import TrajectoryMsg +from buggy.msg import TrajectoryMsg sys.path.append("/rb_ws/src/buggy/scripts") from util.pose import Pose @@ -50,17 +50,18 @@ def __init__(self) -> None: self.get_logger().info('INITIALIZED.') - left_curb_filepath = "" - left_curb_trajectory = Trajectory(left_curb_filepath) - #Parameters self.declare_parameter("traj_name", "buggycourse_safe.json") traj_name = self.get_parameter("traj_name").value self.nominal_traj = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + traj_name) #TODO: Fixed filepath, not good - self.declare_parameter("curb_name", None) + self.declare_parameter("curb_name", "") curb_name = self.get_parameter("curb_name").value - self.left_curb = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + curb_name) #TODO: Fixed filepath, not good + curb_name = None if curb_name == "" else curb_name + if curb_name is None: + self.left_curb = None + else: + self.left_curb = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + curb_name) #TODO: Fixed filepath, not good #Publishers self.other_buggy_xtrack_publisher = self.create_publisher(Float64, "self/debug/other_buggy_xtrack", 10) @@ -86,6 +87,7 @@ def self_pose_callback(self, msg): def other_pose_callback(self, msg): with self.other_pose_lock: self.other_pose = Pose.rospose_to_pose(msg.pose.pose) + self.get_logger().debug("RECEIVED FROM NAND") def timer_callback(self): with self.self_pose_lock and self.other_pose_lock: @@ -187,7 +189,7 @@ def compute_traj( other_cross_track_dist = np.sum( nominal_to_other * other_normal, axis=1) - self.other_buggy_xtrack_publisher.publish(Float64(other_cross_track_dist)) + self.other_buggy_xtrack_publisher.publish(Float64(data=float(other_cross_track_dist))) # here, use passing offsets to weight NAND's cross track signed distance: # if the sample point is far from SC, the cross track distance doesn't matter @@ -227,7 +229,7 @@ def compute_traj( positions = nominal_slice + (passing_offsets[:, None] * nominal_normals) local_traj = Trajectory(json_filepath=None, positions=positions) - self.traj_publisher.publish(local_traj.pack()) + self.traj_publisher.publish(local_traj.pack(self_pose.x, self_pose.y )) def main(args=None): diff --git a/rb_ws/src/buggy/scripts/util/trajectory.py b/rb_ws/src/buggy/scripts/util/trajectory.py index 72fbcae..f674d90 100755 --- a/rb_ws/src/buggy/scripts/util/trajectory.py +++ b/rb_ws/src/buggy/scripts/util/trajectory.py @@ -4,9 +4,10 @@ import numpy as np from scipy.interpolate import Akima1DInterpolator, CubicSpline +from buggy.msg import TrajectoryMsg import utm - +import time class Trajectory: """A wrapper around a trajectory JSON file that does some under-the-hood math. Will @@ -347,10 +348,10 @@ 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] + traj.easting = list(self.positions[:, 0]) + traj.northing = list(self.positions[:, 1]) traj.time = time.time() traj.cur_idx = self.get_closest_index_on_path(x,y) return traj @@ -358,5 +359,5 @@ def get_closest_index_on_path( 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