diff --git a/src/rktl_autonomy/test/test_step_node b/src/rktl_autonomy/test/test_step_node index f8b89359..22711c0c 100755 --- a/src/rktl_autonomy/test/test_step_node +++ b/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) \ No newline at end of file diff --git a/src/rktl_planner/rktl_planner/nodes/path_planner.py b/src/rktl_planner/rktl_planner/nodes/path_planner.py index 28c6772e..96242a69 100755 --- a/src/rktl_planner/rktl_planner/nodes/path_planner.py +++ b/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() diff --git a/src/rktl_sim/rktl_sim/nodes/simulator b/src/rktl_sim/rktl_sim/nodes/simulator index eca0d8b0..cac5f39d 100755 --- a/src/rktl_sim/rktl_sim/nodes/simulator +++ b/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.""" diff --git a/src/rktl_sim/rktl_sim/vis_visualizer.py b/src/rktl_sim/rktl_sim/vis_visualizer.py index 41c7cb19..84188f17 100755 --- a/src/rktl_sim/rktl_sim/vis_visualizer.py +++ b/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,11 +43,11 @@ 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 @@ -54,7 +55,6 @@ def __init__(self): 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