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_game/launch/game.launch.py

@@ -3,6 +3,7 @@
 import launch_ros.actions
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from ament_index_python.packages import get_package_share_directory
+from launch.launch_description_sources import get_launch_description_from_any_launch_file
 
 
 def generate_launch_description():
@@ -14,9 +15,9 @@ def generate_launch_description():
             output='screen'
         ),
         launch.actions.IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
+            launch.launch_description_sources.AnyLaunchDescriptionSource(
                 os.path.join(get_package_share_directory(
-                    'rosbridge_server'), 'launch/rosbridge_websocket.launch.py')
+                    'rosbridge_server'), 'launch/rosbridge_websocket_launch.xml')
             )
         )
     ])

src/rktl_game/rktl_game/nodes/game_manager

@@ -13,7 +13,7 @@ import rclpy
 from std_msgs.msg import Bool, Header
 from nav_msgs.msg import Odometry
 from rktl_msgs.msg import Score, MatchStatus
-from std_srvs.srv import Empty, EmptyResponse
+from std_srvs.srv import Empty, Empty_Response
 
 class ScoreKeeper():
     def __init__(self):
@@ -71,7 +71,7 @@ class ScoreKeeper():
         self.enabled = False
         self.match_status = 0
         self.active_pub.publish(False)
-        return EmptyResponse()
+        return Empty_Response()
 
     def reset(self, _):
         self.orange_score = 0
@@ -83,13 +83,13 @@ class ScoreKeeper():
         header = Header()
         header.stamp = node.get_clock().now()
         self.match_status_pub.publish(MatchStatus(header, 0, self.game_clock, Score(0, 0)))
-        return EmptyResponse()
+        return Empty_Response()
 
     def unpause(self, _):
         self.enabled = True
         self.match_status = 1
         self.active_pub.publish(True)
-        return EmptyResponse()
+        return Empty_Response()
 
     def check_goal(self, ball_pose: Odometry):
         if self.enabled:

src/rktl_game/setup.py

@@ -23,7 +23,6 @@
     license='TODO: License declaration',
     tests_require=['pytest'],
     entry_points={
-        'console_scripts': [
-        ],
-    },
+        'console_scripts': [ ],
+    }
 )

src/rktl_launch/launch/rocket_league_sim.launch.py

@@ -54,10 +54,10 @@ def generate_launch_description():
                         os.path.join(get_package_share_directory(
                         'rktl_sim'), 'launch', 'visualizer.launch.py')
                     ),
-                    condition=launch.conditions.LaunchConfigurationEquals('render', 'true')
+                    condition=IfCondition(EqualsSubstitution(LaunchConfiguration('render'), 'realistic'))
                 ),
             ],
-            condition=launch.conditions.LaunchConfigurationEquals('sim_mode', 'realistic')
+            condition=IfCondition(EqualsSubstitution(LaunchConfiguration('sim_mode'), 'realistic'))
         ),
         launch_ros.actions.SetParametersFromFile(
             filename=os.path.join(get_package_share_directory(
@@ -68,22 +68,22 @@ def generate_launch_description():
             package='rktl_control',
             executable='topic_delay',
             name='pose_delay',
-            condition=launch.conditions.LaunchConfigurationEquals('sim_mode', 'realistic'),
+            condition=IfCondition(EqualsSubstitution(LaunchConfiguration('sim_mode'), 'realistic')),
             arguments=["pose_sync_early", "pose_sync", "geometry_msgs/PoseWithCovarianceStamped", launch.substitutions.LaunchConfiguration('perception_delay')]
         ),
         launch_ros.actions.Node(
             namespace='cars/car0',
             package='rktl_control',
             executable='topic_delay',
             name='pose_delay',
-            condition=launch.conditions.LaunchConfigurationEquals('sim_mode', 'realistic'),
+            condition=IfCondition(EqualsSubstitution(LaunchConfiguration('sim_mode'), 'realistic')),
             arguments=["pose_sync_early", "pose_sync", "geometry_msgs/PoseWithCovarianceStamped", launch.substitutions.LaunchConfiguration('perception_delay')]
         ),
         launch_ros.actions.Node(
             package='rqt_gui',
             executable='rqt_gui',
             name='rqt_gui',
-            condition=launch.conditions.LaunchConfigurationEquals('render', 'true'),
+            condition=IfCondition(EqualsSubstitution(LaunchConfiguration('render'), 'true')),
             arguments=['--perspective-file', os.path.join(get_package_share_directory(
                     'rktl_launch'), 'rqt','rktl.perspective')]
         ),

src/rktl_msgs/CMakeLists.txt

@@ -37,6 +37,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Path.msg"
   "msg/Score.msg"
   "msg/Waypoint.msg"
+  "srv/CreateBezierPath.srv"
   DEPENDENCIES geometry_msgs std_msgs builtin_interfaces # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg
 )
 

src/rktl_msgs/srv/CreateBezierPath.srv

@@ -0,0 +1,8 @@
+float64 velocity
+geometry_msgs/Pose[] target_poses
+builtin_interfaces/Duration[] target_durations
+builtin_interfaces/Duration bezier_segment_duration
+builtin_interfaces/Duration linear_segment_duration
+---
+rktl_msgs/BezierPath[] bezier_paths
+rktl_msgs/Path linear_path

src/rktl_planner/package.xml

@@ -35,4 +35,5 @@
     <export>
         <build_type>ament_python</build_type>
     </export>
+
 </package>

src/rktl_planner/rktl_planner/nodes/path_planner.py

@@ -8,8 +8,8 @@
 from nav_msgs.msg import Odometry
 from geometry_msgs.msg import Pose
 from rktl_msgs.msg import BezierPathList, Path
-from std_srvs.srv import Empty, EmptyRequest, EmptyResponse
-from rktl_planner.srv import CreateBezierPath, CreateBezierPathRequest
+from std_srvs.srv import Empty, Empty_Request, Empty_Response
+from rktl_msgs.srv import CreateBezierPath, CreateBezierPathRequest
 
 def create_simple_path_req(car_odom, ball_odom, goal_pos):
     req = CreateBezierPathRequest()
@@ -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