class CarlaRosScenarioRunner(object): """ Execute scenarios via ros service """ def __init__(self, role_name, host, port, scenario_runner_path, wait_for_ego): """ Constructor """ self._status_publisher = rospy.Publisher("/scenario_runner/status", CarlaScenarioRunnerStatus, queue_size=1, latch=True) self.scenario_runner_status_updated(ApplicationStatus.STOPPED) self._scenario_runner = ScenarioRunnerRunner( scenario_runner_path, host, port, wait_for_ego, self.scenario_runner_status_updated, self.scenario_runner_log) self._request_queue = queue.Queue() self._execute_scenario_service = rospy.Service( '/scenario_runner/execute_scenario', ExecuteScenario, self.execute_scenario) def scenario_runner_log(self, log): # pylint: disable=no-self-use """ Callback for application logs """ rospy.logwarn("[SC]{}".format(log)) def scenario_runner_status_updated(self, status): """ Executed from application runner whenever the status changed """ rospy.loginfo("Status updated to {}".format(status)) val = CarlaScenarioRunnerStatus.STOPPED if status == ApplicationStatus.STOPPED: val = CarlaScenarioRunnerStatus.STOPPED elif status == ApplicationStatus.STARTING: val = CarlaScenarioRunnerStatus.STARTING elif status == ApplicationStatus.RUNNING: val = CarlaScenarioRunnerStatus.RUNNING elif status == ApplicationStatus.SHUTTINGDOWN: val = CarlaScenarioRunnerStatus.SHUTTINGDOWN else: val = CarlaScenarioRunnerStatus.ERROR status = CarlaScenarioRunnerStatus() status.status = val self._status_publisher.publish(status) def execute_scenario(self, req): """ Execute a scenario """ rospy.loginfo("Scenario Execution requested...") response = ExecuteScenarioResponse() response.result = True if not os.path.isfile(req.scenario.scenario_file): rospy.logwarn("Requested scenario file not existing {}".format( req.scenario.scenario_file)) response.result = False else: self._request_queue.put(req.scenario) return response def run(self): """ Control loop :return: """ current_req = None while not rospy.is_shutdown(): if current_req: if self._scenario_runner.is_running(): rospy.loginfo( "Scenario Runner currently running. Shutting down.") self._scenario_runner.shutdown() rospy.loginfo("Scenario Runner stopped.") rospy.loginfo("Executing scenario {}...".format( current_req.name)) # execute scenario scenario_executed = self._scenario_runner.execute_scenario( current_req.scenario_file) if not scenario_executed: rospy.logwarn("Unable to execute scenario.") current_req = None else: try: current_req = self._request_queue.get(block=True, timeout=0.5) except queue.Empty: # no new request pass if self._scenario_runner.is_running(): rospy.loginfo("Scenario Runner currently running. Shutting down.") self._scenario_runner.shutdown()
class CarlaRosScenarioRunner(object): """ Execute scenarios via ros service """ def __init__(self, role_name, host, port, scenario_runner_path, publish_waypoints, publish_goal): """ Constructor """ self._goal_publisher = None if publish_goal: self._goal_publisher = rospy.Publisher( "/carla/{}/goal".format(role_name), PoseStamped, queue_size=1, latch=True) self._target_speed_publisher = rospy.Publisher( "/carla/{}/target_speed".format(role_name), Float64, queue_size=1, latch=True) self._path_publisher = None if publish_waypoints: self._path_publisher = rospy.Publisher( "/carla/{}/waypoints".format(role_name), Path, queue_size=1, latch=True) self._status_publisher = rospy.Publisher("/scenario_runner/status", CarlaScenarioRunnerStatus, queue_size=1, latch=True) self.scenario_runner_status_updated(ApplicationStatus.STOPPED) self._scenario_runner = ScenarioRunnerRunner( scenario_runner_path, host, port, self.scenario_runner_status_updated, self.scenario_runner_log) self._request_queue = queue.Queue() self._execute_scenario_service = rospy.Service( '/scenario_runner/execute_scenario', ExecuteScenario, self.execute_scenario) def scenario_runner_log(self, log): # pylint: disable=no-self-use """ Callback for application logs """ rospy.logwarn("[SC]{}".format(log)) def scenario_runner_status_updated(self, status): """ Executed from application runner whenever the status changed """ rospy.loginfo("Status updated to {}".format(status)) val = CarlaScenarioRunnerStatus.STOPPED if status == ApplicationStatus.STOPPED: val = CarlaScenarioRunnerStatus.STOPPED elif status == ApplicationStatus.STARTING: val = CarlaScenarioRunnerStatus.STARTING elif status == ApplicationStatus.RUNNING: val = CarlaScenarioRunnerStatus.RUNNING elif status == ApplicationStatus.SHUTTINGDOWN: val = CarlaScenarioRunnerStatus.SHUTTINGDOWN else: val = CarlaScenarioRunnerStatus.ERROR status = CarlaScenarioRunnerStatus() status.status = val self._status_publisher.publish(status) def execute_scenario(self, req): """ Execute a scenario """ rospy.loginfo("Scenario Execution requested...") response = ExecuteScenarioResponse() response.result = True if not os.path.isfile(req.scenario.scenario_file): rospy.logwarn("Requested scenario file not existing {}".format( req.scenario.scenario_file)) response.result = False else: self._request_queue.put(req.scenario) return response def run(self): """ Control loop :return: """ current_req = None while not rospy.is_shutdown(): if current_req: if self._scenario_runner.is_running(): rospy.loginfo( "Scenario Runner currently running. Shutting down.") self._scenario_runner.shutdown() rospy.loginfo("Scenario Runner stopped.") rospy.loginfo("Executing scenario {}...".format( current_req.name)) # execute scenario scenario_executed = self._scenario_runner.execute_scenario( current_req.scenario_file) if scenario_executed: # publish target speed self._target_speed_publisher.publish( Float64(data=current_req.target_speed)) # publish last pose of route as goal # (can be used in conjunction with carla_waypoint_publisher) if self._goal_publisher: goal = PoseStamped() if current_req.waypoints: goal.pose = current_req.waypoints[-1] goal.header.stamp = rospy.get_rostime() goal.header.frame_id = "map" self._goal_publisher.publish(goal) # publish the waypoints (can directly be used within carla_ad_agent) if self._path_publisher: path = Path() path.header.stamp = rospy.get_rostime() path.header.frame_id = "map" for pose in current_req.waypoints: path.poses.append(PoseStamped(pose=pose)) self._path_publisher.publish(path) else: rospy.logwarn("Unable to execute scenario.") current_req = None else: try: current_req = self._request_queue.get(block=True, timeout=0.5) except queue.Empty: # no new request pass if self._scenario_runner.is_running(): rospy.loginfo("Scenario Runner currently running. Shutting down.") self._scenario_runner.shutdown()