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

…_league into ros-2-complete

src/rktl_autonomy/test/test_step_node

@@ -12,7 +12,7 @@ import unittest, rostest, rclpy
 from rclpy.duration import Duration
 from nav_msgs.msg import Odometry
 from rktl_msgs.msg import MatchStatus
-from std_srvs.srv import Empty, EmptyResponse
+from std_srvs.srv import Empty, Empty_Response
 from rosgraph_msgs.msg import Clock
 import numpy as np
 from rktl_autonomy.rocket_league_interface import RocketLeagueInterface
@@ -164,7 +164,7 @@ class TestStep(unittest.TestCase):
     def reset(self, __):
         """Do nothing."""
         self.assertTrue(self.want_reset, msg='env reset called when unexpected')
-        return EmptyResponse()
+        return Empty_Response()
 
 if __name__ == '__main__':
     rostest.rosrun('rktl_autonomy', 'test_rktl_rewards', TestStep)

src/rktl_planner/rktl_planner/nodes/path_planner.py

@@ -137,7 +137,7 @@ def car_odom_cb(self, data: Odometry):
     def ball_odom_cb(self, data: Odometry):
         self.ball_odom = data
 
-    def reset(self, _: EmptyRequest):
+    def reset(self, _: Empty_Request):
         req = self.path_req(self.car_odom, self.ball_odom, self.goal_pos)
         res = self.path_client(req)
         if self.linear_path_pub:
@@ -146,7 +146,7 @@ def reset(self, _: EmptyRequest):
             bezier_path_list = BezierPathList()
             bezier_path_list.paths = res.bezier_paths
             self.bezier_path_pub.publish(bezier_path_list)
-        return EmptyResponse()
+        return Empty_Response()
 
 if __name__ == '__main__':
     PathPlanner()

src/rktl_sim/rktl_sim/nodes/simulator

@@ -17,7 +17,7 @@ import time
 
 #import rospy
 
-from std_srvs.srv import Empty, EmptyResponse
+from std_srvs.srv import Empty, Empty_Response
 from threading import Lock
 from enum import Enum
 
@@ -171,7 +171,7 @@ class Simulator(object):
 
         self.last_time = None
         self.reset_lock.release()
-        return EmptyResponse()
+        return Empty_Response()
 
     def reset_ball_cb(self, _):
         """Resets the ball sensor noise and pose WITHOUT resetting the whole sim."""
@@ -183,7 +183,7 @@ class Simulator(object):
         self.ball_init_speed = self.get_sim_param('/ball/init_speed')
 
         self.sim.reset_ball()
-        return EmptyResponse()
+        return Empty_Response()
 
     def loop_once(self):
         """Step the simulation once step, updating match status, moving and publishing new car and ball positions."""

src/rktl_sim/rktl_sim/vis_visualizer.py

@@ -15,6 +15,7 @@
 from nav_msgs.msg import Odometry
 import rclpy
 import sys
+from rclpy.exceptions import ROSInterruptException
 
 from rktl_dependencies.transformations import euler_from_quaternion
 
@@ -42,19 +43,18 @@ def __init__(self):
         # goal_width = rospy.get_param('/field/goal/width')
         # wall_thickness = rospy.get_param('/field/wall_thickness')
         # ball_radius = rospy.get_param("/ball/radius")
-        field_width = node.declare_parameter('/field/width').value
-        field_length = node.declare_parameter('/field/length').value
-        goal_width = node.declare_parameter('/field/goal/width').value
-        wall_thickness = node.declare_parameter('/field/wall_thickness').value
-        ball_radius = node.declare_parameter("/ball/radius").value
+        field_width = node.declare_parameter('/field/width').get_parameter_value().double_value
+        field_length = node.declare_parameter('/field/length').get_parameter_value().double_value
+        goal_width = node.declare_parameter('/field/goal/width').get_parameter_value().double_value
+        wall_thickness = node.declare_parameter('/field/wall_thickness').get_parameter_value().double_value
+        ball_radius = node.declare_parameter("/ball/radius").get_parameter_value().double_value
 
 
 
         # Creating pygame render
 
         self.window = visualizer.Window(
             field_width, field_length, wall_thickness,
-            # rospy.get_param('~window_name', 'Rocket League Visualizer'))
             node.declare_parameter('~window_name', 'Rocket League Visualizer').value)
 
         # Collecting private parameters
@@ -65,7 +65,7 @@ def __init__(self):
         # car_length = rospy.get_param("~cars/body_length")
         self.frame_id = node.declare_parameter("~frame_id", "map").value
         self.timeout = node.declare_parameter("~timeout", 10).value
-        rate = rclpy.create_node('simple_node').create_rate((node.declare_parameter("~rate", 20).value))
+        rate = rclpy.create_node('simple_node').create_rate((node.declare_parameter("~rate", 20).get_parameter_value().double_value))
         car_width = node.declare_parameter("~cars/body_width").value
         car_length = node.declare_parameter("~cars/body_length").value
 
@@ -169,11 +169,11 @@ def __init__(self):
         # rospy.Subscriber("/cars/car0/lookahead_pnt", Float32, self.lookahead_cb)
         # rospy.Subscriber("/agents/agent0/bezier_path", BezierPathList, self.bezier_path_cb)
 
-        node.create_subscription(Odometry, "/ball/odom", self.ball_odom_cb)
-        node.create_subscription(Odometry, "/cars/car0/odom", self.car_odom_cb)
-        node.create_subscription(Path, "/cars/car0/path", self.path_arr_cb)
-        node.create_subscription(Float32, "/cars/car0/lookahead_pnt", self.lookahead_cb)
-        node.create_subscription(BezierPathList, "/agents/agent0/bezier_path", self.bezier_path_cb)
+        node.create_subscription(Odometry, "/ball/odom", self.ball_odom_cb, qos_profile=1)
+        node.create_subscription(Odometry, "/cars/car0/odom", self.car_odom_cb, qos_profile=1)
+        node.create_subscription(Path, "/cars/car0/path", self.path_arr_cb, qos_profile=1)
+        node.create_subscription(Float32, "/cars/car0/lookahead_pnt", self.lookahead_cb, qos_profile=1)
+        node.create_subscription(BezierPathList, "/agents/agent0/bezier_path", self.bezier_path_cb, qos_profile=1)
 
 
         while not rclpy.ok():
@@ -183,7 +183,7 @@ def __init__(self):
                 exit()
             try:
                 rate.sleep()
-            except rclpy.ROSInterruptException:
+            except ROSInterruptException:
                 pass