From a3b3d818cc394cff3d523a8634321adaa7907461 Mon Sep 17 00:00:00 2001 From: Jackack Date: Fri, 3 Jan 2025 16:40:30 +0800 Subject: [PATCH 1/9] added path planner for ROS2 --- .../buggy/buggy/path_planner/path_planner.py | 184 ++++++++++++++++++ 1 file changed, 184 insertions(+) create mode 100644 rb_ws/src/buggy/buggy/path_planner/path_planner.py diff --git a/rb_ws/src/buggy/buggy/path_planner/path_planner.py b/rb_ws/src/buggy/buggy/path_planner/path_planner.py new file mode 100644 index 00000000..37af6337 --- /dev/null +++ b/rb_ws/src/buggy/buggy/path_planner/path_planner.py @@ -0,0 +1,184 @@ +import sys +import numpy as np +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import NavSatFix +from std_msgs.msg import Float64 +from TrajectoryMsg.msg import TrajectoryMsg + +sys.path.append("/rb_ws/src/buggy/buggy") +from util.pose import Pose +from util.trajectory import Trajectory + +class PathPlanner(Node): + """ + Class to generate new trajectory splices for SC autonomous system. + + Takes in a default trajectory and an inner curb trajectory. + + """ + + # move the curb towards the center of the course by CURB_MARGIN meters + # for a margin of safety + CURB_MARGIN = 1 #m + + # the offset is calculated as a mirrored sigmoid function of distance + OFFSET_SCALE_CROSS_TRACK = 2 #m + OFFSET_SCALE_ALONG_TRACK = 0.2 + ACTIVATE_OTHER_SCALE_ALONG_TRACK = 0.1 + OFFSET_SHIFT_ALONG_TRACK = 4 #m + + # number of meters ahead of the buggy to generate local trajectory for + LOCAL_TRAJ_LEN = 50#m + + # start generating local trajectory this many meters ahead of current position + LOOKAHEAD = 2#m + + # number of points to sample along the nominal trajectory + RESOLUTION = 150 + + def __init__(self, nominal_traj:Trajectory, left_curb:Trajectory) -> None: + super().__init__('path_planner') + self.other_buggy_xtrack_publisher = self.create_publisher(Float64, "/auton/debug/other_buggy_xtrack", 10) + self.traj_publisher = self.create_publisher(TrajectoryMsg, "/auton/trajectory", 10) + + self.nominal_traj = nominal_traj + self.left_curb = left_curb + + def offset_func(self, dist): + """ + Args: + dist: (N, ) numpy array, distances between ego-buggy and obstacle, + along the trajectory + Returns: + (N, ) numpy array, offsets from nominal trajectory required to overtake, + defined by a sigmoid function + """ + + return self.OFFSET_SCALE_CROSS_TRACK / \ + (1 + np.exp(-(-self.OFFSET_SCALE_ALONG_TRACK * dist + + self.OFFSET_SHIFT_ALONG_TRACK))) + + def activate_other_crosstrack_func(self, dist): + """ + Args: + dist: (N, ) numpy array, distances between ego-buggy and obstacle, + along the trajectory + Returns: + (N, ) numpy array, multiplier used to weigh the cross-track distance of + the obstacle into the passing offset calculation. + """ + return 1 / \ + (1 + np.exp(-(-self.ACTIVATE_OTHER_SCALE_ALONG_TRACK * dist + + self.OFFSET_SHIFT_ALONG_TRACK))) + + + def compute_traj( + self, + self_pose: Pose, + other_pose: Pose, + ) -> None: + """ + draw trajectory starting at the current pose and ending at a fixed distance + ahead. For each trajectory point, calculate the required offset perpendicular to the nominal + trajectory. A sigmoid function of the distance along track to the other buggy is used to + weigh the other buggy's cross-track distance. This calculation produces a line that + allows the ego-buggy's trajectory to go through the other buggy. Since we want to pass + the other buggy at some constant distance to the left, another sigmoid function is multiplied + by that constant distance to produce a smooth trajectory that passes the other buggy. + + Finally, the trajectory is bounded to the left by the left curb (if it exists), and to the right + by the nominal trajectory. (we never pass on the right) + + passing offsets = + activate_other_crosstrack_func(distance to other buggy along track) * + other buggy cross track distance + + offset_func(distance to other buggy along track) + + trajectory = nominal trajectory + + left nominal trajectory unit normal vector * + clamp(passing offsets, 0, distance from nominal trajectory to left curb) + + Args: + other_pose (Pose): Pose containing NAND's easting (x), + northing(y), and heading (theta), in UTM + + other_speed (float): speed in m/s of NAND + Publishes: + Trajectory: list of x,y coords for ego-buggy to follow. + """ + # grab slice of nominal trajectory + nominal_idx = self.nominal_traj.get_closest_index_on_path(self_pose.x, self_pose.y) + nominal_dist_along = self.nominal_traj.get_distance_from_index(nominal_idx) + + nominal_slice = np.empty((self.RESOLUTION, 2)) + + # compute the distance along nominal trajectory between samples and the obstacle + nominal_slice_dists = np.linspace( + nominal_dist_along + self.LOOKAHEAD, + nominal_dist_along + self.LOOKAHEAD + self.LOCAL_TRAJ_LEN, + self.RESOLUTION) + + for i in range(self.RESOLUTION): + nominal_slice[i, :] = np.array(self.nominal_traj.get_position_by_distance( + nominal_slice_dists[i] + )) + + # get index of the other buggy along the trajetory and convert to distance + other_idx = self.nominal_traj.get_closest_index_on_path(other_pose.x, other_pose.y) + other_dist = self.nominal_traj.get_distance_from_index(other_idx) + nominal_slice_to_other_dist = np.abs(nominal_slice_dists - other_dist) + + passing_offsets = self.offset_func(nominal_slice_to_other_dist) + + # compute signed cross-track distance between NAND and nominal + nominal_to_other = np.array((other_pose.x, other_pose.y)) - \ + np.array(self.nominal_traj.get_position_by_index(other_idx)) + + # dot product with the unit normal to produce left-positive signed distance + other_normal = self.nominal_traj.get_unit_normal_by_index(np.array(other_idx.ravel())) + other_cross_track_dist = np.sum( + nominal_to_other * other_normal, axis=1) + + self.other_buggy_xtrack_publisher.publish(Float64(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 + # if the sample point is near, we add cross track distance to the offset, + # such that the resulting offset is adjusted by position of NAND + + passing_offsets = passing_offsets + \ + self.activate_other_crosstrack_func(nominal_slice_to_other_dist) * other_cross_track_dist + + # clamp passing offset distances to distance to the curb + if self.left_curb is not None: + # grab slice of curb correponding to slice of nominal trajectory. + curb_idx = self.left_curb.get_closest_index_on_path(self_pose.x, self_pose.y) + curb_dist_along = self.left_curb.get_distance_from_index(curb_idx) + curb_idx_end = self.left_curb.get_closest_index_on_path(nominal_slice[-1, 0], nominal_slice[-1, 1]) + curb_dist_along_end = self.left_curb.get_distance_from_index(curb_idx_end) + curb_dists = np.linspace(curb_dist_along, curb_dist_along_end, self.RESOLUTION) + + curb_slice = np.empty((self.RESOLUTION, 2)) + for i in range(self.RESOLUTION): + curb_slice[i, :] = np.array(self.left_curb.get_position_by_distance( + curb_dists[i] + )) + + # compute distances from the sample points to the curb + nominal_slice_to_curb_dist = np.linalg.norm(curb_slice - nominal_slice, axis=1) + passing_offsets = np.minimum(passing_offsets, nominal_slice_to_curb_dist - self.CURB_MARGIN) + + # clamp negative passing offsets to zero, since we always pass on the left, + # the passing offsets should never pull SC to the right. + passing_offsets = np.maximum(0, passing_offsets) + + # shift the nominal slice by passing offsets + nominal_normals = self.nominal_traj.get_unit_normal_by_index( + self.nominal_traj.get_index_from_distance(nominal_slice_dists) + ) + positions = nominal_slice + (passing_offsets[:, None] * nominal_normals) + + local_traj = Trajectory(json_filepath=None, positions=positions) + self.traj_publisher.publish(local_traj.pack()) \ No newline at end of file From 7db20f15747ea7fefab9e7f4d2a0135f5f3226bc Mon Sep 17 00:00:00 2001 From: Jackack Date: Fri, 3 Jan 2025 17:07:18 +0800 Subject: [PATCH 2/9] todo: set trajectory filepath in main func --- .../buggy/buggy/path_planner/path_planner.py | 53 +++++++++++++++++-- 1 file changed, 49 insertions(+), 4 deletions(-) diff --git a/rb_ws/src/buggy/buggy/path_planner/path_planner.py b/rb_ws/src/buggy/buggy/path_planner/path_planner.py index 37af6337..485d4267 100644 --- a/rb_ws/src/buggy/buggy/path_planner/path_planner.py +++ b/rb_ws/src/buggy/buggy/path_planner/path_planner.py @@ -1,9 +1,11 @@ import sys +from threading import Lock + import numpy as np import rclpy from rclpy.node import Node -from sensor_msgs.msg import NavSatFix +from nav_msgs.msg import Odometry from std_msgs.msg import Float64 from TrajectoryMsg.msg import TrajectoryMsg @@ -38,13 +40,38 @@ class PathPlanner(Node): # number of points to sample along the nominal trajectory RESOLUTION = 150 + # frequency to run in hz + FREQUENCY = 10 + def __init__(self, nominal_traj:Trajectory, left_curb:Trajectory) -> None: super().__init__('path_planner') - self.other_buggy_xtrack_publisher = self.create_publisher(Float64, "/auton/debug/other_buggy_xtrack", 10) - self.traj_publisher = self.create_publisher(TrajectoryMsg, "/auton/trajectory", 10) + self.other_buggy_xtrack_publisher = self.create_publisher(Float64, "self/debug/other_buggy_xtrack", 10) + self.traj_publisher = self.create_publisher(TrajectoryMsg, "self/cur_traj", 10) + self.odom_subscriber = self.create_subscription(Odometry, 'self/state', self.odom_listener, 1) self.nominal_traj = nominal_traj self.left_curb = left_curb + self.timer = self.create_timer(1/self.FREQUENCY, self.timer_callback) + + # set up subscriber and callback for pose variables + self.self_pose = None + self.other_pose = None + + self.self_pose_lock = Lock() + self.other_pose_lock = Lock() + + def self_pose_callback(self, msg): + with self.self_pose_lock: + self.self_pose = Pose.rospose_to_pose(msg.pose.pose) + + def other_pose_callback(self, msg): + with self.other_pose_lock: + self.other_pose = Pose.rospose_to_pose(msg.pose.pose) + + def timer_callback(self): + with self.self_pose_lock and self.other_pose_lock: + if (self.self_pose is not None) and (self.other_pose is not None): + self.compute_traj(self.self_pose, self.other_pose) def offset_func(self, dist): """ @@ -181,4 +208,22 @@ 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()) \ No newline at end of file + self.traj_publisher.publish(local_traj.pack()) + + +def main(): + rclpy.init(args=None) + # TODO: set file paths here + # At the time of writing the below snippet, config management is TBD + + traj_filepath = "" + left_curb_filepath = "" + ref_trajectory = Trajectory(traj_filepath) + left_curb_trajectory = Trajectory(left_curb_filepath) + + path_planner = PathPlanner(ref_trajectory, left_curb_trajectory) + rclpy.spin(path_planner) + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file From 8f287be8e773101a16bb8c9b11711875799a57c8 Mon Sep 17 00:00:00 2001 From: Jackack Date: Fri, 3 Jan 2025 17:10:22 +0800 Subject: [PATCH 3/9] fixed subscribers --- rb_ws/src/buggy/buggy/path_planner/path_planner.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rb_ws/src/buggy/buggy/path_planner/path_planner.py b/rb_ws/src/buggy/buggy/path_planner/path_planner.py index 485d4267..b0c93045 100644 --- a/rb_ws/src/buggy/buggy/path_planner/path_planner.py +++ b/rb_ws/src/buggy/buggy/path_planner/path_planner.py @@ -47,7 +47,8 @@ def __init__(self, nominal_traj:Trajectory, left_curb:Trajectory) -> None: super().__init__('path_planner') self.other_buggy_xtrack_publisher = self.create_publisher(Float64, "self/debug/other_buggy_xtrack", 10) self.traj_publisher = self.create_publisher(TrajectoryMsg, "self/cur_traj", 10) - self.odom_subscriber = self.create_subscription(Odometry, 'self/state', self.odom_listener, 1) + self.self_pose_subscriber = self.create_subscription(Odometry, 'self/state', self.self_pose_callback, 1) + self.other_pose_subscriber = self.create_subscription(Odometry, 'other/state', self.other_pose_callback, 1) self.nominal_traj = nominal_traj self.left_curb = left_curb From 51414a9a4d6722e9080dc4a876aaee05e0db3202 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Sun, 5 Jan 2025 13:03:32 -0800 Subject: [PATCH 4/9] deprecation of std_msg --- README_ROBOBUGGY2.md | 4 +- .../buggy/buggy/controller/controller_node.py | 2 +- .../buggy/buggy/path_planner/path_planner.py | 37 +++++++++++++------ .../src/buggy/buggy/serial/ros_to_bnyahaj.py | 2 +- rb_ws/src/buggy/buggy/simulator/engine.py | 2 +- .../src/buggy/buggy/watchdog/watchdog_node.py | 2 +- rb_ws/src/buggy/launch/sim_2d_double.xml | 25 +++++++++++++ rb_ws/src/buggy/setup.py | 1 + 8 files changed, 57 insertions(+), 18 deletions(-) create mode 100644 rb_ws/src/buggy/launch/sim_2d_double.xml diff --git a/README_ROBOBUGGY2.md b/README_ROBOBUGGY2.md index d92d1421..6e48fe55 100755 --- a/README_ROBOBUGGY2.md +++ b/README_ROBOBUGGY2.md @@ -137,8 +137,8 @@ Ask Software Lead (WIP) - UTM coordinates (assume we're in Zone 17N): `/sim_2d/utm` (geometry_msgs/Pose - position.x = Easting meters , position.y = Northing meters, position.z = heading in degrees from East axis + is CCW) - INS Simulation: `/nav/odom` (nsg_msgs/Odometry) (**Noise** is implemented to vary ~1cm) Commands: -- Steering angle: `/buggy/steering` in degrees (std_msgs/Float64) -- Velocity: `/buggy/velocity` in m/s (std_msgs/Float64) +- Steering angle: `/buggy/steering` in degrees (example_interfaces/Float64) +- Velocity: `/buggy/velocity` in m/s (example_interfaces/Float64) ### Auton Logic Ask someone with experience (WIP) diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py index 700bef06..610204df 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/buggy/controller/controller_node.py @@ -6,7 +6,7 @@ import rclpy from rclpy.node import Node -from std_msgs.msg import Float32, Float64, Bool +from example_interfaces.msg import Float32, Float64, Bool from nav_msgs.msg import Odometry sys.path.append("/rb_ws/src/buggy/buggy") diff --git a/rb_ws/src/buggy/buggy/path_planner/path_planner.py b/rb_ws/src/buggy/buggy/path_planner/path_planner.py index b0c93045..c7742907 100644 --- a/rb_ws/src/buggy/buggy/path_planner/path_planner.py +++ b/rb_ws/src/buggy/buggy/path_planner/path_planner.py @@ -6,7 +6,7 @@ from rclpy.node import Node from nav_msgs.msg import Odometry -from std_msgs.msg import Float64 +from example_interfaces.msg import Float64 from TrajectoryMsg.msg import TrajectoryMsg sys.path.append("/rb_ws/src/buggy/buggy") @@ -43,15 +43,31 @@ class PathPlanner(Node): # frequency to run in hz FREQUENCY = 10 - def __init__(self, nominal_traj:Trajectory, left_curb:Trajectory) -> None: + def __init__(self) -> None: super().__init__('path_planner') + 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) + 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 + + #Publishers self.other_buggy_xtrack_publisher = self.create_publisher(Float64, "self/debug/other_buggy_xtrack", 10) self.traj_publisher = self.create_publisher(TrajectoryMsg, "self/cur_traj", 10) + + #Subscribers self.self_pose_subscriber = self.create_subscription(Odometry, 'self/state', self.self_pose_callback, 1) self.other_pose_subscriber = self.create_subscription(Odometry, 'other/state', self.other_pose_callback, 1) - self.nominal_traj = nominal_traj - self.left_curb = left_curb self.timer = self.create_timer(1/self.FREQUENCY, self.timer_callback) # set up subscriber and callback for pose variables @@ -212,18 +228,15 @@ def compute_traj( self.traj_publisher.publish(local_traj.pack()) -def main(): - rclpy.init(args=None) +def main(args=None): + rclpy.init(args=args) # TODO: set file paths here # At the time of writing the below snippet, config management is TBD - traj_filepath = "" - left_curb_filepath = "" - ref_trajectory = Trajectory(traj_filepath) - left_curb_trajectory = Trajectory(left_curb_filepath) - - path_planner = PathPlanner(ref_trajectory, left_curb_trajectory) + path_planner = PathPlanner() rclpy.spin(path_planner) + + path_planner.destroy_node() rclpy.shutdown() if __name__ == '__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 ca4bbea6..edb77ce8 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -7,7 +7,7 @@ from scipy.spatial.transform import Rotation import utm -from std_msgs.msg import Float64, Int8, UInt8, Bool +from example_interfaces.msg import Float64, Int8, UInt8, Bool from host_comm import * diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index e03c2c65..6cfd1004 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -4,7 +4,7 @@ import rclpy from rclpy.node import Node from geometry_msgs.msg import Pose, Twist, PoseWithCovariance, TwistWithCovariance -from std_msgs.msg import Float64 +from example_interfaces.msg import Float64 from sensor_msgs.msg import NavSatFix from nav_msgs.msg import Odometry import numpy as np diff --git a/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py b/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py index b0338da8..7a044f09 100644 --- a/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py +++ b/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py @@ -1,7 +1,7 @@ import rclpy from rclpy.node import Node -from std_msgs.msg import Bool +from example_interfaces.msg import Bool class Watchdog(Node): diff --git a/rb_ws/src/buggy/launch/sim_2d_double.xml b/rb_ws/src/buggy/launch/sim_2d_double.xml new file mode 100644 index 00000000..c256114e --- /dev/null +++ b/rb_ws/src/buggy/launch/sim_2d_double.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py index a8a022ec..03752caf 100644 --- a/rb_ws/src/buggy/setup.py +++ b/rb_ws/src/buggy/setup.py @@ -27,6 +27,7 @@ 'hello_world = buggy.hello_world:main', 'sim_single = buggy.simulator.engine:main', 'controller = buggy.controller.controller_node:main', + 'path_planner = buggy.path_planner.path_planner:main', 'buggy_state_converter = buggy.buggy_state_converter:main', 'watchdog = buggy.watchdog.watchdog_node:main', ], From 16ec98e8aafef837e1d5fe840ec0c1397c8f3e52 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Mon, 6 Jan 2025 22:19:33 -0800 Subject: [PATCH 5/9] moved packages --- .gitignore | 7 +++- Dockerfile | 12 +----- README_ROBOBUGGY2.md | 4 +- rb_ws/src/buggy/CMakeLists.txt | 40 +++++++++++++++++++ rb_ws/src/buggy/launch/controller.xml | 4 -- rb_ws/src/buggy/launch/sim_2d_double.xml | 21 ++++++---- rb_ws/src/buggy/launch/sim_2d_single.xml | 15 ++----- rb_ws/src/buggy/launch/watchdog.xml | 2 +- rb_ws/src/buggy/msg/TrajectoryMsg.msg | 4 -- rb_ws/src/buggy/package.xml | 25 +++++++----- rb_ws/src/buggy/resource/buggy | 0 .../buggy_state_converter.py | 0 .../controller/controller_node.py | 6 ++- .../controller/controller_superclass.py | 2 +- .../controller/stanley_controller.py | 2 +- .../buggy/{buggy => scripts}/hello_world.py | 4 +- .../path_planner/path_planner.py | 6 ++- .../{buggy => scripts}/serial/host_comm.py | 0 .../serial/ros_to_bnyahaj.py | 6 ++- .../{buggy => scripts}/simulator/engine.py | 7 ++-- .../{buggy => scripts}/util/constants.py | 0 .../src/buggy/{buggy => scripts}/util/pose.py | 0 .../{buggy => scripts}/util/trajectory.py | 0 .../watchdog/watchdog_node.py | 4 +- rb_ws/src/buggy/setup.cfg | 4 -- rb_ws/src/buggy/setup.py | 35 ---------------- rb_ws/src/buggy/test/test_copyright.py | 25 ------------ rb_ws/src/buggy/test/test_flake8.py | 25 ------------ rb_ws/src/buggy/test/test_pep257.py | 23 ----------- 29 files changed, 106 insertions(+), 177 deletions(-) create mode 100644 rb_ws/src/buggy/CMakeLists.txt delete mode 100644 rb_ws/src/buggy/launch/controller.xml delete mode 100644 rb_ws/src/buggy/msg/TrajectoryMsg.msg delete mode 100644 rb_ws/src/buggy/resource/buggy rename rb_ws/src/buggy/{buggy => scripts}/buggy_state_converter.py (100%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/controller/controller_node.py (98%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/controller/controller_superclass.py (96%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/controller/stanley_controller.py (99%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/hello_world.py (50%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/path_planner/path_planner.py (99%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/serial/host_comm.py (100%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/serial/ros_to_bnyahaj.py (98%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/simulator/engine.py (96%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/util/constants.py (100%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/util/pose.py (100%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/util/trajectory.py (100%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/watchdog/watchdog_node.py (96%) mode change 100644 => 100755 delete mode 100644 rb_ws/src/buggy/setup.cfg delete mode 100644 rb_ws/src/buggy/setup.py delete mode 100644 rb_ws/src/buggy/test/test_copyright.py delete mode 100644 rb_ws/src/buggy/test/test_flake8.py delete mode 100644 rb_ws/src/buggy/test/test_pep257.py diff --git a/.gitignore b/.gitignore index e8edffcc..2af114f4 100644 --- a/.gitignore +++ b/.gitignore @@ -1,10 +1,15 @@ .DS_Store -rb_ws/bags .docker-compose.yml.un~ .python-requirements.txt.un~ docker-compose.yml~ *TEMP_DO_NOT_EDIT.txt + +# BAGS +rb_ws/bags rb_ws/src/buggy/bags/* *.bag +rb_ws/rosbag2*/* + +# VISION .vision*/* vision/data*/* \ No newline at end of file diff --git a/Dockerfile b/Dockerfile index 6725e778..b855e3c5 100644 --- a/Dockerfile +++ b/Dockerfile @@ -23,16 +23,8 @@ RUN pip3 install -r python-requirements.txt RUN echo 'source "/opt/ros/humble/setup.bash" --' >> ~/.bashrc && \ echo 'cd rb_ws' >> ~/.bashrc && \ echo 'colcon build --symlink-install' >> ~/.bashrc && \ - echo 'source install/local_setup.bash' >> ~/.bashrc -# RUN echo 'source "/opt/ros/humble/setup.bash" --' >> ~/.bashrc && \ -# echo 'cd rb_ws' >> ~/.bashrc && \ -# echo 'catkin_make >/dev/null' >> ~/.bashrc && \ -# echo 'source devel/setup.bash' >> ~/.bashrc - - - -# RUN echo "exec firefox" > ~/.xinitrc && chmod +x ~/.xinitrc -# CMD ["x11vnc", "-create", "-forever"] + echo 'source install/local_setup.bash' >> ~/.bashrc && \ + echo 'cchmod -R +x src/buggy/scripts/' # add mouse to tmux RUN echo 'set -g mouse on' >> ~/.tmux.conf diff --git a/README_ROBOBUGGY2.md b/README_ROBOBUGGY2.md index 6e48fe55..d92d1421 100755 --- a/README_ROBOBUGGY2.md +++ b/README_ROBOBUGGY2.md @@ -137,8 +137,8 @@ Ask Software Lead (WIP) - UTM coordinates (assume we're in Zone 17N): `/sim_2d/utm` (geometry_msgs/Pose - position.x = Easting meters , position.y = Northing meters, position.z = heading in degrees from East axis + is CCW) - INS Simulation: `/nav/odom` (nsg_msgs/Odometry) (**Noise** is implemented to vary ~1cm) Commands: -- Steering angle: `/buggy/steering` in degrees (example_interfaces/Float64) -- Velocity: `/buggy/velocity` in m/s (example_interfaces/Float64) +- Steering angle: `/buggy/steering` in degrees (std_msgs/Float64) +- Velocity: `/buggy/velocity` in m/s (std_msgs/Float64) ### Auton Logic Ask someone with experience (WIP) diff --git a/rb_ws/src/buggy/CMakeLists.txt b/rb_ws/src/buggy/CMakeLists.txt new file mode 100644 index 00000000..3fc99baa --- /dev/null +++ b/rb_ws/src/buggy/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.8) +project(buggy) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) + +# Install Launch Files +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) + +# Install Python executables +install(PROGRAMS + scripts/hello_world.py + scripts/controller/controller_node.py + scripts/path_planner/path_planner.py + scripts/simulator/engine.py + scripts/watchdog/watchdog_node.py + scripts/buggy_state_converter.py + scripts/serial/ros_to_bnyahaj.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/controller.xml b/rb_ws/src/buggy/launch/controller.xml deleted file mode 100644 index 52f0ac7f..00000000 --- a/rb_ws/src/buggy/launch/controller.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ 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 c256114e..1fdaecd6 100644 --- a/rb_ws/src/buggy/launch/sim_2d_double.xml +++ b/rb_ws/src/buggy/launch/sim_2d_double.xml @@ -2,24 +2,29 @@ - + - - + + - + - - - + + + + + + + + \ 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 2c62b1b1..f0e72042 100755 --- a/rb_ws/src/buggy/launch/sim_2d_single.xml +++ b/rb_ws/src/buggy/launch/sim_2d_single.xml @@ -2,23 +2,14 @@ - + - - + + - - - - \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/watchdog.xml b/rb_ws/src/buggy/launch/watchdog.xml index 28c784df..5c74690e 100644 --- a/rb_ws/src/buggy/launch/watchdog.xml +++ b/rb_ws/src/buggy/launch/watchdog.xml @@ -1,4 +1,4 @@ - + \ No newline at end of file diff --git a/rb_ws/src/buggy/msg/TrajectoryMsg.msg b/rb_ws/src/buggy/msg/TrajectoryMsg.msg deleted file mode 100644 index 9c952ccf..00000000 --- a/rb_ws/src/buggy/msg/TrajectoryMsg.msg +++ /dev/null @@ -1,4 +0,0 @@ -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 e1471e2c..68bf6c04 100644 --- a/rb_ws/src/buggy/package.xml +++ b/rb_ws/src/buggy/package.xml @@ -2,18 +2,23 @@ buggy - 0.0.0 - TODO: Package description - root - TODO: License declaration + 2.0.0 + CMU's First Robotic Buggy + CMU's Robotic Club Officers + MIT - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest + ament_cmake + ament_cmake_python + + rclcpp + rclpy + ros2launch + ament_lint_auto + ament_lint_common + - ament_python + ament_cmake - + \ No newline at end of file diff --git a/rb_ws/src/buggy/resource/buggy b/rb_ws/src/buggy/resource/buggy deleted file mode 100644 index e69de29b..00000000 diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/scripts/buggy_state_converter.py old mode 100644 new mode 100755 similarity index 100% rename from rb_ws/src/buggy/buggy/buggy_state_converter.py rename to rb_ws/src/buggy/scripts/buggy_state_converter.py diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/scripts/controller/controller_node.py old mode 100644 new mode 100755 similarity index 98% rename from rb_ws/src/buggy/buggy/controller/controller_node.py rename to rb_ws/src/buggy/scripts/controller/controller_node.py index 117cf715..7d312133 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/scripts/controller/controller_node.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import threading import sys @@ -6,10 +8,10 @@ import rclpy from rclpy.node import Node -from example_interfaces.msg import Float32, Float64, Bool +from std_msgs.msg import Float32, Float64, Bool from nav_msgs.msg import Odometry -sys.path.append("/rb_ws/src/buggy/buggy") +sys.path.append("/rb_ws/src/buggy/scripts") from util.trajectory import Trajectory from controller.stanley_controller import StanleyController diff --git a/rb_ws/src/buggy/buggy/controller/controller_superclass.py b/rb_ws/src/buggy/scripts/controller/controller_superclass.py old mode 100644 new mode 100755 similarity index 96% rename from rb_ws/src/buggy/buggy/controller/controller_superclass.py rename to rb_ws/src/buggy/scripts/controller/controller_superclass.py index f960de75..731c96ee --- a/rb_ws/src/buggy/buggy/controller/controller_superclass.py +++ b/rb_ws/src/buggy/scripts/controller/controller_superclass.py @@ -4,7 +4,7 @@ from nav_msgs.msg import Odometry -sys.path.append("/rb_ws/src/buggy/buggy") +sys.path.append("/rb_ws/src/buggy/scripts") from util.trajectory import Trajectory class Controller(ABC): diff --git a/rb_ws/src/buggy/buggy/controller/stanley_controller.py b/rb_ws/src/buggy/scripts/controller/stanley_controller.py old mode 100644 new mode 100755 similarity index 99% rename from rb_ws/src/buggy/buggy/controller/stanley_controller.py rename to rb_ws/src/buggy/scripts/controller/stanley_controller.py index 34076535..4c5b10fd --- a/rb_ws/src/buggy/buggy/controller/stanley_controller.py +++ b/rb_ws/src/buggy/scripts/controller/stanley_controller.py @@ -5,7 +5,7 @@ from geometry_msgs.msg import Pose as ROSPose from nav_msgs.msg import Odometry -sys.path.append("/rb_ws/src/buggy/buggy") +sys.path.append("/rb_ws/src/buggy/scripts") from util.trajectory import Trajectory from controller.controller_superclass import Controller from util.pose import Pose diff --git a/rb_ws/src/buggy/buggy/hello_world.py b/rb_ws/src/buggy/scripts/hello_world.py old mode 100644 new mode 100755 similarity index 50% rename from rb_ws/src/buggy/buggy/hello_world.py rename to rb_ws/src/buggy/scripts/hello_world.py index 0a119aa0..0a2d9ba7 --- a/rb_ws/src/buggy/buggy/hello_world.py +++ b/rb_ws/src/buggy/scripts/hello_world.py @@ -1,5 +1,7 @@ +#!/usr/bin/env python3 + def main(): - print('Hi from buggy. Hello again') + print('Hi from buggy.') if __name__ == '__main__': diff --git a/rb_ws/src/buggy/buggy/path_planner/path_planner.py b/rb_ws/src/buggy/scripts/path_planner/path_planner.py old mode 100644 new mode 100755 similarity index 99% rename from rb_ws/src/buggy/buggy/path_planner/path_planner.py rename to rb_ws/src/buggy/scripts/path_planner/path_planner.py index c7742907..3357eee5 --- a/rb_ws/src/buggy/buggy/path_planner/path_planner.py +++ b/rb_ws/src/buggy/scripts/path_planner/path_planner.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import sys from threading import Lock @@ -6,10 +8,10 @@ from rclpy.node import Node from nav_msgs.msg import Odometry -from example_interfaces.msg import Float64 +from std_msgs.msg import Float64 from TrajectoryMsg.msg import TrajectoryMsg -sys.path.append("/rb_ws/src/buggy/buggy") +sys.path.append("/rb_ws/src/buggy/scripts") from util.pose import Pose from util.trajectory import Trajectory diff --git a/rb_ws/src/buggy/buggy/serial/host_comm.py b/rb_ws/src/buggy/scripts/serial/host_comm.py old mode 100644 new mode 100755 similarity index 100% rename from rb_ws/src/buggy/buggy/serial/host_comm.py rename to rb_ws/src/buggy/scripts/serial/host_comm.py diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py old mode 100644 new mode 100755 similarity index 98% rename from rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py rename to rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py index edb77ce8..2f8810fb --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py @@ -1,13 +1,15 @@ +#!/usr/bin/env python3 + import argparse from threading import Lock import threading import rclpy -from rclpy import Node +from rclpy.node import Node from scipy.spatial.transform import Rotation import utm -from example_interfaces.msg import Float64, Int8, UInt8, Bool +from std_msgs.msg import Float64, Int8, UInt8, Bool from host_comm import * diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py old mode 100644 new mode 100755 similarity index 96% rename from rb_ws/src/buggy/buggy/simulator/engine.py rename to rb_ws/src/buggy/scripts/simulator/engine.py index 84c9e393..26d82c67 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/scripts/simulator/engine.py @@ -4,13 +4,13 @@ import rclpy from rclpy.node import Node from geometry_msgs.msg import Pose, Twist, PoseWithCovariance, TwistWithCovariance -from example_interfaces.msg import Float64 +from std_msgs.msg import Float64 from sensor_msgs.msg import NavSatFix from nav_msgs.msg import Odometry import numpy as np import utm -sys.path.append("/rb_ws/src/buggy/buggy") -from rb_ws.src.buggy.buggy.util.constants import Constants +sys.path.append("/rb_ws/src/buggy/scripts") +from util.constants import Constants class Simulator(Node): @@ -176,6 +176,7 @@ def loop(self): if self.tick_count % self.interval == 0: self.publish() self.tick_count += 1 + self.get_logger().debug("SIMULATED UTM: ({}, {}), HEADING: {}".format(self.e_utm, self.n_utm, self.heading)) def main(args=None): diff --git a/rb_ws/src/buggy/buggy/util/constants.py b/rb_ws/src/buggy/scripts/util/constants.py old mode 100644 new mode 100755 similarity index 100% rename from rb_ws/src/buggy/buggy/util/constants.py rename to rb_ws/src/buggy/scripts/util/constants.py diff --git a/rb_ws/src/buggy/buggy/util/pose.py b/rb_ws/src/buggy/scripts/util/pose.py old mode 100644 new mode 100755 similarity index 100% rename from rb_ws/src/buggy/buggy/util/pose.py rename to rb_ws/src/buggy/scripts/util/pose.py diff --git a/rb_ws/src/buggy/buggy/util/trajectory.py b/rb_ws/src/buggy/scripts/util/trajectory.py old mode 100644 new mode 100755 similarity index 100% rename from rb_ws/src/buggy/buggy/util/trajectory.py rename to rb_ws/src/buggy/scripts/util/trajectory.py diff --git a/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py b/rb_ws/src/buggy/scripts/watchdog/watchdog_node.py old mode 100644 new mode 100755 similarity index 96% rename from rb_ws/src/buggy/buggy/watchdog/watchdog_node.py rename to rb_ws/src/buggy/scripts/watchdog/watchdog_node.py index 7a044f09..7c155e32 --- a/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py +++ b/rb_ws/src/buggy/scripts/watchdog/watchdog_node.py @@ -1,7 +1,9 @@ +#!/usr/bin/env python3 + import rclpy from rclpy.node import Node -from example_interfaces.msg import Bool +from std_msgs.msg import Bool class Watchdog(Node): diff --git a/rb_ws/src/buggy/setup.cfg b/rb_ws/src/buggy/setup.cfg deleted file mode 100644 index c2b78e07..00000000 --- a/rb_ws/src/buggy/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/buggy -[install] -install_scripts=$base/lib/buggy diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py deleted file mode 100644 index 03752caf..00000000 --- a/rb_ws/src/buggy/setup.py +++ /dev/null @@ -1,35 +0,0 @@ -from glob import glob -import os - -from setuptools import find_packages, setup - -package_name = 'buggy' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join("share", package_name), glob("launch/*.xml")) - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='root', - maintainer_email='root@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'hello_world = buggy.hello_world:main', - 'sim_single = buggy.simulator.engine:main', - 'controller = buggy.controller.controller_node:main', - 'path_planner = buggy.path_planner.path_planner:main', - 'buggy_state_converter = buggy.buggy_state_converter:main', - 'watchdog = buggy.watchdog.watchdog_node:main', - ], - }, -) \ No newline at end of file diff --git a/rb_ws/src/buggy/test/test_copyright.py b/rb_ws/src/buggy/test/test_copyright.py deleted file mode 100644 index 97a39196..00000000 --- a/rb_ws/src/buggy/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/rb_ws/src/buggy/test/test_flake8.py b/rb_ws/src/buggy/test/test_flake8.py deleted file mode 100644 index 27ee1078..00000000 --- a/rb_ws/src/buggy/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/rb_ws/src/buggy/test/test_pep257.py b/rb_ws/src/buggy/test/test_pep257.py deleted file mode 100644 index b234a384..00000000 --- a/rb_ws/src/buggy/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' From 1a2eb709719186bf1da01176511e1efb3f401d7d Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Mon, 6 Jan 2025 23:28:09 -0800 Subject: [PATCH 6/9] 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 3fc99baa..a8a51421 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 1fdaecd6..17eb93dd 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 00000000..9c952ccf --- /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 68bf6c04..26879e3d 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 7d312133..cad007ec 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 3357eee5..272f81da 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 72fbcaee..f674d90b 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 From c9c17d18e403f529dfe125336c23b69ea0c1963d Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Tue, 7 Jan 2025 15:54:50 -0800 Subject: [PATCH 7/9] updated engine to have a small delay --- rb_ws/src/buggy/launch/sim_2d_double.xml | 16 ++++++++-------- rb_ws/src/buggy/scripts/simulator/engine.py | 10 +++++++++- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/rb_ws/src/buggy/launch/sim_2d_double.xml b/rb_ws/src/buggy/launch/sim_2d_double.xml index 17eb93dd..b9d5ba61 100644 --- a/rb_ws/src/buggy/launch/sim_2d_double.xml +++ b/rb_ws/src/buggy/launch/sim_2d_double.xml @@ -1,29 +1,29 @@ - + - - + + - + - - + + - + - + diff --git a/rb_ws/src/buggy/scripts/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py index 26d82c67..f0fe89af 100755 --- a/rb_ws/src/buggy/scripts/simulator/engine.py +++ b/rb_ws/src/buggy/scripts/simulator/engine.py @@ -9,6 +9,8 @@ from nav_msgs.msg import Odometry import numpy as np import utm +import time + sys.path.append("/rb_ws/src/buggy/scripts") from util.constants import Constants @@ -130,7 +132,7 @@ def publish(self): with self.lock: p.position.x = self.e_utm p.position.y = self.n_utm - p.position.z = self.heading + p.position.z = float(self.heading) velocity = self.velocity self.plot_publisher.publish(p) @@ -182,6 +184,12 @@ def loop(self): def main(args=None): rclpy.init(args=args) sim = Simulator() + for i in range(500): + time.sleep(0.01) + sim.publish() + + + sim.get_logger().info("STARTED PUBLISHING") rclpy.spin(sim) sim.destroy_node() From 2fc4cdb1451b908fe6f9fcb33ce69b5d7a15f3a2 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Tue, 7 Jan 2025 16:02:52 -0800 Subject: [PATCH 8/9] pylinter --- rb_ws/src/buggy/scripts/util/trajectory.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rb_ws/src/buggy/scripts/util/trajectory.py b/rb_ws/src/buggy/scripts/util/trajectory.py index f674d90b..1bbdfb46 100755 --- a/rb_ws/src/buggy/scripts/util/trajectory.py +++ b/rb_ws/src/buggy/scripts/util/trajectory.py @@ -1,4 +1,5 @@ import json +import time # from buggy.msg import TrajectoryMsg @@ -7,7 +8,6 @@ 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 From be493a5d46f8118114f09ed0e2bd1c0bb7549e93 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Tue, 7 Jan 2025 16:04:26 -0800 Subject: [PATCH 9/9] i forgot to save??? pylinter --- rb_ws/src/buggy/scripts/path_planner/path_planner.py | 2 +- rb_ws/src/buggy/scripts/simulator/engine.py | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) 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 272f81da..2f91b1c2 100755 --- a/rb_ws/src/buggy/scripts/path_planner/path_planner.py +++ b/rb_ws/src/buggy/scripts/path_planner/path_planner.py @@ -66,7 +66,7 @@ def __init__(self) -> None: #Publishers self.other_buggy_xtrack_publisher = self.create_publisher(Float64, "self/debug/other_buggy_xtrack", 10) self.traj_publisher = self.create_publisher(TrajectoryMsg, "self/cur_traj", 10) - + #Subscribers self.self_pose_subscriber = self.create_subscription(Odometry, 'self/state', self.self_pose_callback, 1) self.other_pose_subscriber = self.create_subscription(Odometry, 'other/state', self.other_pose_callback, 1) diff --git a/rb_ws/src/buggy/scripts/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py index f0fe89af..af2bdc5b 100755 --- a/rb_ws/src/buggy/scripts/simulator/engine.py +++ b/rb_ws/src/buggy/scripts/simulator/engine.py @@ -1,6 +1,7 @@ #! /usr/bin/env python3 import threading import sys +import time import rclpy from rclpy.node import Node from geometry_msgs.msg import Pose, Twist, PoseWithCovariance, TwistWithCovariance @@ -9,7 +10,6 @@ from nav_msgs.msg import Odometry import numpy as np import utm -import time sys.path.append("/rb_ws/src/buggy/scripts") from util.constants import Constants @@ -184,12 +184,12 @@ def loop(self): def main(args=None): rclpy.init(args=args) sim = Simulator() - for i in range(500): + for _ in range(500): time.sleep(0.01) sim.publish() - - sim.get_logger().info("STARTED PUBLISHING") + + sim.get_logger().info("STARTED PUBLISHING") rclpy.spin(sim) sim.destroy_node()