Skip to content

Commit

Permalink
Merge branch 'ros-2-complete' of https://github.com/purdue-arc/rocket…
Browse files Browse the repository at this point in the history 
…_league into ros-2-complete
  • Loading branch information
@jcrm1
jcrm1 committed Apr 6, 2024
2 parents a193a29 + b0ef978 commit 724ed5f
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 30 deletions.
27 changes: 14 additions & 13 deletions src/rktl_game/rktl_game/nodes/game_manager
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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()
Expand Down
34 changes: 17 additions & 17 deletions src/rktl_sim/rktl_sim/sim_simulator.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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')
Expand All @@ -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():
Expand All @@ -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:
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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)
Expand All @@ -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
Expand Down

0 comments on commit 724ed5f

Please sign in to comment.