From b18438480ccec801dc092c281acce4f5741b5d97 Mon Sep 17 00:00:00 2001 From: Harrison McCarty Date: Thu, 14 Apr 2022 22:06:19 -0400 Subject: [PATCH 1/9] added multi-car support to sim --- rktl_sim/config/simulation.yaml | 20 +++-- rktl_sim/nodes/simulator | 127 +++++++++++++++++++------------- rktl_sim/src/simulator/car.py | 18 +++-- rktl_sim/src/simulator/sim.py | 56 ++++++++------ 4 files changed, 134 insertions(+), 87 deletions(-) diff --git a/rktl_sim/config/simulation.yaml b/rktl_sim/config/simulation.yaml index f49ad9f9f..0f79eb0c9 100644 --- a/rktl_sim/config/simulation.yaml +++ b/rktl_sim/config/simulation.yaml @@ -1,12 +1,20 @@ rate: 10 spawn_height: 0.06 -car: - # init_pose: - # pos: [0.5, 0.0, 0.06] - sensor_noise: - pos: [0.01, 0.01, 0.00] - orient: [0.0, 0.0, 0.09] +cars: + - name: "car0" + # init_pose: + # pos: [0.5, 0.0, 0.06] + sensor_noise: + pos: [0.01, 0.01, 0.00] + orient: [0.0, 0.0, 0.09] + + - name: "car1" + # init_pose: + # pos: [0.5, 0.0, 0.06] + sensor_noise: + pos: [0.01, 0.01, 0.00] + orient: [0.0, 0.0, 0.09] ball: init_speed: 0.0 diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index 15ff10288..b808ba2c8 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -105,23 +105,39 @@ class SimulatorROS(object): car_properties['steering_rate'] = rospy.get_param('/cars/steering/rate') car_properties['simulate_effort'] = self.mode == SimulatorMode.REALISTIC - noise = rospy.get_param('~car/sensor_noise', None) - if self.mode == SimulatorMode.IDEAL: - noise = None + self.car_ids = {} + self.car_pose_pubs = {} + self.car_odom_pubs = {} + self.car_subs = {} + car_configs = rospy.get_param('~cars', None) + for car_config in car_configs: + init_pose = None + if 'init_pose' in car_config: + init_pose = car_config['init_pose'] - self.sim.create_car('car', - init_pose=rospy.get_param('~car/init_pose', None), - noise=noise, - props=car_properties) - - if self.mode == SimulatorMode.REALISTIC: - self.car_pose_pub = rospy.Publisher('/cars/car0/pose_sync_early', - PoseWithCovarianceStamped, queue_size=1) - rospy.Subscriber('/cars/car0/effort', ControlEffort, self.effort_cb) - self.car_odom_pub = rospy.Publisher('/cars/car0/odom_truth', Odometry, queue_size=1) - elif self.mode == SimulatorMode.IDEAL: - self.car_odom_pub = rospy.Publisher('/cars/car0/odom', Odometry, queue_size=1) - rospy.Subscriber('/cars/car0/command', ControlCommand, self.cmd_cb) + noise = None + if 'sensor_noise' in car_config and self.mode != SimulatorMode.IDEAL: + noise = car_config['sensor_noise'] + + if 'name' not in car_config: + rospy.signal_shutdown('no "name" set for car config in sim') + car_name = car_config['name'] + + self.car_ids[car_name] = self.sim.create_car( + init_pose=init_pose, noise=noise, props=car_properties, + ) + + self.car_odom_pubs = rospy.Publisher(f'/cars/{car_name}/odom', + Odometry, queue_size=1) + rospy.Subscriber(f'/cars/{car_name}/effort', ControlEffort, + self.effort_cb, callback_args=self.car_ids[car_name]) + rospy.Subscriber(f'/cars/{car_name}/command', Odometry, + self.cmd_cb, callback_args=self.car_ids[car_name]) + + if self.mode == SimulatorMode.REALISTIC: + self.car_pose_pubs[car_name] = rospy.Publisher( + f'/cars/{car_name}/pose_sync_early', + PoseWithCovarianceStamped, queue_size=1) # Node data self.cmd_lock = Lock() @@ -152,14 +168,16 @@ class SimulatorROS(object): rospy.signal_shutdown( 'no urdf file exists at path {}'.format(param)) - def effort_cb(self, effort_msg): + def effort_cb(self, effort_msg, car_id): self.cmd_lock.acquire() - self.car_cmd = (effort_msg.throttle, effort_msg.steering) + self.sim.setCarCommand(car_id, + (effort_msg.throttle, effort_msg.steering)) self.cmd_lock.release() - def cmd_cb(self, cmd_msg): + def cmd_cb(self, cmd_msg, car_id): self.cmd_lock.acquire() - self.car_cmd = (cmd_msg.velocity, cmd_msg.curvature) + self.sim.setCarCommand(car_id, + (cmd_msg.velocity, cmd_msg.curvature)) self.cmd_lock.release() def reset_cb(self, _): @@ -189,7 +207,7 @@ class SimulatorROS(object): status.status = MatchStatus.ONGOING self.status_pub.publish(status) - self.sim.step(self.car_cmd, delta_t) + self.sim.step(delta_t) if self.mode == SimulatorMode.REALISTIC: # Publish ball sync pose @@ -207,18 +225,20 @@ class SimulatorROS(object): self.ball_pose_pub.publish(ball_msg) # Publish car sync pose - car_msg = PoseWithCovarianceStamped() - car_msg.header.stamp = now - car_msg.header.frame_id = self.frame_id - car_pos, car_quat = self.sim.getCarPose(add_noise=True) - car_msg.pose.pose.position.x = car_pos[0] - car_msg.pose.pose.position.y = car_pos[1] - car_msg.pose.pose.position.z = car_pos[2] - car_msg.pose.pose.orientation.x = car_quat[0] - car_msg.pose.pose.orientation.y = car_quat[1] - car_msg.pose.pose.orientation.z = car_quat[2] - car_msg.pose.pose.orientation.w = car_quat[3] - self.car_pose_pub.publish(car_msg) + for car_name in self.car_ids: + car_msg = PoseWithCovarianceStamped() + car_msg.header.stamp = now + car_msg.header.frame_id = self.frame_id + car_pos, car_quat = self.sim.getCarPose( + self.car_ids[car_name], add_noise=True) + car_msg.pose.pose.position.x = car_pos[0] + car_msg.pose.pose.position.y = car_pos[1] + car_msg.pose.pose.position.z = car_pos[2] + car_msg.pose.pose.orientation.x = car_quat[0] + car_msg.pose.pose.orientation.y = car_quat[1] + car_msg.pose.pose.orientation.z = car_quat[2] + car_msg.pose.pose.orientation.w = car_quat[3] + self.car_pose_pubs[car_name].publish(car_msg) # Publish ball odometry ball_msg = Odometry() @@ -242,25 +262,28 @@ class SimulatorROS(object): self.ball_odom_pub.publish(ball_msg) # Publish bot odometry - car_msg = Odometry() - car_msg.header.stamp = now - car_msg.header.frame_id = self.frame_id - car_pos, car_quat = self.sim.getCarPose() - car_msg.pose.pose.position.x = car_pos[0] - car_msg.pose.pose.position.y = car_pos[1] - car_msg.pose.pose.position.z = car_pos[2] - car_msg.pose.pose.orientation.x = car_quat[0] - car_msg.pose.pose.orientation.y = car_quat[1] - car_msg.pose.pose.orientation.z = car_quat[2] - car_msg.pose.pose.orientation.w = car_quat[3] - car_linear, car_angular = self.sim.getCarVelocity() - car_msg.twist.twist.linear.x = car_linear[0] - car_msg.twist.twist.linear.y = car_linear[1] - car_msg.twist.twist.linear.z = car_linear[2] - car_msg.twist.twist.angular.x = car_angular[0] - car_msg.twist.twist.angular.y = car_angular[1] - car_msg.twist.twist.angular.z = car_angular[2] - self.car_odom_pub.publish(car_msg) + for car_name in self.car_ids: + car_msg = Odometry() + car_msg.header.stamp = now + car_msg.header.frame_id = self.frame_id + car_pos, car_quat = self.sim.getCarPose( + self.car_ids[car_name]) + car_msg.pose.pose.position.x = car_pos[0] + car_msg.pose.pose.position.y = car_pos[1] + car_msg.pose.pose.position.z = car_pos[2] + car_msg.pose.pose.orientation.x = car_quat[0] + car_msg.pose.pose.orientation.y = car_quat[1] + car_msg.pose.pose.orientation.z = car_quat[2] + car_msg.pose.pose.orientation.w = car_quat[3] + car_linear, car_angular = self.sim.getCarVelocity( + self.car_ids[car_name]) + car_msg.twist.twist.linear.x = car_linear[0] + car_msg.twist.twist.linear.y = car_linear[1] + car_msg.twist.twist.linear.z = car_linear[2] + car_msg.twist.twist.angular.x = car_angular[0] + car_msg.twist.twist.angular.y = car_angular[1] + car_msg.twist.twist.angular.z = car_angular[2] + self.car_odom_pubs[car_name].publish(car_msg) self.last_time = now self.reset_lock.release() diff --git a/rktl_sim/src/simulator/car.py b/rktl_sim/src/simulator/car.py index 106b956d1..3d948b543 100644 --- a/rktl_sim/src/simulator/car.py +++ b/rktl_sim/src/simulator/car.py @@ -39,15 +39,23 @@ def __init__(self, carID, pos, orient, car_properties): p.resetJointState(self.id, self.joint_ids[1], targetValue=pos[1]) p.resetJointState(self.id, self.joint_ids[2], targetValue=orient[2]) - def step(self, cmd, dt): + self.cmd = None + + def setCmd(self, cmd): + self.cmd = cmd + + def step(self, dt): + if self.cmd is None: + return + # get current yaw angle _, orient = self.getPose() theta = p.getEulerFromQuaternion(orient)[2] if self.simulate_effort: # transfrom control input to reference angles and velocities - v_rear_ref = cmd[0] * self._MAX_SPEED - psi_ref = cmd[1] * self._STEERING_THROW + v_rear_ref = self.cmd[0] * self._MAX_SPEED + psi_ref = self.cmd[1] * self._STEERING_THROW # update rear wheel velocity using 1st order model self._v_rear = (self._v_rear - v_rear_ref) * math.exp(-dt/self._THROTTLE_TAU) + v_rear_ref @@ -68,11 +76,11 @@ def step(self, cmd, dt): math.sqrt(math.pow(math.tan(self._psi), 2.0) / 4.0 + 1.0) omega = self._v_rear * math.tan(self._psi) / self._LENGTH else: - body_vel = cmd[0] + body_vel = self.cmd[0] if abs(body_vel) > self._MAX_SPEED: body_vel = math.copysign(self._MAX_SPEED, body_vel) - body_curv = cmd[1] + body_curv = self.cmd[1] if abs(body_curv) > self._MAX_CURVATURE: body_curv = math.copysign(self._MAX_CURVATURE, body_curv) diff --git a/rktl_sim/src/simulator/sim.py b/rktl_sim/src/simulator/sim.py index 0ee2c94a2..2ccf0740d 100644 --- a/rktl_sim/src/simulator/sim.py +++ b/rktl_sim/src/simulator/sim.py @@ -104,6 +104,7 @@ def __init__(self, urdf_paths, field_setup, spawn_bounds, render_enabled): self._walls[brBackwallID] = True self._cars = {} + self._car_data = {} self._ballID = None self.touched_last = None @@ -155,7 +156,7 @@ def create_car(self, urdf_name, init_pose=None, noise=None, props=None): if urdf_name in self.urdf_paths: zeroPos = [0.0, 0.0, 0.0] zeroOrient = p.getQuaternionFromEuler([0.0, 0.0, 0.0]) - self._carID = p.loadURDF(self.urdf_paths[urdf_name], zeroPos, zeroOrient) + carID = p.loadURDF(self.urdf_paths[urdf_name], zeroPos, zeroOrient) if init_pose: if "pos" in init_pose: carPos = init_pose["pos"] @@ -167,8 +168,8 @@ def create_car(self, urdf_name, init_pose=None, noise=None, props=None): else: carOrient = zeroOrient - self.initCarPos = carPos - self.initCarOrient = carOrient + initCarPos = carPos + initCarOrient = carOrient else: carPos = [ random.uniform(self.spawn_bounds[0][0], self.spawn_bounds[0][1]), @@ -176,20 +177,25 @@ def create_car(self, urdf_name, init_pose=None, noise=None, props=None): random.uniform(self.spawn_bounds[2][0], self.spawn_bounds[2][1]), ] carOrient = [0.0, 0.0, random.uniform(0, 2 * math.pi)] - self.initCarPos = None - self.initCarOrient = None - self._cars[self._carID] = Car( - self._carID, + initCarPos = None + initCarOrient = None + + self._cars[carID] = Car( + carID, carPos, carOrient, props ) - self.car_noise = noise - return self._carID + self._car_data[carID] = { + "posInit": initCarPos, + "orientInit": initCarOrient, + "noise": noise, + } + return carID else: return None - def step(self, car_cmd, dt): + def step(self, dt): """Advance one time-step in the sim.""" if self._ballID is not None: ballContacts = p.getContactPoints(bodyA=self._ballID) @@ -208,27 +214,29 @@ def step(self, car_cmd, dt): for _ in range(round(dt / p_dt)): # Step kinematic objects independently, at max possible rate for car in self._cars.values(): - car.step(car_cmd, p_dt) + car.step(p_dt) p.stepSimulation() - def getCarPose(self, add_noise=False): - cars = list(self._cars.values()) - if len(cars) == 0: + def getCarPose(self, id, add_noise=False): + if id not in self._cars: return None - # TODO: Provide translation from ARC IDs to Sim IDs if add_noise: - return cars[0].getPose(noise=self.car_noise) + return self._cars[id].getPose(noise=self.car_noise) else: - return cars[0].getPose(noise=None) + return self._cars[id].getPose(noise=None) + + def getCarVelocity(self, id): + if id not in self._cars: + return None + + return self._cars[id].getVelocity() - def getCarVelocity(self): - cars = list(self._cars.values()) - if len(cars) == 0: + def setCarCommand(self, id, cmd): + if id not in self._cars: return None - # TODO: Provide translation from ARC IDs to Sim IDs - return cars[0].getVelocity() + return self._cars[id].setCmd(cmd) def getBallPose(self, add_noise=False): if self._ballID is None: @@ -270,8 +278,8 @@ def reset(self): p.resetBaseVelocity(self._ballID, ballVel, [0, 0, 0]) for car in self._cars.values(): - carPos = self.initCarPos - carOrient = self.initCarOrient + carPos = self._car_data[car.id]["initPos"] + carOrient = self._car_data[car.id]["initOrient "] if carPos is None: carPos = [random.uniform(self.spawn_bounds[0][0], self.spawn_bounds[0][1]), From 24be24f5827e63209b58704a65f8078b02cef393 Mon Sep 17 00:00:00 2001 From: Harrison McCarty Date: Fri, 15 Apr 2022 14:18:02 -0400 Subject: [PATCH 2/9] modified visualizer for multi-car support --- rktl_launch/launch/rocket_league_sim.launch | 4 + rktl_sim/nodes/simulator | 6 +- rktl_sim/nodes/visualizer | 161 ++++++++------------ rktl_sim/src/simulator/sim.py | 3 +- rktl_sim/src/visualizer/__init__.py | 3 +- rktl_sim/src/visualizer/asset.py | 13 +- rktl_sim/src/visualizer/window.py | 38 +---- 7 files changed, 96 insertions(+), 132 deletions(-) diff --git a/rktl_launch/launch/rocket_league_sim.launch b/rktl_launch/launch/rocket_league_sim.launch index 906678b77..f557d65c0 100644 --- a/rktl_launch/launch/rocket_league_sim.launch +++ b/rktl_launch/launch/rocket_league_sim.launch @@ -47,6 +47,10 @@ + + + + diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index b808ba2c8..fb17de72d 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -124,10 +124,10 @@ class SimulatorROS(object): car_name = car_config['name'] self.car_ids[car_name] = self.sim.create_car( - init_pose=init_pose, noise=noise, props=car_properties, - ) + 'car', init_pose=init_pose, noise=noise, props=car_properties) - self.car_odom_pubs = rospy.Publisher(f'/cars/{car_name}/odom', + car_id = self.car_ids[car_name] + self.car_odom_pubs[car_name] = rospy.Publisher(f'/cars/{car_name}/odom', Odometry, queue_size=1) rospy.Subscriber(f'/cars/{car_name}/effort', ControlEffort, self.effort_cb, callback_args=self.car_ids[car_name]) diff --git a/rktl_sim/nodes/visualizer b/rktl_sim/nodes/visualizer index 9538cf34f..e3a001987 100755 --- a/rktl_sim/nodes/visualizer +++ b/rktl_sim/nodes/visualizer @@ -32,14 +32,13 @@ class VisualizerROS(object): # Collecting global parameters field_width = rospy.get_param('/field/width') field_length = rospy.get_param('/field/length') - goal_width = rospy.get_param('/field/goal/width') wall_thickness = rospy.get_param('/field/wall_thickness') + goal_width = rospy.get_param('/field/goal/width') ball_radius = rospy.get_param("/ball/radius") car_width = rospy.get_param("/cars/width") car_length = rospy.get_param("/cars/length") # Creating pygame render - self.window = visualizer.Window( field_width, field_length, wall_thickness, rospy.get_param('~window_name', 'Rocket League Visualizer')) @@ -50,99 +49,67 @@ class VisualizerROS(object): rate = rospy.Rate(rospy.get_param("~rate", 20)) # Setting up field - id = 0 field_img_path = rospy.get_param("~media/field", None) if field_img_path is not None: - self.window.createAsset( - id, field_width, field_length, imgPath=field_img_path, - initPos=(0, 0)) - id += 1 - - # Setting up sidewalls - sidewall_length = field_length + (wall_thickness * 2.0) - self.window.createAsset( - id, wall_thickness, sidewall_length, color=(0, 0, 0), - initPos=(0., (field_width + wall_thickness) / 2.)) - id += 1 - - self.window.createAsset( - id, wall_thickness, sidewall_length, color=(0, 0, 0), - initPos=(0., -(field_width + wall_thickness) / 2.)) - id += 1 - - # Setting up backwalls - backwall_width = (field_width - goal_width) / 2. - backwall_x = (field_length + wall_thickness) / 2. - backwall_y = (field_width / 2.) - (backwall_width / 2.) - self.window.createAsset( - id, backwall_width, wall_thickness, color=(0, 0, 0), - initPos=(backwall_x, backwall_y)) - id += 1 - - self.window.createAsset( - id, backwall_width, wall_thickness, color=(0, 0, 0), - initPos=(backwall_x, -backwall_y)) - id += 1 - - self.window.createAsset( - id, backwall_width, wall_thickness, color=(0, 0, 0), - initPos=(-backwall_x, backwall_y)) - id += 1 - - self.window.createAsset( - id, backwall_width, wall_thickness, color=(0, 0, 0), - initPos=(-backwall_x, -backwall_y)) - id += 1 + asset = visualizer.Image( + self.window.scaleSize(field_width), + self.window.scaleSize(field_length), + field_img_path) + x, y = self.window.transformPos(0, 0) + asset.setPos(x, y) + self.window.addAsset(asset) # Setting up car car_img_path = rospy.get_param("~media/car", None) - if car_img_path is not None: - self.car_id = id - self.window.createAsset( - self.car_id, car_width, car_length, imgPath=car_img_path) - id += 1 # Setting up ball ball_img_path = rospy.get_param("~media/ball", None) - if ball_img_path is not None: - self.ball_id = id - self.window.createAsset( - self.ball_id, ball_radius * 2, ball_radius * 2, imgPath=ball_img_path) - id += 1 # Setting up goals - self.goal1_id = id - self.window.createAsset( - self.goal1_id, goal_width, wall_thickness, color=(255, 255, 255)) - self.window.updateAssetPos( - self.goal1_id, (field_length / 2) + (wall_thickness / 2), 0) - id += 1 - - self.goal2_id = id - self.window.createAsset( - self.goal2_id, goal_width, wall_thickness, color=(255, 255, 255)) - self.window.updateAssetPos( - self.goal2_id, -((field_length / 2) + (wall_thickness / 2)), 0) - id += 1 - - self.lines_id = 11 - self.window.createAsset(self.lines_id, 0, 0, color=(255, 0, 0), lines=True) - self.circle_id = 12 - self.window.createAsset(self.circle_id, 0, 0, circle=True, color=(255,0,0), radius=0) + asset = visualizer.Rectangle( self.window.scaleSize(goal_width), + self.window.scaleSize(wall_thickness), (255,255,255)) + x, y = self.window.transformPos((field_length / 2) + (wall_thickness / 2), 0) + asset.setPos(x, y) + self.window.addAsset(asset) + + asset = visualizer.Rectangle( self.window.scaleSize(goal_width), + self.window.scaleSize(wall_thickness), (255,255,255)) + x, y = self.window.transformPos(-((field_length / 2) + (wall_thickness / 2)), 0) + asset.setPos(x, y) + self.window.addAsset(asset) self.lock = Lock() self.last_time = None self.path = None # Subscribers - rospy.Subscriber("/ball/odom", Odometry, self.ball_odom_cb) - rospy.Subscriber("/cars/car0/odom", Odometry, self.car_odom_cb) - rospy.Subscriber("/cars/car0/path", Path, self.path_arr_cb) - rospy.Subscriber( - "/cars/car0/lookahead_pnt", Float32, self.lookahead_cb - ) - rospy.Subscriber("/agents/agent0/bezier_path", BezierPathList, self.bezier_path_cb) + topics = rospy.get_published_topics() + for topic_name, _ in topics: + topic_ns = topic_name.split('/')[1:] + asset = None + if topic_ns[0] == "cars" and topic_ns[2] == "odom": + if car_img_path is not None: + width = self.window.scaleSize(car_width) + length = self.window.scaleSize(car_length) + asset = visualizer.Image(width, length, car_img_path) + rospy.Subscriber(topic_name, Odometry, + self.car_odom_cb, asset) + elif topic_ns[0] == "agents" and topic_ns[2] == "bezier_path": + asset = visualizer.Lines((255,0,0)) + rospy.Subscriber(topic_name, BezierPathList, + self.bezier_path_cb, asset) + elif topic_ns[0] == "cars" and topic_ns[2] == "lookahead_pnt": + asset = visualizer.Circle((255,0,0), 0) + rospy.Subscriber(topic_name, Float32, + self.lookahead_cb, asset) + elif topic_ns[0] == "ball" and topic_ns[1] == "odom": + diameter = self.window.scaleSize(ball_radius * 2) + asset = visualizer.Image(diameter, diameter, ball_img_path) + rospy.Subscriber(topic_name, Odometry, + self.ball_odom_cb, asset) + if asset != None: + self.window.addAsset(asset) while not rospy.is_shutdown(): try: @@ -154,46 +121,48 @@ class VisualizerROS(object): except rospy.ROSInterruptException: pass - def car_odom_cb(self, odom_msg): + def car_odom_cb(self, odom_msg, asset): x = odom_msg.pose.pose.position.x y = odom_msg.pose.pose.position.y + x, y = self.window.transformPos(x, y) + asset.setPos(x, y) + orient = odom_msg.pose.pose.orientation quat = [orient.x, orient.y, orient.z, orient.w] heading = euler_from_quaternion(quat)[2] heading = heading * 180. / math.pi - self.window.updateAssetPos(self.car_id, x, y) - self.window.updateAssetAngle(self.car_id, heading) - self.window.updateAssetPos(self.circle_id, x, y) + asset.setAngle(heading) - def ball_odom_cb(self, odom_msg): + def ball_odom_cb(self, odom_msg, asset): x = odom_msg.pose.pose.position.x y = odom_msg.pose.pose.position.y - self.window.updateAssetPos(self.ball_id, x, y) + x, y = self.window.transformPos(x, y) + asset.setPos(x, y) - def path_arr_cb(self, path_msg: Path): + def path_arr_cb(self, path_msg: Path, asset): """Callback for path array messages.""" - self.window.resetAssetLines(self.lines_id) - self.path = path_msg.waypoint - for point in self.path: + asset.resetPoints() + path = path_msg.waypoint + for point in path: x = point.pose.position.x y = point.pose.position.y - self.window.updateAssetPos(self.lines_id, x, y) + x, y = self.window.transformPos(x, y) + asset.setPos(x, y) - def bezier_path_cb(self, msg: BezierPathList): + def bezier_path_cb(self, msg: BezierPathList, asset): """Callback for bezier path messages.""" - self.window.resetAssetLines(self.lines_id) + asset.resetPoints() paths = [BezierPath(x) for x in msg.paths] for path in paths: - #for point in path.bezier_curve.control_points: - # self.window.updateAssetPos(self.lines_id, point.x, point.y) sec = path.duration.to_sec() for t in np.linspace(0., sec, int(50 * sec + 0.5)): point = path.at(t) - self.window.updateAssetPos(self.lines_id, point.x, point.y) + x, y = self.window.transformPos(point.x, point.y) + asset.setPos(x, y) - def lookahead_cb(self, msg: Float32): + def lookahead_cb(self, msg: Float32, asset): """Callback for lookahead_pnt messages.""" - self.window.updateAssetRadius(self.circle_id, msg.data) + asset.setRadius(self.window.scaleSize(msg.data)) if __name__ == "__main__": VisualizerROS() diff --git a/rktl_sim/src/simulator/sim.py b/rktl_sim/src/simulator/sim.py index 2ccf0740d..ec4fe49a1 100644 --- a/rktl_sim/src/simulator/sim.py +++ b/rktl_sim/src/simulator/sim.py @@ -221,8 +221,9 @@ def getCarPose(self, id, add_noise=False): if id not in self._cars: return None + noise = self._car_data[id]['noise'] if add_noise: - return self._cars[id].getPose(noise=self.car_noise) + return self._cars[id].getPose(noise=noise) else: return self._cars[id].getPose(noise=None) diff --git a/rktl_sim/src/visualizer/__init__.py b/rktl_sim/src/visualizer/__init__.py index 60ba054b6..26306caa2 100644 --- a/rktl_sim/src/visualizer/__init__.py +++ b/rktl_sim/src/visualizer/__init__.py @@ -29,5 +29,6 @@ """ from visualizer.window import Window +from visualizer.asset import Image, Rectangle, Lines, Circle -__all__ = ["Window",] +__all__ = ["Window", "Image", "Rectangle", "Lines", "Circle"] \ No newline at end of file diff --git a/rktl_sim/src/visualizer/asset.py b/rktl_sim/src/visualizer/asset.py index 32e45b4c6..e75067835 100644 --- a/rktl_sim/src/visualizer/asset.py +++ b/rktl_sim/src/visualizer/asset.py @@ -8,7 +8,13 @@ # 3rd Party Modules import pygame from abc import ABC, abstractmethod +from enum import Enum +class AssetType(Enum): + ImageAsset = 0 + RectangleAsset = 1 + LinesAsset = 2 + CircleAsset = 3 class Asset(ABC): @abstractmethod @@ -19,7 +25,6 @@ def setPos(self): def blit(self): pass - class Image(Asset): def __init__(self, width, length, img_path): self.width = width @@ -33,6 +38,8 @@ def __init__(self, width, length, img_path): self.img = self.init_img def setPos(self, x, y): + x = int(x) + y = int(y) self.pos = (x, y) def setAngle(self, angle): @@ -52,6 +59,8 @@ def __init__(self, width, length, color): self.rect = pygame.Rect(0, 0, width, length) def setPos(self, x, y): + x = int(x) + y = int(y) self.pos = (x, y) self.rect.center = self.pos @@ -80,6 +89,8 @@ def __init__(self, color, radius): self.pos = (0, 0) def setPos(self, x, y): + x = int(x) + y = int(y) self.pos = (x, y) def setRadius(self, radius): diff --git a/rktl_sim/src/visualizer/window.py b/rktl_sim/src/visualizer/window.py index 6e0ed6b72..b8b9e4ea6 100644 --- a/rktl_sim/src/visualizer/window.py +++ b/rktl_sim/src/visualizer/window.py @@ -31,47 +31,25 @@ def __init__(self, map_width, map_length, wall_thickness, name='Rocket League Vi self.window_width = int( (map_width + (wall_thickness*2.)) * self.scaling) - self.assets = {} + self.assets = [] pygame.display.init() pygame.display.set_caption(name) self._screen = pygame.display.set_mode( (self.window_width, self.window_length)) - def createAsset(self, id, width, length, initPos=None, imgPath=None, color=None, radius=None, lines=False, circle=False): - width = int(width * self.scaling) - length = int(length * self.scaling) + def addAsset(self, asset): + self.assets.append(asset) - if lines: - self.assets[id] = Lines(color) - elif circle: - radius = int(radius * self.scaling) - self.assets[id] = Circle(color, radius) - elif imgPath is None: - self.assets[id] = Rectangle(width, length, color) - else: - self.assets[id] = Image(width, length, imgPath) - - if initPos is not None: - self.updateAssetPos(id, initPos[0], initPos[1]) - - def updateAssetPos(self, id, x, y): - # Adjust for simulation coordinate frame + def transformPos(self, x, y): x = self.window_length - \ (int(x * self.scaling) + (self.window_length // 2)) y = self.window_width - \ (int(y * self.scaling) + (self.window_width // 2)) - self.assets[id].setPos(y, x) - - def updateAssetRadius(self, id, radius): - radius = int(radius * self.scaling) - self.assets[id].setRadius(radius) + return y, x - def updateAssetAngle(self, id, angle): - self.assets[id].setAngle(angle) - - def resetAssetLines(self, id): - self.assets[id].resetPoints() + def scaleSize(self, s): + return s * self.scaling def show(self): for event in pygame.event.get(): @@ -80,7 +58,7 @@ def show(self): raise self.ShutdownError() self._screen.fill(self.BACKGROUND_COLOR) - for asset in self.assets.values(): + for asset in self.assets: asset.blit(self._screen) pygame.display.flip() From 823b910cf561454d44cabe622afbfec5379ac659 Mon Sep 17 00:00:00 2001 From: Harrison McCarty Date: Fri, 15 Apr 2022 17:57:40 -0400 Subject: [PATCH 3/9] fixed a few rendering issues and refactored old code --- rktl_launch/launch/rocket_league_sim.launch | 8 -- rktl_sim/nodes/simulator | 19 +++-- rktl_sim/nodes/visualizer | 86 +++++++++++++-------- rktl_sim/src/visualizer/asset.py | 6 +- rktl_sim/src/visualizer/window.py | 9 ++- 5 files changed, 74 insertions(+), 54 deletions(-) diff --git a/rktl_launch/launch/rocket_league_sim.launch b/rktl_launch/launch/rocket_league_sim.launch index f557d65c0..9d1a2ea61 100644 --- a/rktl_launch/launch/rocket_league_sim.launch +++ b/rktl_launch/launch/rocket_league_sim.launch @@ -11,14 +11,6 @@ - - - - - - - - diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index fb17de72d..f997f8fac 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -92,9 +92,11 @@ class SimulatorROS(object): if self.mode == SimulatorMode.REALISTIC: self.ball_pose_pub = rospy.Publisher('/ball/pose_sync_early', PoseWithCovarianceStamped, queue_size=1) - self.ball_odom_pub = rospy.Publisher('/ball/odom_truth', Odometry, queue_size=1) + self.ball_odom_pub = rospy.Publisher('/ball/odom_truth', + Odometry, queue_size=1) elif self.mode == SimulatorMode.IDEAL: - self.ball_odom_pub = rospy.Publisher('/ball/odom', Odometry, queue_size=1) + self.ball_odom_pub = rospy.Publisher('/ball/odom', + Odometry, queue_size=1) # Creating a single car car_properties = {} @@ -103,7 +105,7 @@ class SimulatorROS(object): car_properties['throttle_tau'] = rospy.get_param('/cars/throttle/tau') car_properties['steering_throw'] = rospy.get_param('/cars/steering/max_throw') car_properties['steering_rate'] = rospy.get_param('/cars/steering/rate') - car_properties['simulate_effort'] = self.mode == SimulatorMode.REALISTIC + car_properties['simulate_effort'] = (self.mode == SimulatorMode.REALISTIC) self.car_ids = {} self.car_pose_pubs = {} @@ -127,17 +129,20 @@ class SimulatorROS(object): 'car', init_pose=init_pose, noise=noise, props=car_properties) car_id = self.car_ids[car_name] - self.car_odom_pubs[car_name] = rospy.Publisher(f'/cars/{car_name}/odom', - Odometry, queue_size=1) rospy.Subscriber(f'/cars/{car_name}/effort', ControlEffort, - self.effort_cb, callback_args=self.car_ids[car_name]) + self.effort_cb, callback_args=car_id) rospy.Subscriber(f'/cars/{car_name}/command', Odometry, - self.cmd_cb, callback_args=self.car_ids[car_name]) + self.cmd_cb, callback_args=car_id) if self.mode == SimulatorMode.REALISTIC: self.car_pose_pubs[car_name] = rospy.Publisher( f'/cars/{car_name}/pose_sync_early', PoseWithCovarianceStamped, queue_size=1) + self.car_odom_pubs[car_name] = rospy.Publisher( + f'/cars/{car_name}/odom_truth', Odometry, queue_size=1) + elif self.mode == SimulatorMode.IDEAL: + self.car_odom_pubs[car_name] = rospy.Publisher( + f'/cars/{car_name}/odom', Odometry, queue_size=1) # Node data self.cmd_lock = Lock() diff --git a/rktl_sim/nodes/visualizer b/rktl_sim/nodes/visualizer index e3a001987..e8043b029 100755 --- a/rktl_sim/nodes/visualizer +++ b/rktl_sim/nodes/visualizer @@ -48,6 +48,14 @@ class VisualizerROS(object): self.timeout = rospy.get_param("~timeout", 10) rate = rospy.Rate(rospy.get_param("~rate", 20)) + car_img_path = rospy.get_param('~media/car', None) + if car_img_path is not None: + rospy.logwarn('no car image specified, not visualizing car') + + ball_img_path = rospy.get_param('~media/ball', None) + if ball_img_path is not None: + rospy.logwarn('no ball image specified, not visualizing ball') + # Setting up field field_img_path = rospy.get_param("~media/field", None) if field_img_path is not None: @@ -59,14 +67,7 @@ class VisualizerROS(object): asset.setPos(x, y) self.window.addAsset(asset) - # Setting up car - car_img_path = rospy.get_param("~media/car", None) - - # Setting up ball - ball_img_path = rospy.get_param("~media/ball", None) - # Setting up goals - asset = visualizer.Rectangle( self.window.scaleSize(goal_width), self.window.scaleSize(wall_thickness), (255,255,255)) x, y = self.window.transformPos((field_length / 2) + (wall_thickness / 2), 0) @@ -79,38 +80,55 @@ class VisualizerROS(object): asset.setPos(x, y) self.window.addAsset(asset) - self.lock = Lock() - self.last_time = None - self.path = None - - # Subscribers + # Setup assets and subscribers using available topics topics = rospy.get_published_topics() for topic_name, _ in topics: topic_ns = topic_name.split('/')[1:] + asset = None - if topic_ns[0] == "cars" and topic_ns[2] == "odom": + if topic_ns[0] == "cars": if car_img_path is not None: width = self.window.scaleSize(car_width) length = self.window.scaleSize(car_length) - asset = visualizer.Image(width, length, car_img_path) + if topic_ns[2] == "odom": + asset = visualizer.Image(width, length, car_img_path) + rospy.Subscriber(topic_name, Odometry, + self.car_odom_cb, asset) + elif topic_ns[2] == "odom_truth": + asset = visualizer.Image(width, length, + car_img_path, opacity=128) + rospy.Subscriber(topic_name, Odometry, + self.car_odom_cb, asset) + elif topic_ns[2] == "lookahead_pnt": + asset = visualizer.Circle((255,0,0), 0) + rospy.Subscriber(topic_name, Float32, + self.lookahead_cb, asset) + elif topic_ns[0] == "ball": + if ball_img_path is not None: + diameter = self.window.scaleSize(ball_radius * 2) + if topic_ns[1] == "odom": + asset = visualizer.Image(diameter, diameter, + ball_img_path) + elif topic_ns[1] == "odom_truth": + asset = visualizer.Image(diameter, diameter, + ball_img_path, opacity=128) + else: + continue + rospy.Subscriber(topic_name, Odometry, - self.car_odom_cb, asset) + self.ball_odom_cb, asset) elif topic_ns[0] == "agents" and topic_ns[2] == "bezier_path": asset = visualizer.Lines((255,0,0)) rospy.Subscriber(topic_name, BezierPathList, self.bezier_path_cb, asset) - elif topic_ns[0] == "cars" and topic_ns[2] == "lookahead_pnt": - asset = visualizer.Circle((255,0,0), 0) - rospy.Subscriber(topic_name, Float32, - self.lookahead_cb, asset) - elif topic_ns[0] == "ball" and topic_ns[1] == "odom": - diameter = self.window.scaleSize(ball_radius * 2) - asset = visualizer.Image(diameter, diameter, ball_img_path) - rospy.Subscriber(topic_name, Odometry, - self.ball_odom_cb, asset) + if asset != None: self.window.addAsset(asset) + self.lock = Lock() + self.last_time = None + self.path = None + while not rospy.is_shutdown(): try: self.window.show() @@ -122,9 +140,10 @@ class VisualizerROS(object): pass def car_odom_cb(self, odom_msg, asset): - x = odom_msg.pose.pose.position.x - y = odom_msg.pose.pose.position.y - x, y = self.window.transformPos(x, y) + """Callback for car odometry messages.""" + field_x = odom_msg.pose.pose.position.x + field_y = odom_msg.pose.pose.position.y + x, y = self.window.transformPos(field_x, field_y) asset.setPos(x, y) orient = odom_msg.pose.pose.orientation @@ -134,9 +153,10 @@ class VisualizerROS(object): asset.setAngle(heading) def ball_odom_cb(self, odom_msg, asset): - x = odom_msg.pose.pose.position.x - y = odom_msg.pose.pose.position.y - x, y = self.window.transformPos(x, y) + """Callback for ball odometry messages.""" + field_x = odom_msg.pose.pose.position.x + field_y = odom_msg.pose.pose.position.y + x, y = self.window.transformPos(field_x, field_y) asset.setPos(x, y) def path_arr_cb(self, path_msg: Path, asset): @@ -144,9 +164,9 @@ class VisualizerROS(object): asset.resetPoints() path = path_msg.waypoint for point in path: - x = point.pose.position.x - y = point.pose.position.y - x, y = self.window.transformPos(x, y) + field_x = point.pose.position.x + field_y = point.pose.position.y + x, y = self.window.transformPos(field_x, field_y) asset.setPos(x, y) def bezier_path_cb(self, msg: BezierPathList, asset): diff --git a/rktl_sim/src/visualizer/asset.py b/rktl_sim/src/visualizer/asset.py index e75067835..f05047afc 100644 --- a/rktl_sim/src/visualizer/asset.py +++ b/rktl_sim/src/visualizer/asset.py @@ -1,4 +1,4 @@ -"""Contains the Asset class. +"""Represents data to be visualized. License: BSD 3-Clause License Copyright (c) 2021, Autonomous Robotics Club of Purdue (Purdue ARC) @@ -26,11 +26,12 @@ def blit(self): pass class Image(Asset): - def __init__(self, width, length, img_path): + def __init__(self, width, length, img_path, opacity=255): self.width = width self.length = length self.pos = (0, 0) self.angle = 0 + self.opacity = opacity self.init_img = pygame.image.load(img_path) self.init_img = pygame.transform.scale( @@ -50,6 +51,7 @@ def setAngle(self, angle): def blit(self, screen): rect = self.img.get_rect(center=self.pos) + self.img.set_alpha(self.opacity) screen.blit(self.img, rect) diff --git a/rktl_sim/src/visualizer/window.py b/rktl_sim/src/visualizer/window.py index b8b9e4ea6..f3ab70583 100644 --- a/rktl_sim/src/visualizer/window.py +++ b/rktl_sim/src/visualizer/window.py @@ -1,4 +1,4 @@ -"""Contains the Window class. +"""Handles scaling and renders assets. License: BSD 3-Clause License Copyright (c) 2021, Autonomous Robotics Club of Purdue (Purdue ARC) @@ -8,9 +8,6 @@ # 3rd party modules import pygame -# Local modules -from visualizer.asset import Image, Rectangle, Lines, Circle - class Window(object): """Interfaces PyGame for rendering.""" @@ -39,9 +36,11 @@ def __init__(self, map_width, map_length, wall_thickness, name='Rocket League Vi (self.window_width, self.window_length)) def addAsset(self, asset): + """Store asset for rendering.""" self.assets.append(asset) def transformPos(self, x, y): + """Transform from field coordinates to screen coordinates.""" x = self.window_length - \ (int(x * self.scaling) + (self.window_length // 2)) y = self.window_width - \ @@ -49,9 +48,11 @@ def transformPos(self, x, y): return y, x def scaleSize(self, s): + """Transform from meters to pixels.""" return s * self.scaling def show(self): + """Render all stored assets.""" for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.quit() From f17da95c27c0840d62480e5a9fcdea5fdb18ceb6 Mon Sep 17 00:00:00 2001 From: Harrison McCarty Date: Fri, 15 Apr 2022 17:59:19 -0400 Subject: [PATCH 4/9] removed old TODOs --- rktl_sim/nodes/simulator | 4 ---- rktl_sim/nodes/visualizer | 3 --- rktl_sim/src/simulator/sim.py | 1 - 3 files changed, 8 deletions(-) diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index f997f8fac..d716b3fb6 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -4,10 +4,6 @@ License: BSD 3-Clause License Copyright (c) 2020, Autonomous Robotics Club of Purdue (Purdue ARC) All rights reserved. - -TODO: -- Scale to support multiple vehicles -- Add offset for walls """ # 3rd party modules diff --git a/rktl_sim/nodes/visualizer b/rktl_sim/nodes/visualizer index e8043b029..fc3f2b07d 100755 --- a/rktl_sim/nodes/visualizer +++ b/rktl_sim/nodes/visualizer @@ -4,9 +4,6 @@ License: BSD 3-Clause License Copyright (c) 2020, Autonomous Robotics Club of Purdue (Purdue ARC) All rights reserved. - -TODO: -- Scale to support multiple cars """ # 3rd party modules diff --git a/rktl_sim/src/simulator/sim.py b/rktl_sim/src/simulator/sim.py index ec4fe49a1..4e1bd71db 100644 --- a/rktl_sim/src/simulator/sim.py +++ b/rktl_sim/src/simulator/sim.py @@ -66,7 +66,6 @@ def __init__(self, urdf_paths, field_setup, spawn_bounds, render_enabled): self._walls[rSidewallId] = True if "backwall" in urdf_paths: - # TODO: Improve handling of split walls flBackwallID = p.loadURDF( urdf_paths["backwall"], field_setup["flbackwall"], From 71d9aeafe10f0959a3c6ba87b1ca0a6ba7623e26 Mon Sep 17 00:00:00 2001 From: Harrison McCarty Date: Fri, 15 Apr 2022 18:36:33 -0400 Subject: [PATCH 5/9] cleaned up sim nodes --- rktl_sim/nodes/simulator | 18 +++++++----------- rktl_sim/nodes/visualizer | 4 ++-- 2 files changed, 9 insertions(+), 13 deletions(-) diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index d716b3fb6..1b7efb1af 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -23,7 +23,7 @@ class SimulatorMode(Enum): IDEAL = 1 REALISTIC = 2 -class SimulatorROS(object): +class Simulator(object): """ROS wrapper for the simulator.""" def __init__(self): @@ -94,7 +94,7 @@ class SimulatorROS(object): self.ball_odom_pub = rospy.Publisher('/ball/odom', Odometry, queue_size=1) - # Creating a single car + # Creating cars car_properties = {} car_properties['length'] = rospy.get_param('/cars/length') car_properties['max_speed'] = rospy.get_param('/cars/throttle/max_speed') @@ -127,7 +127,7 @@ class SimulatorROS(object): car_id = self.car_ids[car_name] rospy.Subscriber(f'/cars/{car_name}/effort', ControlEffort, self.effort_cb, callback_args=car_id) - rospy.Subscriber(f'/cars/{car_name}/command', Odometry, + rospy.Subscriber(f'/cars/{car_name}/command', ControlCommand, self.cmd_cb, callback_args=car_id) if self.mode == SimulatorMode.REALISTIC: @@ -143,7 +143,6 @@ class SimulatorROS(object): # Node data self.cmd_lock = Lock() self.reset_lock = Lock() - self.car_cmd = (0.0, 0.0) self.last_time = None # Publishers @@ -197,7 +196,9 @@ class SimulatorROS(object): if self.last_time is not None and self.last_time != now: # Iterate sim one step delta_t = (now - self.last_time).to_sec() + self.sim.step(delta_t) + # Publish game status status = MatchStatus() if self.sim.scored: if self.sim.winner == "A": @@ -208,10 +209,8 @@ class SimulatorROS(object): status.status = MatchStatus.ONGOING self.status_pub.publish(status) - self.sim.step(delta_t) - + # Publish pose and odometry data if self.mode == SimulatorMode.REALISTIC: - # Publish ball sync pose ball_msg = PoseWithCovarianceStamped() ball_msg.header.stamp = now ball_msg.header.frame_id = self.frame_id @@ -225,7 +224,6 @@ class SimulatorROS(object): ball_msg.pose.pose.orientation.w = ball_quat[3] self.ball_pose_pub.publish(ball_msg) - # Publish car sync pose for car_name in self.car_ids: car_msg = PoseWithCovarianceStamped() car_msg.header.stamp = now @@ -241,7 +239,6 @@ class SimulatorROS(object): car_msg.pose.pose.orientation.w = car_quat[3] self.car_pose_pubs[car_name].publish(car_msg) - # Publish ball odometry ball_msg = Odometry() ball_msg.header.stamp = now ball_msg.header.frame_id = self.frame_id @@ -262,7 +259,6 @@ class SimulatorROS(object): ball_msg.twist.twist.angular.z = ball_angular[2] self.ball_odom_pub.publish(ball_msg) - # Publish bot odometry for car_name in self.car_ids: car_msg = Odometry() car_msg.header.stamp = now @@ -290,4 +286,4 @@ class SimulatorROS(object): self.reset_lock.release() if __name__ == "__main__": - SimulatorROS() + Simulator() diff --git a/rktl_sim/nodes/visualizer b/rktl_sim/nodes/visualizer index fc3f2b07d..378c2cc1f 100755 --- a/rktl_sim/nodes/visualizer +++ b/rktl_sim/nodes/visualizer @@ -46,11 +46,11 @@ class VisualizerROS(object): rate = rospy.Rate(rospy.get_param("~rate", 20)) car_img_path = rospy.get_param('~media/car', None) - if car_img_path is not None: + if car_img_path is None: rospy.logwarn('no car image specified, not visualizing car') ball_img_path = rospy.get_param('~media/ball', None) - if ball_img_path is not None: + if ball_img_path is None: rospy.logwarn('no ball image specified, not visualizing ball') # Setting up field From 5f9c517710db16f0e2975e95ba0de4a66c4b46c6 Mon Sep 17 00:00:00 2001 From: Harrison McCarty Date: Fri, 15 Apr 2022 21:16:05 -0400 Subject: [PATCH 6/9] refactored launch files --- .../{car.launch => agent_control.launch} | 0 .../{ball.launch => ball_filter.launch} | 0 rktl_launch/launch/car.launch | 28 ++++++++++ rktl_launch/launch/rocket_league.launch | 39 +++++++++---- rktl_launch/launch/rocket_league_sim.launch | 56 ------------------- 5 files changed, 56 insertions(+), 67 deletions(-) rename rktl_control/launch/{car.launch => agent_control.launch} (100%) rename rktl_control/launch/{ball.launch => ball_filter.launch} (100%) create mode 100644 rktl_launch/launch/car.launch delete mode 100644 rktl_launch/launch/rocket_league_sim.launch diff --git a/rktl_control/launch/car.launch b/rktl_control/launch/agent_control.launch similarity index 100% rename from rktl_control/launch/car.launch rename to rktl_control/launch/agent_control.launch diff --git a/rktl_control/launch/ball.launch b/rktl_control/launch/ball_filter.launch similarity index 100% rename from rktl_control/launch/ball.launch rename to rktl_control/launch/ball_filter.launch diff --git a/rktl_launch/launch/car.launch b/rktl_launch/launch/car.launch new file mode 100644 index 000000000..537d7377b --- /dev/null +++ b/rktl_launch/launch/car.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rktl_launch/launch/rocket_league.launch b/rktl_launch/launch/rocket_league.launch index 5acd5c523..a43eb8930 100644 --- a/rktl_launch/launch/rocket_league.launch +++ b/rktl_launch/launch/rocket_league.launch @@ -1,7 +1,10 @@ - + + + + @@ -9,20 +12,34 @@ + + + + + - + + + + - - + + + + + + + - - - - - - - + + + + + + diff --git a/rktl_launch/launch/rocket_league_sim.launch b/rktl_launch/launch/rocket_league_sim.launch deleted file mode 100644 index 9d1a2ea61..000000000 --- a/rktl_launch/launch/rocket_league_sim.launch +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 6432a89a2895bc3f667d16063aa00482ea7bd5c5 Mon Sep 17 00:00:00 2001 From: Harrison McCarty Date: Fri, 15 Apr 2022 21:46:44 -0400 Subject: [PATCH 7/9] added option to refresh assets in vis --- rktl_sim/nodes/visualizer | 108 +++++++++++++++++------------- rktl_sim/src/visualizer/window.py | 4 ++ 2 files changed, 67 insertions(+), 45 deletions(-) diff --git a/rktl_sim/nodes/visualizer b/rktl_sim/nodes/visualizer index 378c2cc1f..73888586c 100755 --- a/rktl_sim/nodes/visualizer +++ b/rktl_sim/nodes/visualizer @@ -12,8 +12,8 @@ from std_msgs.msg import Float32 from nav_msgs.msg import Odometry import rospy from tf.transformations import euler_from_quaternion -from threading import Lock import numpy as np +from std_srvs.srv import Empty, EmptyResponse # Local library import visualizer @@ -31,9 +31,9 @@ class VisualizerROS(object): field_length = rospy.get_param('/field/length') wall_thickness = rospy.get_param('/field/wall_thickness') goal_width = rospy.get_param('/field/goal/width') - ball_radius = rospy.get_param("/ball/radius") - car_width = rospy.get_param("/cars/width") - car_length = rospy.get_param("/cars/length") + self.ball_radius = rospy.get_param("/ball/radius") + self.car_width = rospy.get_param("/cars/width") + self.car_length = rospy.get_param("/cars/length") # Creating pygame render self.window = visualizer.Window( @@ -45,15 +45,16 @@ class VisualizerROS(object): self.timeout = rospy.get_param("~timeout", 10) rate = rospy.Rate(rospy.get_param("~rate", 20)) - car_img_path = rospy.get_param('~media/car', None) - if car_img_path is None: + self.car_img_path = rospy.get_param('~media/car', None) + if self.car_img_path is None: rospy.logwarn('no car image specified, not visualizing car') - ball_img_path = rospy.get_param('~media/ball', None) - if ball_img_path is None: + self.ball_img_path = rospy.get_param('~media/ball', None) + if self.ball_img_path is None: rospy.logwarn('no ball image specified, not visualizing ball') # Setting up field + self.field_assets = [] field_img_path = rospy.get_param("~media/field", None) if field_img_path is not None: asset = visualizer.Image( @@ -62,79 +63,96 @@ class VisualizerROS(object): field_img_path) x, y = self.window.transformPos(0, 0) asset.setPos(x, y) - self.window.addAsset(asset) + self.field_assets.append(asset) # Setting up goals asset = visualizer.Rectangle( self.window.scaleSize(goal_width), self.window.scaleSize(wall_thickness), (255,255,255)) x, y = self.window.transformPos((field_length / 2) + (wall_thickness / 2), 0) asset.setPos(x, y) - self.window.addAsset(asset) + self.field_assets.append(asset) asset = visualizer.Rectangle( self.window.scaleSize(goal_width), self.window.scaleSize(wall_thickness), (255,255,255)) x, y = self.window.transformPos(-((field_length / 2) + (wall_thickness / 2)), 0) asset.setPos(x, y) - self.window.addAsset(asset) + self.field_assets.append(asset) + + # Setup object assets + self.object_subs = [] + self.refresh_assets(None) + rospy.Service('refresh_visualizer', Empty, self.refresh_assets) + + while not rospy.is_shutdown(): + try: + self.window.show() + except self.window.ShutdownError: + exit() + try: + rate.sleep() + except rospy.ROSInterruptException: + pass + + def refresh_assets(self, _): + self.window.resetAssets() + for asset in self.field_assets: + self.window.addAsset(asset) + + if len(self.object_subs) > 0: + for sub in self.object_subs: + sub.unregister() - # Setup assets and subscribers using available topics + self.object_subs = [] topics = rospy.get_published_topics() for topic_name, _ in topics: topic_ns = topic_name.split('/')[1:] asset = None if topic_ns[0] == "cars": - if car_img_path is not None: - width = self.window.scaleSize(car_width) - length = self.window.scaleSize(car_length) + if self.car_img_path is not None: + width = self.window.scaleSize(self.car_width) + length = self.window.scaleSize(self.car_length) if topic_ns[2] == "odom": - asset = visualizer.Image(width, length, car_img_path) - rospy.Subscriber(topic_name, Odometry, - self.car_odom_cb, asset) + asset = visualizer.Image(width, length, + self.car_img_path) + self.object_subs.append( + rospy.Subscriber(topic_name, Odometry, + self.car_odom_cb, asset)) elif topic_ns[2] == "odom_truth": asset = visualizer.Image(width, length, - car_img_path, opacity=128) - rospy.Subscriber(topic_name, Odometry, - self.car_odom_cb, asset) + self.car_img_path, opacity=128) + self.object_subs.append( + rospy.Subscriber(topic_name, Odometry, + self.car_odom_cb, asset)) elif topic_ns[2] == "lookahead_pnt": asset = visualizer.Circle((255,0,0), 0) - rospy.Subscriber(topic_name, Float32, - self.lookahead_cb, asset) + self.object_subs.append( + rospy.Subscriber(topic_name, Float32, + self.lookahead_cb, asset)) elif topic_ns[0] == "ball": - if ball_img_path is not None: - diameter = self.window.scaleSize(ball_radius * 2) + if self.ball_img_path is not None: + diameter = self.window.scaleSize(self.ball_radius * 2) if topic_ns[1] == "odom": asset = visualizer.Image(diameter, diameter, - ball_img_path) + self.ball_img_path) elif topic_ns[1] == "odom_truth": asset = visualizer.Image(diameter, diameter, - ball_img_path, opacity=128) + self.ball_img_path, opacity=128) else: continue - rospy.Subscriber(topic_name, Odometry, - self.ball_odom_cb, asset) + self.object_subs.append( + rospy.Subscriber(topic_name, Odometry, + self.ball_odom_cb, asset)) elif topic_ns[0] == "agents" and topic_ns[2] == "bezier_path": asset = visualizer.Lines((255,0,0)) - rospy.Subscriber(topic_name, BezierPathList, - self.bezier_path_cb, asset) + self.object_subs.append( + rospy.Subscriber(topic_name, BezierPathList, + self.bezier_path_cb, asset)) if asset != None: self.window.addAsset(asset) - - self.lock = Lock() - self.last_time = None - self.path = None - - while not rospy.is_shutdown(): - try: - self.window.show() - except self.window.ShutdownError: - exit() - try: - rate.sleep() - except rospy.ROSInterruptException: - pass + return EmptyResponse() def car_odom_cb(self, odom_msg, asset): """Callback for car odometry messages.""" diff --git a/rktl_sim/src/visualizer/window.py b/rktl_sim/src/visualizer/window.py index f3ab70583..5d804eee3 100644 --- a/rktl_sim/src/visualizer/window.py +++ b/rktl_sim/src/visualizer/window.py @@ -35,6 +35,10 @@ def __init__(self, map_width, map_length, wall_thickness, name='Rocket League Vi self._screen = pygame.display.set_mode( (self.window_width, self.window_length)) + def resetAssets(self): + """Reset stored assets.""" + self.assets = [] + def addAsset(self, asset): """Store asset for rendering.""" self.assets.append(asset) From 573358bd1f9f999d96325533a9246ca1004d4531 Mon Sep 17 00:00:00 2001 From: Harrison McCarty Date: Sat, 16 Apr 2022 12:03:02 -0400 Subject: [PATCH 8/9] fixed launch of perception delay --- rktl_launch/launch/car.launch | 22 ++++++++++++---------- rktl_launch/launch/rocket_league.launch | 2 +- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/rktl_launch/launch/car.launch b/rktl_launch/launch/car.launch index 537d7377b..dc5f5cd1b 100644 --- a/rktl_launch/launch/car.launch +++ b/rktl_launch/launch/car.launch @@ -6,23 +6,25 @@ - - - - + + + - - + - + - + diff --git a/rktl_launch/launch/rocket_league.launch b/rktl_launch/launch/rocket_league.launch index a43eb8930..affaddc7a 100644 --- a/rktl_launch/launch/rocket_league.launch +++ b/rktl_launch/launch/rocket_league.launch @@ -18,7 +18,7 @@ - + From 3bb6321175c86c333f7559bce733d12ea36f0588 Mon Sep 17 00:00:00 2001 From: Harrison McCarty Date: Sat, 16 Apr 2022 13:57:57 -0400 Subject: [PATCH 9/9] fixed to pass test cases --- .../rocket_league/rocket_league_train.launch | 2 +- rktl_launch/launch/rocket_league.launch | 13 ++++++++----- rktl_sim/nodes/simulator | 4 ++-- rktl_sim/src/simulator/sim.py | 16 +++++++++------- rktl_sim/test/test_car_node | 7 ++++--- 5 files changed, 24 insertions(+), 18 deletions(-) diff --git a/rktl_autonomy/launch/rocket_league/rocket_league_train.launch b/rktl_autonomy/launch/rocket_league/rocket_league_train.launch index cf7a34195..98a24af55 100644 --- a/rktl_autonomy/launch/rocket_league/rocket_league_train.launch +++ b/rktl_autonomy/launch/rocket_league/rocket_league_train.launch @@ -18,7 +18,7 @@ - + diff --git a/rktl_launch/launch/rocket_league.launch b/rktl_launch/launch/rocket_league.launch index affaddc7a..1bf128ed1 100644 --- a/rktl_launch/launch/rocket_league.launch +++ b/rktl_launch/launch/rocket_league.launch @@ -4,7 +4,7 @@ - + @@ -26,20 +26,23 @@ - + + + - + - + diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index 1b7efb1af..3f4602725 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -82,7 +82,7 @@ class Simulator(object): if self.mode == SimulatorMode.IDEAL: ball_noise = None - self.sim.create_ball('ball', init_pose=ball_init_pose, + self.sim.createBall('ball', init_pose=ball_init_pose, init_speed=ball_init_speed, noise=ball_noise) if self.mode == SimulatorMode.REALISTIC: @@ -121,7 +121,7 @@ class Simulator(object): rospy.signal_shutdown('no "name" set for car config in sim') car_name = car_config['name'] - self.car_ids[car_name] = self.sim.create_car( + self.car_ids[car_name] = self.sim.createCar( 'car', init_pose=init_pose, noise=noise, props=car_properties) car_id = self.car_ids[car_name] diff --git a/rktl_sim/src/simulator/sim.py b/rktl_sim/src/simulator/sim.py index 4e1bd71db..a0ada0c2f 100644 --- a/rktl_sim/src/simulator/sim.py +++ b/rktl_sim/src/simulator/sim.py @@ -113,7 +113,7 @@ def __init__(self, urdf_paths, field_setup, spawn_bounds, render_enabled): p.setPhysicsEngineParameter(useSplitImpulse=1, restitutionVelocityThreshold=0.0001) p.setGravity(0, 0, -10) - def create_ball(self, urdf_name, init_pose=None, init_speed=None, noise=None): + def createBall(self, urdf_name, init_pose=None, init_speed=None, noise=None): if urdf_name in self.urdf_paths: zeroOrient = p.getQuaternionFromEuler([0.0, 0.0, 0.0]) if init_pose: @@ -131,11 +131,13 @@ def create_ball(self, urdf_name, init_pose=None, init_speed=None, noise=None): p.changeDynamics( bodyUniqueId=self._ballID, linkIndex=-1, + mass=0.200, + lateralFriction=0.4, + spinningFriction=0.001, + rollingFriction=0.0001, restitution=0.7, linearDamping=0, angularDamping=0, - rollingFriction=0.0001, - spinningFriction=0.001, ) # initize ball with some speed @@ -151,7 +153,7 @@ def create_ball(self, urdf_name, init_pose=None, init_speed=None, noise=None): else: return None - def create_car(self, urdf_name, init_pose=None, noise=None, props=None): + def createCar(self, urdf_name, init_pose=None, noise=None, props=None): if urdf_name in self.urdf_paths: zeroPos = [0.0, 0.0, 0.0] zeroOrient = p.getQuaternionFromEuler([0.0, 0.0, 0.0]) @@ -186,8 +188,8 @@ def create_car(self, urdf_name, init_pose=None, noise=None, props=None): props ) self._car_data[carID] = { - "posInit": initCarPos, - "orientInit": initCarOrient, + "initPos": initCarPos, + "initOrient": initCarOrient, "noise": noise, } return carID @@ -279,7 +281,7 @@ def reset(self): for car in self._cars.values(): carPos = self._car_data[car.id]["initPos"] - carOrient = self._car_data[car.id]["initOrient "] + carOrient = self._car_data[car.id]["initOrient"] if carPos is None: carPos = [random.uniform(self.spawn_bounds[0][0], self.spawn_bounds[0][1]), diff --git a/rktl_sim/test/test_car_node b/rktl_sim/test/test_car_node index 8537fc6d9..02d3a8b62 100755 --- a/rktl_sim/test/test_car_node +++ b/rktl_sim/test/test_car_node @@ -34,7 +34,7 @@ class TestCar(unittest.TestCase): } car_init_pose = {"pos": (0.0, 0.0, 0.06), "orient":(0.0, 0.0, 0.0)} - cls.sim.create_car('car', + cls.car_id = cls.sim.createCar('car', init_pose=car_init_pose, noise=None, props=car_properties) @@ -49,10 +49,11 @@ class TestCar(unittest.TestCase): rospy.sleep(rospy.Duration(0.2)) freq = 12.0 # must be evenly divisible by i_wait + self.sim.setCarCommand(self.car_id, (i_throttle, 0.0)) for _ in range(round(i_wait * freq)): - self.sim.step((i_throttle, 0.0), 1.0/freq) + self.sim.step(1.0/freq) - vel = self.sim.getCarVelocity()[0][0] + vel = self.sim.getCarVelocity(self.car_id)[0][0] self.assertLess(vel, e_vel + e_err, 'car moving too fast: ' + f'expected velocity {e_vel}, actual velocity {vel}')