From 51414a9a4d6722e9080dc4a876aaee05e0db3202 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Sun, 5 Jan 2025 13:03:32 -0800 Subject: [PATCH] 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 d92d142..6e48fe5 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 700bef0..610204d 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 b0c9304..c774290 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 ca4bbea..edb77ce 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 e03c2c6..6cfd100 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 b0338da..7a044f0 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 0000000..c256114 --- /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 a8a022e..03752ca 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', ],