diff --git a/src/rktl_game/rktl_game/nodes/game_manager b/src/rktl_game/rktl_game/nodes/game_manager index 2a6852e3..932e4f0e 100755 --- a/src/rktl_game/rktl_game/nodes/game_manager +++ b/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') - self.active_pub = node.create_publisher(Bool, 'cars/enable') + 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() diff --git a/src/rktl_sim/rktl_sim/sim_simulator.py b/src/rktl_sim/rktl_sim/sim_simulator.py index aebc9701..db45e534 100755 --- a/src/rktl_sim/rktl_sim/sim_simulator.py +++ b/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(): @@ -117,7 +117,7 @@ def check_urdf(self, urdf_path): 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: @@ -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: @@ -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