diff --git a/examples/large_map_example.py b/examples/large_map_example.py index 4c8593ef..6b08734a 100644 --- a/examples/large_map_example.py +++ b/examples/large_map_example.py @@ -1,9 +1,7 @@ import invertedai as iai -from invertedai.large.common import Region -from invertedai.common import AgentAttributes, AgentType +from invertedai.common import AgentType import argparse -import matplotlib.pyplot as plt import random import time diff --git a/invertedai/logs/logger.py b/invertedai/logs/logger.py index 2cfb1a86..0ecf7486 100644 --- a/invertedai/logs/logger.py +++ b/invertedai/logs/logger.py @@ -81,9 +81,8 @@ def add_time_step_data( current_present_indexes=current_present_indexes ) self.present_indexes.append(current_present_indexes) - self.agent_states.append(current_agent_states) + self.agent_states.append(current_agent_states) - class LogBase(): """ A class for containing features relevant to both log reading and writing such as visualization. @@ -93,6 +92,67 @@ def __init__(self): self._scenario_log = None self.simulation_length = None + @classmethod + def scenario_log_from_debug_log( + cls, + debug_log_path + ): + with open(debug_log_path) as f: + DEBUG_LOG_DATA = json.load(f) + + last_init_res = json.loads(DEBUG_LOG_DATA["initialize_responses"][-1]) + last_init_req = json.loads(DEBUG_LOG_DATA["initialize_requests"][-1]) + last_drive_req = json.loads(DEBUG_LOG_DATA["drive_requests"][-1]) + all_drive_responses = [] + all_agent_states = [] + all_traffic_lights_states = [] + for res_ in DEBUG_LOG_DATA["drive_responses"]: + res = json.loads(res_) + all_drive_responses.append(res) + all_agent_states.append([AgentState.fromlist(state) for state in res["agent_states"]]) + if res["traffic_lights_states"] is not None: + all_traffic_lights_states.append(res["traffic_lights_states"]) + else: + all_traffic_lights_states = None + + agent_properties = [AgentProperties.deserialize(prop) for prop in last_init_res["agent_properties"]] + + log_location = last_init_req["location"] + + if len(DEBUG_LOG_DATA["location_responses"]) > 0: + loc_res = json.loads(DEBUG_LOG_DATA["location_responses"][-1]) + rendering_center = loc_res["map_center"] + rendering_fov=loc_res["map_fov"] + else: + location_info_response = location_info( + location=log_location, + ) + rendering_center = [ + location_info_response.map_center.x, + location_info_response.map_center.y + ] + rendering_fov=location_info_response.map_fov + + scenario_log = ScenarioLog( + agent_states=all_agent_states, + agent_properties=agent_properties, + traffic_lights_states=all_traffic_lights_states, + location=log_location, + rendering_center=rendering_center, + rendering_fov=rendering_fov, + lights_random_seed=last_drive_req["random_seed"], + initialize_random_seed=last_init_req["random_seed"], + drive_random_seed=last_drive_req["random_seed"], + initialize_model_version=last_init_res["model_version"], + drive_model_version=all_drive_responses[-1]["model_version"], + light_recurrent_states=all_drive_responses[-1]["light_recurrent_states"], + recurrent_states=[RecurrentState.fromval(rec_state) for rec_state in all_drive_responses[-1]["recurrent_states"]], + waypoints={str(i):[prop.waypoint] for i, prop in enumerate(agent_properties)}, + present_indexes=[list(range(len(agent_properties)))]*len(all_agent_states) + ) + + return scenario_log + @validate_arguments def visualize_range( self, @@ -355,7 +415,7 @@ def export_to_file( outfile, indent=4 ) - + @classmethod def export_log_to_file( cls, diff --git a/invertedai_cpp/examples/initialize_body.json b/invertedai_cpp/examples/initialize_body.json index 43c6f929..81bef128 100755 --- a/invertedai_cpp/examples/initialize_body.json +++ b/invertedai_cpp/examples/initialize_body.json @@ -4,10 +4,10 @@ "states_history": null, "agent_properties": [ {"agent_type":"car"}, - {"agent_type":"pedestrian"}, - {"agent_type":"pedestrian"}, - {"agent_type":"pedestrian"}, - {"agent_type":"pedestrian"} + {"agent_type":"car"}, + {"agent_type":"car"}, + {"agent_type":"car"}, + {"agent_type":"car"} ], "traffic_light_state_history": null, "get_birdview": true, diff --git a/invertedai_cpp/invertedai/logger.cc b/invertedai_cpp/invertedai/logger.cc index d595fb39..e888281b 100644 --- a/invertedai_cpp/invertedai/logger.cc +++ b/invertedai_cpp/invertedai/logger.cc @@ -11,7 +11,7 @@ using json = nlohmann::json; namespace invertedai { - + std::string LogWriter::get_current_time_UTC_(){ auto now = std::chrono::system_clock::now(); auto duration = now.time_since_epoch(); @@ -61,33 +61,278 @@ namespace invertedai { } }; - void LogWriter::write_log_to_file(const std::string &dir_path){ - json log; + nlohmann::ordered_json get_agent_state_data(nlohmann::ordered_json res_agents, const int &index) { + std::vector state = res_agents[index]; + + nlohmann::ordered_json state_info; + state_info["center"]["x"] = state[0]; + state_info["center"]["y"] = state[1]; + state_info["orientation"] = state[2]; + state_info["speed"] = state[3]; + + return state_info; + } + + void LogWriter::write_scenario_log(const std::string &dir_path, const std::string &log_path = ""){ + // Produce an IAI formatted log that can be used in various applications + // Assumptions: The number of vehicles stays consistent throughout the simulation + + //Get data to parse from specified source + std::vector drive_responses; + nlohmann::ordered_json last_init_res; + nlohmann::ordered_json last_init_req; + nlohmann::ordered_json last_drive_req; + nlohmann::ordered_json last_loc_res; + nlohmann::ordered_json last_loc_req; + + if (log_path.empty()){ + last_init_res = json::parse(this->init_responses_.back()); + last_init_req = json::parse(this->init_requests_.back()); + + for (auto res: this->drive_responses_) { + drive_responses.push_back(json::parse(res)); + } + last_drive_req = json::parse(this->drive_requests_.back()); + + //TODO: Find a way to produce data that would be in location info without using Session object. + if (this->loc_responses_.empty()) { + last_loc_res = NULL; + } + else { + last_loc_res = json::parse(this->loc_responses_.back()); + } + if (this->loc_requests_.empty()) { + last_loc_req = NULL; + } + else { + try { + last_loc_req = json::parse(this->loc_requests_.back()); + } + catch (nlohmann::json_abi_v3_11_2::detail::parse_error err) { + std::cout << "WARNING: Could not parse request message: " << this->loc_requests_.back() << ". Not processing data from this message." << std::endl; + } + } + } + else { + std::ifstream f(log_path); + nlohmann::ordered_json data = json::parse(f); + + last_init_res = data["initialize_responses"].back(); + last_init_req = data["initialize_requests"].back(); + std::vector drive_responses; + for (auto res: data["drive_responses"]) { + drive_responses.push_back(res); + } + last_drive_req = data["drive_requests"].back(); + + if (data["location_responses"].empty()) { + last_loc_res = NULL; + } + else { + last_loc_res = data["location_responses"].back(); + } + if (data["location_requests"].empty()) { + last_loc_req = NULL; + } + else { + last_loc_req = data["location_requests"].back(); + } + } + + //Begin parsing data + int num_drive_responses = drive_responses.size(); + nlohmann::ordered_json predetermined_agents; + nlohmann::ordered_json predetermined_controls; - log["location_requests"] = this->loc_requests_; - log["location_responses"] = this->loc_responses_; + //Get agent properties, as well as initial simulation states for agents and controls + nlohmann::ordered_json num_controls; + num_controls["traffic_light"] = 0; + num_controls["yield_sign"] = 0; + num_controls["stop_sign"] = 0; + num_controls["other"] = 0; - log["location_request_timestamps"] = this->loc_request_timestamps_; - log["location_response_timestamps"] = this->loc_response_timestamps_; + if (!last_loc_res.is_null()) { + if (!last_loc_res["static_actors"].empty()) { + for (auto actor: last_loc_res["static_actors"]) { + std::string agent_type = actor["agent_type"]; + if (num_controls.contains(agent_type)) { + num_controls[agent_type] += 1; + } + else { + num_controls["other"] += 1; + } + nlohmann::ordered_json control_data; + control_data["entity_type"] = agent_type; + nlohmann::ordered_json static_attributes; + static_attributes["length"] = actor["length"]; + static_attributes["width"] = actor["width"]; + static_attributes["rear_axis_offset"] = 0.0; + control_data["static_attributes"] = static_attributes; - log["initialize_requests"] = this->init_requests_; - log["initialize_responses"] = this->init_responses_; + nlohmann::ordered_json states; + control_data["states"] = states; + int id_int = actor["actor_id"]; + std::string actor_id = std::to_string(id_int); + predetermined_controls[actor_id] = control_data; + + nlohmann::ordered_json controls_info; + controls_info["center"]["x"] = actor["x"]; + controls_info["center"]["y"] = actor["y"]; + controls_info["orientation"] = actor["orientation"]; + controls_info["speed"] = 0.0; + if (agent_type == "traffic_light") { + controls_info["control_state"] = last_init_res["traffic_lights_states"][actor_id]; + } + else { + controls_info["control_state"] = "none"; + } + predetermined_controls[actor_id]["states"]["0"] = controls_info; + } + } + + } + + //Get agent properties, as well as initial simulation states for agents and controls + int num_vehicles = 0; + int num_pedestrians = 0; + for (int i = 0; i < last_init_res["agent_properties"].size(); i++) { + nlohmann::ordered_json prop = last_init_res["agent_properties"][i]; + std::string entity_type = prop["agent_type"]; + std::string agent_id = std::to_string(i); + + if (entity_type == "car") { + num_vehicles++; + } + if (entity_type == "pedestrian") { + num_pedestrians++; + } + + predetermined_agents[agent_id]["entity_type"] = entity_type; + + nlohmann::ordered_json static_attributes; + static_attributes["length"] = prop["length"]; + static_attributes["width"] = prop["width"]; + static_attributes["rear_axis_offset"] = prop["rear_axis_offset"]; + static_attributes["is_parked"] = false; + predetermined_agents[agent_id]["static_attributes"] = static_attributes; + + nlohmann::ordered_json states; + predetermined_agents[agent_id]["states"] = states; + predetermined_agents[agent_id]["states"]["0"] = get_agent_state_data(last_init_res["agent_states"],i); + } + + //Get all agent states data for all time steps + for (int i = 0; i < num_drive_responses; i++) { + nlohmann::ordered_json drive_res = drive_responses[i]; + std::string ts = std::to_string(i+1); + + for (int j = 0; j < drive_res["agent_states"].size(); j++) { + std::string agent_id = std::to_string(j); + predetermined_agents[agent_id]["states"][ts] = get_agent_state_data(drive_res["agent_states"],j); + } + + if (!drive_res["traffic_lights_states"].empty() && !last_loc_res.is_null()) { + for (const auto& tl_state: drive_res["traffic_lights_states"].items()) { + nlohmann::ordered_json tl_state_template = predetermined_controls[tl_state.key()]["states"]["0"]; + tl_state_template["control_state"] = tl_state.value(); + predetermined_controls[tl_state.key()]["states"][ts] = tl_state_template; + } + } + } - log["initialize_request_timestamps"] = this->init_request_timestamps_; - log["initialize_response_timestamps"] = this->init_response_timestamps_; + //Get waypoint information for last time step + nlohmann::ordered_json individual_suggestions; + if (!last_drive_req.is_null()) { + for (int i = 0; i < last_drive_req["agent_properties"].size(); i++) { + nlohmann::ordered_json prop = last_drive_req["agent_properties"][i]; - log["drive_requests"] = this->drive_requests_; - log["drive_responses"] = this->drive_responses_; + if (prop.contains("waypoint")) { + nlohmann::ordered_json wp; + wp["suggestion_strength"] = 0.8; + std::vector wp_states; + nlohmann::ordered_json wp_state_next; + + wp_state_next["center"]["x"] = prop["waypoint"][0]; + wp_state_next["center"]["y"] = prop["waypoint"][1]; + wp_states.push_back(wp_state_next); + wp["states"] = wp_states; - log["drive_request_timestamps"] = this->drive_request_timestamps_; - log["drive_response_timestamps"] = this->drive_response_timestamps_; + individual_suggestions[std::to_string(i)] = wp; + } - std::string file_path = "iai_log_" + this->get_current_time_UTC_() + ".json"; + } + } + + nlohmann::ordered_json birdview_options; + if (!last_loc_req.is_null()) { + birdview_options["rendering_center"] = last_loc_req["rendering_center"]; + birdview_options["renderingFOV"] = last_loc_req["renderingFOV"]; + } + nlohmann::ordered_json light_recurrent_states = drive_responses.back()["light_recurrent_states"]; + + nlohmann::ordered_json scenario_log; + scenario_log["location"]["identifier"] = last_init_req["location"]; + scenario_log["scenario_length"] = num_drive_responses; + scenario_log["num_agents"]["car"] = num_vehicles; + scenario_log["num_agents"]["pedestrian"] = num_pedestrians; + scenario_log["predetermined_agents"] = predetermined_agents; + scenario_log["num_controls"] = num_controls; + scenario_log["predetermined_controls"] = predetermined_controls; + scenario_log["individual_suggestions"] = individual_suggestions; + if (!last_drive_req.is_null()) { + scenario_log["drive_random_seed"] = last_drive_req["random_seed"]; + scenario_log["drive_model_version"] = last_drive_req["model_version"]; + } + else { + scenario_log["drive_random_seed"] = NULL; + scenario_log["drive_model_version"] = NULL; + } + scenario_log["birdview_options"] = birdview_options; + scenario_log["light_recurrent_states"] = light_recurrent_states; + + + std::string file_path = "iai_scenario_log_" + this->get_current_time_UTC_() + ".json"; std::string full_path = dir_path + file_path; - std::cout << "INFO: IAI Log written to path: " << full_path << std::endl; + std::cout << "INFO: IAI Scenario Log written to path: " << full_path << std::endl; std::ofstream o(full_path); - o << std::setw(4) << log << std::endl; + o << std::setw(4) << scenario_log << std::endl; + }; + + void LogWriter::write_log_to_file(const std::string &dir_path, const bool &is_scenario_log = false){ + if (is_scenario_log){ + this->write_scenario_log(dir_path); + } + else { + json log; + + log["location_requests"] = this->loc_requests_; + log["location_responses"] = this->loc_responses_; + + log["location_request_timestamps"] = this->loc_request_timestamps_; + log["location_response_timestamps"] = this->loc_response_timestamps_; + + log["initialize_requests"] = this->init_requests_; + log["initialize_responses"] = this->init_responses_; + + log["initialize_request_timestamps"] = this->init_request_timestamps_; + log["initialize_response_timestamps"] = this->init_response_timestamps_; + + log["drive_requests"] = this->drive_requests_; + log["drive_responses"] = this->drive_responses_; + + log["drive_request_timestamps"] = this->drive_request_timestamps_; + log["drive_response_timestamps"] = this->drive_response_timestamps_; + + std::string file_name = this->get_current_time_UTC_() + ".json"; + std::string file_path = "iai_log_" + file_name; + std::string full_path = dir_path + file_path; + + std::cout << "INFO: IAI Log written to path: " << full_path << std::endl; + + std::ofstream o(full_path); + o << std::setw(4) << log << std::endl; + } }; } \ No newline at end of file diff --git a/invertedai_cpp/invertedai/logger.h b/invertedai_cpp/invertedai/logger.h index cc593e23..aa982e8d 100644 --- a/invertedai_cpp/invertedai/logger.h +++ b/invertedai_cpp/invertedai/logger.h @@ -38,7 +38,9 @@ namespace invertedai { void append_response(const std::string &res, const std::string &mode); - void write_log_to_file(const std::string &file_path); + void write_scenario_log(const std::string &dir_path,const std::string &log_path); + + void write_log_to_file(const std::string &file_path, const bool &is_scenario_log); }; } diff --git a/invertedai_cpp/invertedai/session.cc b/invertedai_cpp/invertedai/session.cc index 33e1892f..843ac070 100644 --- a/invertedai_cpp/invertedai/session.cc +++ b/invertedai_cpp/invertedai/session.cc @@ -61,7 +61,7 @@ void Session::connect() { Session::~Session(){ if (this->is_logging){ - this->logger.write_log_to_file(this->iai_logger_path); + this->logger.write_log_to_file(this->iai_logger_path,false); } } @@ -111,7 +111,14 @@ const std::string Session::request( ) { if (this->is_logging){ - this->logger.append_request(body_str,mode); + std::string msg_str; + if (!body_str.empty()){ + msg_str = body_str; + } + else if (!url_query_string.empty()) { + msg_str = url_query_string; + } + this->logger.append_request(msg_str,mode); } std::string target = subdomain + mode + url_query_string;