fixed merge conflict

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', queue_size=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, qos_profile=1)
-        self.node.create_subscription(topic_type, input_name, self.msg_cb, qos_profile=1)
+        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_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()

src/rktl_sim/rktl_sim/sim_simulator.py

@@ -62,8 +62,8 @@ def __init__(self):
         # setting config parameters (stay constant for the whole simulator run)
         # rospy.init_node('simulator')
         rclpy.init(args=sys.argv)
-        global node
-        node = rclpy.create_node("simulator")
+        #global node
+        self.node = rclpy.create_node("simulator")
 
 
         mode = self.get_sim_param('~mode')
@@ -73,31 +73,31 @@ def __init__(self):
             self.mode = SimulatorMode.REALISTIC
         else:
             #rospy.signal_shutdown('unknown sim mode set "{}"'.format(mode))
-            self.destroy_node('unknown sim mode set "{}"'.format(mode))
+            self.node.destroy_node()#f'unknown sim mode set "{mode}"')
 
 
         self.cmd_lock = Lock()
         self.reset_lock = Lock()
         self.car_cmd = (0.0, 0.0)
 
-        render_enabled = self.get_sim_param('~render', secondParam=False)
+        #render_enabled = self.get_sim_param('~render', secondParam=False)
         #self.status_pub = rospy.Publisher('match_status', MatchStatus, queue_size=1)
-        self.status_pub = node.create_publisher(MatchStatus, 'match_status', 1)
+        self.status_pub = self.node.create_publisher(MatchStatus, 'match_status', 1)
         self.ball_pose_pub, self.ball_odom_pub = None, None
         if self.mode == SimulatorMode.REALISTIC:
             #self.ball_pose_pub = rospy.Publisher('/ball/pose_sync_early', PoseWithCovarianceStamped, queue_size=1)
-            self.ball_pose_pub = node.create_publisher(PoseWithCovarianceStamped, '/ball/pose_sync_early', 1)
+            self.ball_pose_pub = self.node.create_publisher(PoseWithCovarianceStamped, '/ball/pose_sync_early', 1)
             #self.ball_odom_pub = rospy.Publisher('/ball/odom_truth', Odometry, queue_size=1)
-            self.ball_odom_pub = node.create_publisher(Odometry, '/ball/odom_truth', 1)
+            self.ball_odom_pub = self.node.create_publisher(Odometry, '/ball/odom_truth', 1)
         elif self.mode == SimulatorMode.IDEAL:
             #self.ball_odom_pub = rospy.Publisher('/ball/odom', Odometry, queue_size=1)
-            self.ball_odom_pub = node.create_publisher(Odometry, '/ball/odom', 1)
+            self.ball_odom_pub = self.node.create_publisher(Odometry, '/ball/odom', 1)
 
         # Services
         # rospy.Service('sim_reset_car_and_ball', Empty, self.reset_cb)
         # rospy.Service('sim_reset_ball', Empty, self.reset_ball_cb)
-        node.create_service(Empty, 'sim_reset_car_and_ball', self.reset_cb)
-        node.create_service(Empty, 'sim_reset_ball', self.reset_ball_cb)
+        self.node.create_service(Empty, 'sim_reset_car_and_ball', self.reset_cb)
+        self.node.create_service(Empty, 'sim_reset_ball', self.reset_ball_cb)
 
 
         while not rclpy.ok():
@@ -113,11 +113,11 @@ def check_urdf(self, urdf_path):
 
         if urdf_path is None:
             #rospy.signal_shutdown('no urdf path set for "{}"'.format(urdf_path))
-            self.destroy_node('no urdf path set for "{}"'.format(urdf_path))
+            self.node.destroy_node()
             try:
                 raise Exception('no urdf path set for "{}"'.format(urdf_path))
             except Exception as e:
-                node.get_logger().error(str(e))
+                self.node.get_logger().error(str(e))
                 rclpy.shutdown()
         i = 0
         while not os.path.isfile(urdf_path) and i < 5:
@@ -127,7 +127,7 @@ def check_urdf(self, urdf_path):
 
         if not os.path.isfile(urdf_path):
             #rospy.signal_shutdown('no urdf file exists at path {}'.format(urdf_path))
-            self.destroy_node('no urdf file exists at path {}'.format(urdf_path))
+            self.node.destroy_node()
 
 
     def effort_cb(self, effort_msg, car_id):
@@ -318,7 +318,7 @@ def get_sim_param(self, path, returnValue=False, secondParam=None):
                     #return rospy.get_param(f'{path}')
                     return self.node.declare_parameter(f'{path}')
 
-            type_rospy = type(rclpy_param)
+            type_rclpy = type(rclpy_param)
 
 
             if type_rclpy == dict:
@@ -404,7 +404,7 @@ def update_all_cars(self):
 
             if 'name' not in car_config:
                 #rospy.signal_shutdown('no "name" set for car config in sim')
-                self.destroy_node('no "name" set for car config in sim')
+                self.node.destroy_node()
 
             car_name = car_config['name']
             if 'randomize_pose' in car_config:
@@ -422,13 +422,13 @@ def update_all_cars(self):
                 # self.car_effort_subs[car_name] = rospy.Subscriber(
                 #     f'/cars/{car_name}/effort', ControlEffort,
                 #     self.effort_cb, callback_args=car_id)
-                self.car_effort_subs[car_name] = node.create_subscription(
+                self.car_effort_subs[car_name] = self.node.create_subscription(
                     ControlEffort, f'/cars/{car_name}/effort',
                     self.effort_cb, callback_args=car_id)
                 # self.car_cmd_subs[car_name] = rospy.Subscriber(
                 #     f'/cars/{car_name}/command', ControlCommand,
                 #     self.cmd_cb, callback_args=car_id)
-                self.car_cmd_subs[car_name] = node.create_subscription(
+                self.car_cmd_subs[car_name] = self.node.create_subscription(
                     ControlCommand, f'/cars/{car_name}/command',
 
                     self.cmd_cb, callback_args=car_id)
@@ -438,19 +438,19 @@ def update_all_cars(self):
                     # self.car_pose_pubs[car_name] = rospy.Publisher(
                     #     f'/cars/{car_name}/pose_sync_early',
                     #     PoseWithCovarianceStamped, queue_size=1)
-                    self.car_pose_pubs[car_name] = node.create_publisher(
+                    self.car_pose_pubs[car_name] = self.node.create_publisher(
                         PoseWithCovarianceStamped, f'/cars/{car_name}/pose_sync_early',
                         1)
 
                     # self.car_odom_pubs[car_name] = rospy.Publisher(
                     #     f'/cars/{car_name}/odom_truth', Odometry, queue_size=1)
-                    self.car_pose_pubs[car_name] = node.create_publisher(
+                    self.car_pose_pubs[car_name] = self.node.create_publisher(
                         Odometry, f'/cars/{car_name}/odom_truth', 1)
 
                 elif self.mode == SimulatorMode.IDEAL:
                     # self.car_odom_pubs[car_name] = rospy.Publisher(
                     #     f'/cars/{car_name}/odom', Odometry, queue_size=1)
-                    self.car_odom_pubs[car_name] = node.create_publisher(
+                    self.car_odom_pubs[car_name] = self.node.create_publisher(
                         Odometry, f'/cars/{car_name}/odom', 1)
 
         return True