Merge branch 'ros-2-complete' of https://github.com/purdue-arc/rocket…

…_league into ros-2-complete

src/rktl_autonomy/rktl_autonomy/_ros_interface.py

@@ -87,13 +87,13 @@ def __init__(self, node_name='gym_interface', eval=False, launch_file=None, laun
             # initialize self
             os.environ['ROS_MASTER_URI'] = f'http://localhost:{port}'
             # rospy.init_node(node_name)
-            self.node = rclpy.Node(node_name)
+            self.node = rclpy.create_node(node_name)
             # let someone else take a turn
             os.remove(f'/tmp/{run_id}_launch')
         else:
             # use an existing ROS network
             # rospy.init_node(node_name)
-            self.node = rclpy.Node(node_name)
+            self.node = rclpy.create_node(node_name)
 
         # private variables
         self._cond = Condition()

src/rktl_autonomy/rktl_autonomy/plotter.py

@@ -18,7 +18,7 @@
 import matplotlib.pyplot as plt
 from numpy import append
 from os.path import expanduser, normpath
-class Plotter(rclpy.Node):
+class Plotter(rclpy.create_node):
     """Plot progress during training."""
     def __init__(self):
         #rospy.init_node('model_progress_plotter')

src/rktl_autonomy/rktl_autonomy/rocket_league_interface.py

@@ -109,7 +109,7 @@ def __init__(self, eval=False, launch_file=('rktl_autonomy', 'rocket_league_trai
 
         # Publishers
         self._command_pub = self.node.create_publisher(ControlCommand, 'cars/car0/command', 1)
-        self._reset_srv = self.node.create_client(Empty, 'sim_reset')
+        self._reset_srv = self.node.create_client(Empty, 'sim_reset', qos_profile=1)
 
         # State variables
         self._car_odom = None

src/rktl_autonomy/test/test_step_node

@@ -31,7 +31,7 @@ class TestStep(unittest.TestCase):
     def test_all(self):
         # initialize node and interface to code under test
         #rospy.init_node('rocket_league_agent')
-        self.node = rclpy.Node('rocket_league_agent')
+        self.node = rclpy.create_node('rocket_league_agent')
 
         #self.car_pub = rospy.Publisher('cars/car0/odom', Odometry, queue_size=1) 
         self.car_pub = self.node.create_publisher(Odometry, 'cars/car0/odom', 1)

src/rktl_control/rktl_control/keyboard_interface.py

@@ -67,7 +67,7 @@ def main():
     rclpy.init(args=sys.argv)
     node = rclpy.create_node('keyboard')
 
-    effort_pub = node.create_publisher(ControlEffort, 'effort', 1)
+    effort_pub = node.create_publisher(ControlEffort, 'effort')
     reset_srv = node.create_client(Empty, '/sim_reset')
 
     wrapper(main)

src/rktl_control/rktl_control/topic_delay.py

@@ -38,8 +38,8 @@ def __init__(self):
     #            msg_type = type
     #    assert msg_type is not None
 
-        pub = self.node.create_publisher(topic_type, output_name, queue_size=1)
-        self.node.create_subscription(topic_type, input_name, self.msg_cb)
+        pub = self.node.create_publisher(topic_type, output_name, 1)
+        self.node.create_subscription(topic_type, input_name, self.msg_cb, 1)
         self.buffer = deque()
 
         while rclpy.ok():

src/rktl_control/test/test_mean_filter_node

@@ -52,7 +52,7 @@ class TestMeanFilter(unittest.TestCase):
         rclpy.init(args=sys.argv)
         self.node = rclpy.create_node('test_mean_filter_node')
 
-        pub = self.node.create_publisher(PoseWithCovarianceStamped, 'pose_sync', queue_size=1)
+        pub = self.node.create_publisher(PoseWithCovarianceStamped, 'pose_sync')
         self.node.create_subscription(Odometry, 'odom', self.odom_cb)
 
         # member variables used for test

src/rktl_control/test/test_sync_node

@@ -47,9 +47,9 @@ class TestSync(unittest.TestCase):
         rclpy.init(args=sys.args)
         self.node = rclpy.create_node('test_sync_node')
 
-        car0_pub = self.node.create_publisher(PoseWithCovarianceStamped, 'cars/car0/pose', queue_size=0, latch=True)
-        car1_pub = self.node.create_publisher(PoseWithCovarianceStamped, 'cars/car1/pose', queue_size=0, latch=True)
-        ball_pub = self.node.create_publisher(PoseWithCovarianceStamped, 'ball/pose', queue_size=0, latch=True)
+        car0_pub = self.node.create_publisher(PoseWithCovarianceStamped, 'cars/car0/pose')
+        car1_pub = self.node.create_publisher(PoseWithCovarianceStamped, 'cars/car1/pose')
+        ball_pub = self.node.create_publisher(PoseWithCovarianceStamped, 'ball/pose')
 
         self.node.create_subscription(PoseWithCovarianceStamped, 'cars/car0/pose_sync', self.car0_cb)
         self.node.create_subscription(PoseWithCovarianceStamped, 'cars/car1/pose_sync', self.car1_cb)

src/rktl_game/rktl_game/nodes/game_manager

@@ -10,6 +10,7 @@ License:
 
 # ROS
 import rclpy
+from rclpy.parameter import Parameter
 from std_msgs.msg import Bool, Header
 from nav_msgs.msg import Odometry
 from rktl_msgs.msg import Score, MatchStatus
@@ -23,30 +24,30 @@ class ScoreKeeper():
 
         self.orange_score = 0
         self.blue_score = 0
-        self.orange_goal = (node.get_parameter('/field/length', 1) - 0.15) / 2 
-        self.blue_goal = (node.get_parameter('/field/length', 1)  - 0.15) / -2
+        self.orange_goal = (node.get_parameter_or('/field/length', Parameter(1)).get_parameter_value().double_value - 0.15) / 2 
+        self.blue_goal = (node.get_parameter_or('/field/length', Parameter(1)).get_parameter_value().double_value  - 0.15) / -2
         self.enabled = False
         self.match_status = 0
-        self.game_clock = node.get_parameter('/game_length', 90)
-        self.manager_rate = node.get_parameter('manager_rate', 10)
+        self.game_clock = node.get_parameter_or('/game_length', Parameter(90))
+        self.manager_rate = node.get_parameter_or('manager_rate', Parameter(10))
 
         # Publishers
-        self.match_status_pub = node.create_publisher(MatchStatus, 'match_status', queue_size=1)
-        self.active_pub = node.create_publisher(Bool, 'cars/enable', queue_size=1)
+        self.match_status_pub = node.create_publisher(MatchStatus, 'match_status', qos_profile=1)
+        self.active_pub = node.create_publisher(Bool, 'cars/enable', qos_profile=1)
 
         # Subscribers
-        node.create_subscription(Odometry, 'ball/odom', self.check_goal)
-        node.create_subscription(Bool, 'cars/enable', self.enable)
+        node.create_subscription(Odometry, 'ball/odom', self.check_goal, qos_profile=1)
+        node.create_subscription(Bool, 'cars/enable', self.enable, qos_profile=1)
 
         # Services
-        node.create_service('reset_game', Empty, self.reset)
-        node.create_service('unpause_game', Empty, self.unpause)
-        node.create_service('pause_game', Empty, self.pause)
+        node.create_service(srv_type=Empty, srv_name='reset_game', callback=self.reset)
+        node.create_service(srv_type=Empty, srv_name='unpause_game', callback=self.unpause)
+        node.create_service(srv_type=Empty, srv_name='pause_game', callback=self.pause)
 
         # Counts loops and decrements game clock every second
         self.timer_ctr = 0
         # main loop
-        rate = node.create_rate(self.manager_rate)
+        rate = node.create_rate(self.manager_rate.get_parameter_value().double_value)
         while rclpy.ok():
             if self.match_status == 1:
                 self.timer_ctr += 1
@@ -78,7 +79,7 @@ class ScoreKeeper():
         self.blue_score = 0
         self.enabled = False
         self.match_status = 0
-        self.game_clock = node.get_parameter('/game_length', 90)
+        self.game_clock = node.get_parameter_or('/game_length', Parameter(90)).get_parameter_value().double_value
         self.active_pub.publish(False)
         header = Header()
         header.stamp = node.get_clock().now()

src/rktl_perception/nodes/pose_to_tf

@@ -12,7 +12,7 @@ from geometry_msgs.msg import PoseWithCovarianceStamped, TransformStamped
 from tf2_ros import TransformBroadcaster
 
 
-class Publisher(rclpy.Node):
+class Publisher(rclpy.create_node):
     """Workaround class until TF is implemented"""
 
     def __init__(self):

src/rktl_perception/nodes/projector

@@ -15,7 +15,7 @@ from rktl_dependencies.transformations import translation_matrix, quaternion_mat
 import numpy as np
 
 
-class Projector(rclpy.Node):
+class Projector(rclpy.create_node):
     """Class to synchronize and buffer all poses."""
 
     def __init__(self):

src/rktl_planner/rktl_planner/nodes/__init__.py

@@ -5,9 +5,9 @@
   All rights reserved.
 """
 
-from path_follower import PathFollower
-from path_planner import PathPlanner
-from patrol_planner import PatrolPlanner
+from .path_follower import PathFollower
+from .path_planner import PathPlanner
+from .patrol_planner import PatrolPlanner
 
 __all__ = [
     "PathFollower",

src/rktl_planner/rktl_planner/nodes/bezier_path_server.py

@@ -4,7 +4,7 @@
 import rclpy
 import math
 from rktl_dependencies.transformations import euler_from_quaternion, quaternion_from_euler
-from rktl_planner.srv import CreateBezierPath, CreateBezierPathRequest, CreateBezierPathResponse
+from rktl_msgs.srv import CreateBezierPath
 from rktl_planner import BezierPath
 from rktl_msgs.msg import Path as PathMsg, Waypoint as WaypointMsg
 from geometry_msgs.msg import Pose, Point, Vector3
@@ -53,7 +53,7 @@ def create_segment(start_pose: Pose, end_pose: Pose, velocity: float,
     return segments
 
 
-def bezier_path_server(req: CreateBezierPathRequest):
+def bezier_path_server(req: CreateBezierPath.Request):
     velocity = req.velocity
     bezier_segments = []
     durations = []
@@ -70,7 +70,7 @@ def bezier_path_server(req: CreateBezierPathRequest):
             else:
                 durations.append(segment.duration.to_sec())
 
-    res = CreateBezierPathResponse()
+    res = CreateBezierPath.Response()
     res.bezier_paths = [x.to_msg() for x in bezier_segments]
     res.linear_path = PathMsg()
     res.linear_path.velocity = velocity
@@ -96,8 +96,11 @@ def bezier_path_server(req: CreateBezierPathRequest):
             idx += 1
     return res
 
-if __name__ == '__main__':
+def main():
     rclpy.init()
     node = rclpy.create_node('bezier_path_server')
     service = node.create_service(CreateBezierPath, 'create_bezier_path', bezier_path_server)
-    rclpy.spin()
+    rclpy.spin()
+
+if __name__ == '__main__':
+    main()

src/rktl_planner/rktl_planner/nodes/path_follower.py

@@ -26,32 +26,31 @@ class PathFollower(object):
     def __init__(self):
         rclpy.init()
         global node
-        node = rclpy.Node('path_follower')
+        node = rclpy.create_node('path_follower')
 
         self.path_start_time = None
         self.path = None
         self.last_pose_idx = None
         self.start_time = None
         self.max_speed = None
 
-        self.frame_id = node.declare_parameter('~frame_id', 'map')
+        self.frame_id = node.get_parameter_or('~frame_id', rclpy.Parameter('map')).get_parameter_value().string_value
 
         # Max speed to travel path
-        self.max_speed = node.declare_parameter('~max_speed', 0.1)
+        self.max_speed = node.get_parameter_or('~max_speed', rclpy.Parameter(0.1)).get_parameter_value().double_value
 
         # Radius to search for intersections
-        self.lookahead_dist = node.declare_parameter('~lookahead_dist', 0.15)
+        self.lookahead_dist = node.get_parameter_or('~lookahead_dist', rclpy.Parameter(0.15)).get_parameter_value().double_value
 
         # Coeffient to adjust lookahead distance by speed
-        self.lookahead_gain = node.declare_parameter('~lookahead_gain', 0.035)
+        self.lookahead_gain = node.get_parameter_or('~lookahead_gain', rclpy.Parameter(.055)).get_parameter_value().double_value
 
         # Number of waypoints to search per pass (-1 is full path)
-        self.lookahead_pnts = node.declare_parameter('~lookahead_pnts', -1)
+        self.lookahead_pnts = node.get_parameter_or('~lookahead_pnts', rclpy.Parameter(-1)).get_parameter_value().integer_value
 
         # Publishers
-        car_name = node.declare_parameter('~car_name')
-        self.bot_velocity_cmd = node.create_publisher(ControlCommand, f'/cars/{car_name}/command', 1)
-
+        car_name = node.get_parameter('~car_name').get_parameter_value().string_value
+
         # Subscribers
         node.create_subscription(Odometry, f'/cars/{car_name}/odom', self.odom_cb, qos_profile=1)
         node.create_subscription(Path, 'linear_path', self.path_cb, qos_profile=1)
@@ -137,6 +136,8 @@ def odom_cb(self, odom_msg: Odometry):
             cmd.curvature = -(1.0 / turn_rad)
             self.bot_velocity_cmd.publish(cmd)
 
+def main():
+    PathFollower()
 
 if __name__ == "__main__":
-    PathFollower()
+    main()

src/rktl_planner/rktl_planner/nodes/path_planner.py

@@ -148,5 +148,8 @@ def reset(self, _: Empty_Request):
             self.bezier_path_pub.publish(bezier_path_list)
         return Empty_Response()
 
-if __name__ == '__main__':
+def main():
     PathPlanner()
+
+if __name__ == '__main__':
+    main()

src/rktl_planner/rktl_planner/nodes/patrol_planner.py

@@ -12,7 +12,7 @@
 from rktl_msgs.msg import ControlCommand
 
 import math, time
-from rktl_dependencies.rktl_dependencies.angles import shortest_angular_distance as sad
+from rktl_dependencies.angles import shortest_angular_distance as sad
 
 class PatrolPlanner(object):
     """A very simple strategy for rktl. Patrol around the field in a circle and
@@ -285,5 +285,8 @@ def control_curvature(self, error):
         self.prev_error = error
         return curvature
 
-if __name__ == "__main__":
+def main():
     PatrolPlanner()
+
+if __name__ == "__main__":
+    main()