diff --git a/subt_example/CMakeLists.txt b/subt_example/CMakeLists.txt index 898ac882..37d4ae0c 100644 --- a/subt_example/CMakeLists.txt +++ b/subt_example/CMakeLists.txt @@ -46,8 +46,6 @@ add_executable(teleop_node src/teleop_node.cc) add_dependencies(teleop_node ${catkin_EXPORTED_TARGETS}) target_link_libraries(teleop_node ${catkin_LIBRARIES}) -# add_subdirectory(src/truth_controller) - ########### ## Tests ## diff --git a/subt_ign/CMakeLists.txt b/subt_ign/CMakeLists.txt index 5aecdab0..014e5c3c 100644 --- a/subt_ign/CMakeLists.txt +++ b/subt_ign/CMakeLists.txt @@ -343,6 +343,35 @@ install(TARGETS ${gas_emitter_detector_plugin_name} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +# Create the libPlaybackEventRecorder.so library. +set(playback_event_recorder_name PlaybackEventRecorder) +add_library(${playback_event_recorder_name} SHARED + src/PlaybackEventRecorder.cc + src/PlaybackEventRecorder.cc +) + +target_include_directories(${playback_event_recorder_name} + PRIVATE + ${CATKIN_DEVEL_PREFIX}/include) +target_link_libraries(${playback_event_recorder_name} + PRIVATE + ignition-gazebo${IGN_GAZEBO_VER}::core + ignition-common3::ignition-common3 + ignition-launch4::ignition-launch4 + ignition-math6::ignition-math6 + ignition-msgs7::ignition-msgs7 + ignition-plugin1::loader + ignition-transport9::ignition-transport9 + sdformat10::sdformat10 + ${catkin_LIBRARIES} + ${YAML_CPP_LIBRARIES} +) + +install(TARGETS ${playback_event_recorder_name} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + # Create log_checker executable. add_executable(log_checker src/LogChecker.cc) target_link_libraries(log_checker diff --git a/subt_ign/launch/path_tracer.ign b/subt_ign/launch/path_tracer.ign index d2454945..3043127f 100644 --- a/subt_ign/launch/path_tracer.ign +++ b/subt_ign/launch/path_tracer.ign @@ -48,6 +48,9 @@ <% elsif $worldName.include?('urban_circuit_') && !$worldName.include?('practice') %> urban_circuit/<%= worldNumber %>/<%= $worldName %>.sdf + <% elsif $worldName.include?('cave_circuit_') && + !$worldName.include?('practice') %> + cave_circuit/<%= worldNumber %>/<%= $worldName %>.sdf <% else %> <%= $worldName %>.sdf <% end %> diff --git a/subt_ign/scripts/playback_event_recorder/README.md b/subt_ign/scripts/playback_event_recorder/README.md new file mode 100644 index 00000000..0cccfd8c --- /dev/null +++ b/subt_ign/scripts/playback_event_recorder/README.md @@ -0,0 +1,82 @@ +# Playback Event Recorder + +The playback event recorder creates videos for events that are stored in the +events.yml log file in the state log directory. + +Here are the events being recorded and their associated camera mode: + +* robots exiting staging area: static camera above staging area looking down at angle +* rock fall: static camera above dynamic rocks model looking directly downward +* marsupial vehicle detaching child robot: camera follows the child robot +* artifact proximity: camera follows the robot +* region of interest (decision, vertical, elevation): camera follows the robot + +## Running the demo + +To create event videos for a subt log, run: + +``` +bash record_playback_events.bash +``` + +The script launches ign gazebo in playback mode and creates videos using the +GUI camera video recording feature. For each event, it seeks playback to some +time before the event, moves the camera to the desired location (either in +follow mode or moves to a static pose), and starts recording until some time +after the event. After recording all events, ign-gazebo will exit on its own +and you can find the recorded videos in a timestamped directory inside the +directory where the bash script is run. + +## Video recorder settings + +By default, the video recorder plugin records videos based on real time and +in lower quality than desired. In order to produce smooth high quality videos +with accurate timing, we need to specify a few video recorder +configurations in the `gui.config` file. The config file should be located in +`$HOME/.ignition/gazebo`. Find the 3D Scene plugin and add the following +`` settings: + +```xml + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + 6 0 6 0 0.5 3.14 + + + true + true + 8000000 + + + +``` + +This makes sure the video recording is done in lock step with simulation +state updates so that we do not miss any frames during encoding. Additionally +sim time is used as timestamp so that the generated video length is unaffected +by the speed at which log playback is running in. For example, it may take +up to several hours to playback an event in a subt log due to low RTF but the +resulting video should still have a length that matches the specified duration +of the event. + +## Known issue + +The video recorder configurations are set to produce higher quality videos +but the encoding process is also more expensive. Since the server and the GUI +run asynchronously in two separate processes, we usually see that the GUI start +to lag behind the server after several minutes into recording. Once the ignition +transport msg queue is full, it starts dropping state msgs, and we would +notice missing frames in the generated videos. This is currently mitigated by +using a "catch-up" recording strategy in the playback event recorder, which +basically pauses the simulation when we detect the the video recorder is lagging +behind the server above some threshold, and only resumes playing back simulation +when the lag is within a reasonable amount of time. diff --git a/subt_ign/scripts/playback_event_recorder/playback_event_recorder.sdf b/subt_ign/scripts/playback_event_recorder/playback_event_recorder.sdf new file mode 100644 index 00000000..760ee0e1 --- /dev/null +++ b/subt_ign/scripts/playback_event_recorder/playback_event_recorder.sdf @@ -0,0 +1,104 @@ + + + + + + + + + + 25 + + + tmp_record + + + + + + + tmp_record + true + true + + + + + diff --git a/subt_ign/scripts/playback_event_recorder/record_playback_events.bash b/subt_ign/scripts/playback_event_recorder/record_playback_events.bash new file mode 100644 index 00000000..e3e840d1 --- /dev/null +++ b/subt_ign/scripts/playback_event_recorder/record_playback_events.bash @@ -0,0 +1,57 @@ +#!/bin/bash + +# The playback event recorder creates videos for events that are stored +# in the events.yml log file in the state log directory. The script launches +# ign gazebo in playback mode and creates videos using the gui camera video +# recording feature. For each event, it seeks playback to some time before the +# event, moves the camera to the desired location, and starts recording until +# some time after the event. + +echo "===================================" +echo "Staring Playback Event Recorder" +echo "===================================" + +if [ -z "$1" ]; then + echo "Usage: bash ./record_playback_events.bash [path_to_log_dir]" + exit 0 +fi + +logDirPath=$1 + +if [ ! -d "$logDirPath" ]; then + echo "Directory does not exist: $logDirPath" + exit 0 +fi + +scriptDir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +tmpDir="tmp_record" + +echo "Creating tmp dir for recording: $tmpDir" + +if [ -d "$tmpDir" ]; then + rm -fr $tmpDir +fi + +ln -s $logDirPath $tmpDir + +echo "Starting log playback and video recording" + +export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=$LD_LIBRARY_PATH +sdfName="playback_event_recorder" +ign gazebo -v 4 "$scriptDir/$sdfName.sdf" + +echo "Video recording ended. Shutting down playback" + +pgrep -f $sdfName | xargs kill -9 &> /dev/null + +videoDir=$(date +%s) +echo "Moving mp4 videos to dir: $videoDir" +mkdir $videoDir +mv *.mp4 $videoDir + +# remove tmp dir +if [ -d "$tmpDir" ]; then + rm -fr $tmpDir +fi +echo "Done" + diff --git a/subt_ign/src/PlaybackEventRecorder.cc b/subt_ign/src/PlaybackEventRecorder.cc new file mode 100644 index 00000000..61ef73a6 --- /dev/null +++ b/subt_ign/src/PlaybackEventRecorder.cc @@ -0,0 +1,1296 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + + +#include "PlaybackEventRecorder.hh" + +IGNITION_ADD_PLUGIN( + subt::PlaybackEventRecorder, + ignition::gazebo::System, + subt::PlaybackEventRecorder::ISystemConfigure, + subt::PlaybackEventRecorder::ISystemPreUpdate, + subt::PlaybackEventRecorder::ISystemPostUpdate) + +namespace subt +{ + /// \brief Data structure to store event info + class Event + { + public: void GenerateFilename(const std::string &_format) + { + this->filename = std::to_string( + static_cast(this->startRecordTime)) + "-" + this->type; + if (!this->detector.empty()) + this->filename += "-" + this->detector; + if (!this->model.empty()) + this->filename += "-" + this->model; + if (!this->extra.empty()) + this->filename += "-" + this->extra; + this->filename += "-" + this->robot + + + "-" + std::to_string(this->id) +"." + _format; + } + + /// \brief Event id + public: unsigned int id; + + /// \brief Type of event + public: std::string type; + + /// \brief Time of event in seconds + public: double time = 0; + + /// \brief Robot associated with the event + public: std::string robot; + + /// \brief For "detect" event type only - the detector of the event + public: std::string detector; + + /// \brief For "detect" event type only - the state of the event, + /// e.g. enter / exit + public: std::string state; + + /// \brief Model associated with this event + public: std::string model; + + /// \brief Extra typpe info associate with event + public: std::string extra; + + /// \brief Start time for recording video + public: double startRecordTime = 0; + + /// \brief End time for recording video + public: double endRecordTime = 0; + + /// \brief Name of the output file. + public: std::string filename; + }; + + /// \brief Playback recording state + enum State + { + /// \brief Idle + IDLE = 0, + + /// \brief Seek log to time of event + SEEK_EVENT = 1, + + /// \brief Seek log to time before event + SEEK_BEGIN = 2, + + /// \brief Recording in progress + RECORDING = 3, + }; + + /// \brief Camera modes when recording + enum CameraMode + { + /// \brief Static camera pose + STATIC_CAMERA = 0, + + /// \brief Camera follows robot + FOLLOW_CAMERA = 1 + }; +} + +using namespace ignition; +using namespace gazebo; +using namespace systems; +using namespace subt; + +/// \brief Private data class for PlaybackEventRecorder +class subt::PlaybackEventRecorderPrivate +{ + /// \brief Seek to time in log playback + /// \param[in] _t Time in sim seconds + public: void Seek(double _t); + + /// \brief Move gui camera to pose + /// \param[in] _pose Pose to move to + public: void MoveTo(const math::Pose3d &_pose); + + /// \brief Move gui camera to entity + /// \param[in] _entity Entity to move to + public: void MoveTo(const std::string &_entity); + + /// \brief Move gui camera to follow entity + /// \param[in] _entity Entity to follow + public: void Follow(const std::string &_entity); + + /// \brief Start/stop video recording + /// \param[in] _record True to start, false to stop + public: void Record(bool _record); + + /// \brief Spawn a light + public: void SpawnLight(); + + /// \brief Callback for a video recorder stats msg + /// \param[in] _msg Message containing video recorder stats + public: void OnRecorderStats(const msgs::Time &_msg); + + /// \brief Recorder stats msg. + public: msgs::Time recorderStatsMsg; + + /// \brief Mutex to protect recorder stats msg. + public: std::mutex recorderStatsMutex; + + /// \brief Ignition Transport node. + public: transport::Node node; + + /// \brief Name of the world stored in the log + public: std::string logWorldName; + + /// \brief True to spawn light on playback + public: bool spawnLight = false; + + /// \brief True to make camera follow robot instead of staying in fixed + /// position + public: bool cameraFollow = false; + + /// \brief True if camera is currently following the target + public: bool cameraFollowing = false; + + /// \brief True if entity (that the camera is asked to follow) exists in + /// the world + public: bool entityExists = false; + + /// \brief Time when system is loaded + public: std::chrono::time_point loadTime; + + /// \brief If scene has been initialized + public: bool started = false; + + /// \brief Indicate if the system requested to wait for certain amount of + /// time (real time) before continuing to the playback recording process. + /// This is needed for example to wait until the gui camera has arrived at + /// the specified pose + public: bool waiting = false; + + /// \brief Wait for scene to be created on the gui side + public: bool waitForScene = false; + + /// \brief Start of wait time in wall clock time + public: std::chrono::time_point waitStartTime; + + /// \brief Auto exit when log playback recording ends + public: bool exitOnFinish = false; + + /// \brief Pointer to the event manager + public: EventManager *eventManager{nullptr}; + + /// \brief current event being recorded. + public: Event event; + + /// \brief Path to the directory containing the state log file + public: std::string logPath; + + /// \brief A list of events to record + public: std::list events; + + /// \brief Move to pose service name + public: std::string moveToPoseService; + + /// \brief Move to service name + public: std::string moveToService; + + /// \brief Move to service name + public: std::string followService; + + /// \brief Seek service name + public: std::string seekService; + + /// \brief Time when video recording stop request is made + public: std::chrono::time_point recordStopTime; + + /// \brief Video record service name + public: std::string videoRecordService; + + /// \brief Video encoding format + public: std::string videoFormat{"mp4"}; + + /// \brief Filename of temp video recording + public: std::string tmpVideoFilename = + "tmp_video_recording." + this->videoFormat; + + /// \brief Request to stop video recording + public: bool recordStopRequested{false}; + + /// \brief Current state of the system + public: State state = IDLE; + + /// \brief A map of event type to its start and end recording time + public: std::map> eventRecordDuration; + + /// \brief A map of event type to the camera mode + public: std::map eventCameraMode; + + /// \brief Unique dynamic rock models + public: std::set uniqueRockModels; + + /// \brief Dynamic rock pose + public: std::map rockModelPose; + + /// \brief A list of unique detectors + public: std::set detectors; + + /// \brief A list of unique robots + public: std::set robotNames; + + /// \brief Number of robots + public: unsigned int robotCount = 0; + + /// \brief total duration of the run in sim time + public: unsigned int logEndTime = 0; + + /// \brief Time when all robots have been spawned + public: double robotsSpawnTime = 0; + + /// \brief Time to append to the end of record time + public: double endTimeBuffer = 0; + + /// \brief True if recorder needs to catch up on recording so it + /// does not lag behind too much + public: bool catchupOnRecording = false; + + /// \brief Status log + public: std::ofstream statusLog; +}; + +///////////////////////////////////////////// +PlaybackEventRecorder::PlaybackEventRecorder() + : dataPtr(new PlaybackEventRecorderPrivate) +{ + // set up event types and recording duration: + // * robot deployed a marsupial child (follow camera) + // * robot deployed a breadcrumb (follow camera) + // * robot flipped over (follow camera) + // * robot enter proximity of artifact (follow camera) + // * robot exited staging area (static camera) + // * robot triggered rock fall (static camera) + this->dataPtr->eventRecordDuration["detach"] = std::make_pair(60, 120); + this->dataPtr->eventRecordDuration["breadcrumb_deploy"] = + std::make_pair(60, 120); + this->dataPtr->eventRecordDuration["flip"] = std::make_pair(120, 30); + this->dataPtr->eventRecordDuration["detect"] = std::make_pair(60, 60); + this->dataPtr->eventRecordDuration["rock_fall"] = std::make_pair(60, 120); + + this->dataPtr->eventCameraMode["detach"] = FOLLOW_CAMERA; + this->dataPtr->eventCameraMode["breadcrumb_deploy"] = FOLLOW_CAMERA; + this->dataPtr->eventCameraMode["flip"] = FOLLOW_CAMERA; + // detect event type - follow for artifacts + // There is a special check added later to use static camera for staging area + this->dataPtr->eventCameraMode["detect"] = FOLLOW_CAMERA; + this->dataPtr->eventCameraMode["rock_fall"] = STATIC_CAMERA; + + // breadcrumb and flip events no longer needed + this->dataPtr->detectors.insert("staging_area"); + + // artifact proximity events no longer needed + this->dataPtr->detectors.insert("backpack"); + this->dataPtr->detectors.insert("phone"); + this->dataPtr->detectors.insert("rescue_randy"); + this->dataPtr->detectors.insert("rope"); + this->dataPtr->detectors.insert("helmet"); + + // region of interest events - decision, vertical, elevation + this->dataPtr->detectors.insert("tile"); +} + +///////////////////////////////////////////// +PlaybackEventRecorder::~PlaybackEventRecorder() +{ +} + +////////////////////////////////////////////////// +void PlaybackEventRecorder::Configure(const ignition::gazebo::Entity &, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &/*_ecm*/, + ignition::gazebo::EventManager &_eventMgr) +{ + // Ugly, but needed because the sdf::Element::GetElement is not a const + // function and _sdf is a const shared pointer to a const sdf::Element. + auto ptr = const_cast(_sdf.get()); + + if (_sdf->HasElement("exit_on_finish")) + { + this->dataPtr->exitOnFinish = ptr->Get("exit_on_finish"); + } + + if (!_sdf->HasElement("log_path")) + { + ignerr << "Unable to record playback video. " + << "Please specify 'log_path'." << std::endl; + return; + } + + if (_sdf->HasElement("spawn_light")) + { + this->dataPtr->spawnLight = ptr->Get("spawn_light"); + } + + const sdf::ElementPtr logPathElem = ptr->GetElement("log_path"); + this->dataPtr->logPath = logPathElem->Get(); + + // load run.yml + YAML::Node runNode = YAML::LoadFile( + common::joinPaths(this->dataPtr->logPath, "run.yml")); + + // parse number of robots + this->dataPtr->robotCount = runNode["robot_count"].as(); + + // get total duration of log + std::unique_ptr log = + std::make_unique(); + std::string dbPath = + common::joinPaths(this->dataPtr->logPath, "state.tlog"); + if (!log->Open(dbPath)) + { + ignerr << "Failed to open log file [" << dbPath << "]" << std::endl; + } + this->dataPtr->logEndTime = + std::chrono::duration_cast( + log->EndTime()).count(); + + + std::string eventFilename = "filtered_events.yaml"; + if (!common::exists(common::joinPaths(this->dataPtr->logPath, eventFilename))) + eventFilename = "events.yml"; + + this->dataPtr->statusLog.open("record_status.log", std::ofstream::app); + + // load filtered_events.yml + YAML::Node node = YAML::LoadFile( + common::joinPaths(this->dataPtr->logPath, eventFilename)); + + // staging area event - there should be one in each playback + // record video until last robot exists staging area + Event stagingAreaEvent; + stagingAreaEvent.type = "detect"; + stagingAreaEvent.detector = "staging_area"; + stagingAreaEvent.time = 0; + stagingAreaEvent.state = "exit"; + stagingAreaEvent.startRecordTime = 0; + stagingAreaEvent.endRecordTime = 60; + std::map stagingAreaEventTime; + + // parse and store the list of events + for (const auto &n : node) + { + std::string type = n["type"].as(); + auto it = this->dataPtr->eventRecordDuration.find(type); + if (it == this->dataPtr->eventRecordDuration.end()) + continue; + + Event e; + e.type = type; + e.time = n["time_sec"].as(); + + // check for detect event type and record videos only for detectors that we + // are interested in. + // Currently only interested in staging area event + if (type == "detect") + { + std::string detector; + std::string state; + if (auto detectorParam = n["detector"]) + { + // filter detector events to the list defined in this->dataPtr->detector + detector = detectorParam.as(); + bool validDetector = false; + for (const auto &d : this->dataPtr->detectors) + { + if (detector.find(d) != std::string::npos) + { + validDetector = true; + break; + } + } + if (!validDetector) + continue; + } + else + continue; + + if (auto stateParam = n["state"]) + { + state = stateParam.as(); + } + else + { + continue; + } + + // Get the time when the last robot exits the staging area + // take into accout that some robots may never leave, in which case + // record until the last unique robot exit event + if (detector == "staging_area") + { + if (state == "exit" && stagingAreaEventTime.size() < + this->dataPtr->robotCount) + { + std::string robot = n["robot"].as(); + double time = n["time_sec"].as(); + if (stagingAreaEventTime.find(robot) == stagingAreaEventTime.end()) + { + stagingAreaEventTime[robot] = time; + if (time > stagingAreaEvent.time) + { + stagingAreaEvent.time = time; + stagingAreaEvent.robot = robot; + stagingAreaEvent.endRecordTime = time + it->second.second; + stagingAreaEvent.id = n["id"].as(); + } + } + } + // continue because we don't need to store every staging area detector + // event in the list. There should only be one, which we manually push + // into the events list later + continue; + } + + if (auto extraParam = n["extra"]) + { + if (auto extraTypeParam = extraParam["type"]) + { + e.extra = extraTypeParam.as(); + } + } + + e.detector = detector; + } + + // for rock fall events, we need to check the corresponding performer + // detector events to get the robot associated with this event + if (type == "rock_fall") + { + std::string model = n["model"].as(); + std::string suffix = model.substr(model.rfind("_")); + for (const auto &ev : node) + { + if (ev["type"].as() == "detect" && + math::equal(ev["time_sec"].as(), e.time) && + ev["detector"].as().find("rockfall" + suffix) + != std::string::npos) + { + e.robot = ev["robot"].as(); + e.model = model; + this->dataPtr->uniqueRockModels.insert(model); + break; + } + } + } + else + { + e.robot = n["robot"].as(); + } + + if (n["id"]) + e.id = n["id"].as(); + else + e.id = this->dataPtr->events.size(); + + e.startRecordTime = std::max(e.time - it->second.first, 0.0); + e.endRecordTime = e.time + it->second.second; + e.GenerateFilename(this->dataPtr->videoFormat); + if (!ignition::common::exists(this->dataPtr->logPath + "/" + e.filename)) + this->dataPtr->events.push_back(e); + else + { + std::cout << e.filename << " exists, skipping\n"; + auto now = std::chrono::steady_clock::now(); + this->dataPtr->statusLog << ignition::math::timePointToString(now) + << " " << e.filename << " exists, skipping" << std::endl; + } + } + + // merge artifact proximity detector events + // we don't want to record mulitple videos for each artifact detector event + // If mulitple events occurred for the same robot and artifact within some + // time period, then merge the events + double maxTimeDiff = 60.0; + std::set toRemove; + for (auto it = this->dataPtr->events.begin(); + it != this->dataPtr->events.end(); ++it) + { + auto &e = *it; + if (toRemove.find(e.id) != toRemove.end()) + continue; + if (e.type != "detect") + continue; + // merge current event with other detector events for the same robot + // if time difference between the two is less than maxTimeDiff + for (auto it2 = std::next(it, 1); it2 != this->dataPtr->events.end(); + ++it2) + { + auto &e2 = *it2; + if (e2.type == "detect" && e2.robot == e.robot && + e2.detector == e.detector && e2.extra == e.extra) + { + double dt = e2.startRecordTime - e.startRecordTime; + if (dt < maxTimeDiff && dt >= 0) + { + e.endRecordTime = e2.endRecordTime; + toRemove.insert(e2.id); + } + } + } + } + // remove events that were merged and marked for removal + this->dataPtr->events.remove_if( + [&toRemove](Event &e) {return toRemove.find(e.id) != toRemove.end();}); + + // append buffer time to the end + for (auto &e : this->dataPtr->events) + e.endRecordTime += this->dataPtr->endTimeBuffer; + stagingAreaEvent.endRecordTime += this->dataPtr->endTimeBuffer; + + // add the staging area event + if (!stagingAreaEventTime.empty()) + { + stagingAreaEvent.GenerateFilename(this->dataPtr->videoFormat); + if (!ignition::common::exists(this->dataPtr->logPath + "/" + + stagingAreaEvent.filename)) + { + // this->dataPtr->events.push_back(stagingAreaEvent); + } + else + { + std::cout << stagingAreaEvent.filename << " exists, skipping\n"; + auto now = std::chrono::steady_clock::now(); + this->dataPtr->statusLog << ignition::math::timePointToString(now) + << " " << stagingAreaEvent.filename << " exists, skipping" << std::endl; + } + } + + // don't do anything if there are no events + if (this->dataPtr->events.empty()) + { + std::cout << "No events to record: " << std::endl; + return; + } + std::cout << "Events to record: " << std::endl; + for (const auto &e : this->dataPtr->events) + { + std::cout << "Event: " << std::endl; + std::cout << " type: " << e.type << std::endl; + std::cout << " robot: " << e.robot << std::endl; + std::cout << " time: " << e.time << std::endl; + std::cout << " detector: " << ((e.detector.empty()) ? "N/A" : e.detector) + << std::endl; + std::cout << " state: " << ((e.state.empty()) ? "N/A" : e.state) + << std::endl; + std::cout << " model: " << ((e.model.empty()) ? "N/A" : e.model) + << std::endl; + std::cout << " extra: " << ((e.extra.empty()) ? "N/A" : e.extra) + << std::endl; + std::cout << " start time: " << e.startRecordTime << std::endl; + std::cout << " end time: " << e.endRecordTime << std::endl; + + } + + this->dataPtr->eventManager = &_eventMgr; + + // For move to service requests + this->dataPtr->moveToPoseService = "/gui/move_to/pose"; + this->dataPtr->moveToService = "/gui/move_to"; + + // For follow service requests + this->dataPtr->followService = "/gui/follow"; + + // For video record requests + this->dataPtr->videoRecordService = "/gui/record_video"; + + this->dataPtr->node.Subscribe("/gui/record_video/stats", + &PlaybackEventRecorderPrivate::OnRecorderStats, this->dataPtr.get()); + + + this->dataPtr->loadTime = std::chrono::system_clock::now(); +} + +////////////////////////////////////////////////// +void PlaybackEventRecorder::PreUpdate( + const ignition::gazebo::UpdateInfo &/*_info*/, + ignition::gazebo::EntityComponentManager &/*_ecm*/) +{ +} + +////////////////////////////////////////////////// +void PlaybackEventRecorder::PostUpdate( + const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + if (this->dataPtr->state == IDLE && + this->dataPtr->events.empty()) + { + if (this->dataPtr->exitOnFinish) + exit(0); + return; + } + + // Get world name recorded in log + if (this->dataPtr->logWorldName.empty()) + { + _ecm.Each( + [&](const Entity & /*_entity*/, + const components::World *, + const components::Name *_name)->bool + { + this->dataPtr->logWorldName = _name->Data(); + return true; + }); + } + + // Get rock fall model pose. + { + _ecm.Each( + [&](const Entity & /*_entity*/, + const components::Model *, + const components::Name *_name, + const components::Pose *_pose, + const components::Static *)->bool + { + std::string rockName = _name->Data(); + if (this->dataPtr->rockModelPose.find(rockName) + == this->dataPtr->rockModelPose.end() && + rockName.find("dynamic_rocks") != std::string::npos) + { + this->dataPtr->rockModelPose[rockName] = _pose->Data(); + } + return true; + }); + } + + // wait for a few seconds before doing anything + std::chrono::time_point t = + std::chrono::system_clock::now(); + if (t - this->dataPtr->loadTime < std::chrono::seconds(5)) + return; + + int64_t s, ns; + std::tie(s, ns) = ignition::math::durationToSecNsec(_info.simTime); + + int64_t rs, rns; + std::tie(rs, rns) = ignition::math::durationToSecNsec(_info.realTime); + + // step the sim for a few seconds for scene to load on gui side + if (!this->dataPtr->started) + { + if (_info.paused) + this->dataPtr->eventManager->Emit(false); + + // look for robots + _ecm.Each( + [&](const gazebo::Entity &, + const gazebo::components::Sensor *, + const gazebo::components::ParentEntity *_parent) -> bool + { + + // Get the model. We are assuming that a sensor is attached to + // a link. + auto model = _ecm.Component( + _parent->Data()); + + if (model) + { + auto mName = + _ecm.Component(model->Data()); + if (this->dataPtr->robotNames.find(mName->Data()) == + this->dataPtr->robotNames.end()) + this->dataPtr->robotNames.insert(mName->Data()); + } + + return true; + }); + + // wait for robots to spawn + if (s < 60 && this->dataPtr->robotNames.size() < this->dataPtr->robotCount) + { + static bool informed = false; + if (!informed) + { + ignmsg << "Waiting for robots to spawn" << std::endl; + informed = true; + } + return; + } + else if (this->dataPtr->robotsSpawnTime <= 0) + { + this->dataPtr->robotsSpawnTime = s; + } + + // robots have spawned, now wait for the initial levels to load + if (s < this->dataPtr->robotsSpawnTime + 15.0) + { + static bool informed = false; + if (!informed) + { + ignmsg << "Waiting for levels to load" << std::endl; + informed = true; + } + + return; + } + + this->dataPtr->started = true; + } + + // idle state - get next event + if (this->dataPtr->state == IDLE) + { + if (this->dataPtr->events.empty()) + { + if (this->dataPtr->exitOnFinish) + exit(0); + return; + } + + // get next event to record + this->dataPtr->event = this->dataPtr->events.front(); + this->dataPtr->events.pop_front(); + + auto now = std::chrono::steady_clock::now(); + this->dataPtr->statusLog << ignition::math::timePointToString(now) + << " " << this->dataPtr->event.filename << " starting. " + << this->dataPtr->events.size() << " remaining." << std::endl; + + // set camera mode + auto cameraMode = this->dataPtr->eventCameraMode[this->dataPtr->event.type]; + this->dataPtr->cameraFollow = (cameraMode == FOLLOW_CAMERA) && + (this->dataPtr->event.detector != "staging_area"); + + ignmsg << "Playing event: " << this->dataPtr->event.startRecordTime << " " + << this->dataPtr->event.type << " " + << (this->dataPtr->event.detector.empty() ? + "" : this->dataPtr->event.detector) << " " + << (this->dataPtr->event.model.empty() ? + "" : this->dataPtr->event.model) << " " + << (this->dataPtr->event.extra.empty() ? + "" : this->dataPtr->event.extra) << " " + << this->dataPtr->event.robot<< std::endl; + + this->dataPtr->Seek(this->dataPtr->event.startRecordTime); + this->dataPtr->state = SEEK_BEGIN; + ignmsg << "State: Transitioning to SEEK_BEGIN" << std::endl; + return; + } + + // seek event state - seek to time of event and get robot pose so we can + // move camera to a pose where the event occurred +// if (this->dataPtr->state == SEEK_EVENT) +// { +// // time of event - find robot and set up camera +// if (s == static_cast(this->dataPtr->event.time)) +// { +// // wait for a few real time seconds after arriving at time of event +// if (!this->dataPtr->waiting) +// { +// this->dataPtr->eventManager->Emit(true); +// this->dataPtr->waiting = true; +// this->dataPtr->waitStartTime = std::chrono::system_clock::now(); +// return; +// } +// else if (t - this->dataPtr->waitStartTime > std::chrono::seconds(5)) +// { +// this->dataPtr->waiting = false; +// this->dataPtr->eventManager->Emit(false); +// } +// else +// { +// return; +// } +// +// // get pose of robot for the current event +// auto entity = _ecm.EntityByComponents( +// components::Name(this->dataPtr->event.robot), +// components::Model()); +// if (entity == kNullEntity) +// { +// ignerr << "Unable to record event. Failed to get robot with name: " +// << this->dataPtr->event.robot << std::endl; +// this->dataPtr->state = IDLE; +// return; +// } +// +// auto poseComp = _ecm.Component(entity); +// if (!poseComp) +// { +// ignerr << "Unable to record event. Failed to get pose for robot: " +// << this->dataPtr->event.robot << std::endl; +// this->dataPtr->state = IDLE; +// return; +// } +// +// math::Pose3d pose = poseComp->Data(); +// +// // rock fall event: move camera to some offset above rock fall model and +// // orient it to face down +// if (this->dataPtr->event.type == "rock_fall") +// { +// auto it = this->dataPtr->rockModelPose.find(this->dataPtr->event.model); +// if (it != this->dataPtr->rockModelPose.end()) +// { +// math::Pose3d p = it->second; +// p.Pos() += math::Vector3d(-12.5, -12.5, 5.5); +// p.Rot() = math::Quaterniond(0, 0.4, IGN_PI/4.0); +// this->dataPtr->MoveTo(p); +// } +// else +// { +// this->dataPtr->MoveTo(this->dataPtr->event.robot); +// } +// } +// // staging area event: move camera to somewhere above the staging area +// // looking down at all the robots +// else if (this->dataPtr->event.type == "detect" && +// this->dataPtr->event.detector == "staging_area") +// { +// math::Pose3d p(-3.5, 0, 5, 0, 0.4, 0); +// this->dataPtr->MoveTo(p); +// } +// // all other events: move gui camera to robot for now +// else +// { +// this->dataPtr->MoveTo(this->dataPtr->event.robot); +// } +// +// // seek to a time x min before the event. +// this->dataPtr->Seek(this->dataPtr->event.startRecordTime); +// this->dataPtr->state = SEEK_BEGIN; +// ignmsg << "State: Transitioning to SEEK_BEGIN" << std::endl; +// } +// +// return; +// } + + // seek begin state - seek playback to x min before the event and start + // recording + if (this->dataPtr->state == SEEK_BEGIN) + { + if (s == static_cast(this->dataPtr->event.startRecordTime)) + { + // wait for a few real time seconds after arriving at time before event + if (!this->dataPtr->waiting) + { + this->dataPtr->eventManager->Emit(true); + this->dataPtr->waiting = true; + this->dataPtr->waitStartTime = std::chrono::system_clock::now(); + return; + } + else if (t - this->dataPtr->waitStartTime > std::chrono::seconds(5)) + { + this->dataPtr->waiting = false; + this->dataPtr->eventManager->Emit(false); + } + else + { + return; + } + + // spawn a light if needed + // we need to spawn a light on every seek event because new entities + // that get spawned in playback are removed when jumping back in time + if (this->dataPtr->spawnLight) + { + auto lightEntity = _ecm.EntityByComponents( + components::Name("spawned_light")); + if (lightEntity == kNullEntity) + this->dataPtr->SpawnLight(); + } + + // if in camera follow mode, reset entity exists values so we can do + // check to make sure the entity exists first before asking the gui + // camera to follow it + if (this->dataPtr->cameraFollow) + { + this->dataPtr->entityExists = false; + this->dataPtr->cameraFollowing = false; + } + // static camera mode + else + { + // rock fall event: move camera to some offset above rock fall model and + // orient it to face down + if (this->dataPtr->event.type == "rock_fall") + { + auto it = this->dataPtr->rockModelPose.find(this->dataPtr->event.model); + if (it != this->dataPtr->rockModelPose.end()) + { + math::Pose3d p = it->second; + p.Pos() += math::Vector3d(-12.5, -12.5, 5.5); + p.Rot() = math::Quaterniond(0, 0.4, IGN_PI/4.0); + this->dataPtr->MoveTo(p); + } + else + { + ignerr << "Unable to move camera to dynamic rock model: " + << this->dataPtr->event.model << std::endl; + } + } + // staging area event: move camera to somewhere above the staging area + // looking down at all the robots + else if (this->dataPtr->event.type == "detect" && + this->dataPtr->event.detector == "staging_area") + { + math::Pose3d p(-3.5, 0, 5, 0, 0.4, 0); + this->dataPtr->MoveTo(p); + } + } + + this->dataPtr->waitForScene = true; + this->dataPtr->state = RECORDING; + ignmsg << "State: Transitioning to RECORDING" << std::endl; + } + } + + // recording state - record video to disk until y min after time of event + if (this->dataPtr->state == RECORDING) + { + // pause and wait a few seconds for scene to initialize on gui + if (this->dataPtr->waitForScene) + { + if (!this->dataPtr->waiting) + { + this->dataPtr->waitStartTime = t; + this->dataPtr->waiting = true; + } + // play for a small period of time to get scene state msg over to gui + else if (t - this->dataPtr->waitStartTime > std::chrono::milliseconds(10)) + { + // pause and wait for scene to initialize on gui + this->dataPtr->eventManager->Emit(true); + if (t - this->dataPtr->waitStartTime > std::chrono::seconds(15)) + { + this->dataPtr->eventManager->Emit(false); + this->dataPtr->waiting = false; + this->dataPtr->recorderStatsMsg.Clear(); + this->dataPtr->catchupOnRecording = false; + // start video recording + this->dataPtr->Record(true); + this->dataPtr->waitForScene = false; + ignmsg << "Recording started: " << s << "s (sim time), " + << rs << "s (real time)" << std::endl; + } + } + return; + } + + // catch edge case. If we seek to a time in playback that the robot has + // not been spawned yet, e.g. beginning of sim, then we need to wait + // for robot to spawn before sending the follow cmd + if (this->dataPtr->cameraFollow) + { + if (!this->dataPtr->entityExists) + { + // check if robot exists + auto entity = _ecm.EntityByComponents( + components::Name(this->dataPtr->event.robot), + components::Model()); + if (entity != kNullEntity) + { + this->dataPtr->entityExists = true; + this->dataPtr->Follow(this->dataPtr->event.robot); + } + } + else if (!this->dataPtr->cameraFollowing) + { + // wait for a few real time seconds for the robot entity data to be ready on + // gui side + if (!this->dataPtr->waiting) + { + this->dataPtr->waiting = true; + this->dataPtr->waitStartTime = std::chrono::system_clock::now(); + return; + } + else if (t - this->dataPtr->waitStartTime > std::chrono::seconds(10)) + { + this->dataPtr->waiting = false; + this->dataPtr->Follow(this->dataPtr->event.robot); + this->dataPtr->cameraFollowing = true; + } + } + } + + // record until we reached the end record time or end of playback + msgs::Time recorderStats; + { + std::lock_guard lock(this->dataPtr->recorderStatsMutex); + recorderStats = this->dataPtr->recorderStatsMsg; + + } + if (!this->dataPtr->recordStopRequested) + { + // check for end of playback, indicated by paused state and + // also double check against log end time + bool endOfPlayback = false; + if (_info.paused && recorderStats.sec() >= + (this->dataPtr->logEndTime - + this->dataPtr->event.startRecordTime - 1.0)) + { + endOfPlayback = true; + ignmsg << "Possible end of Playback reached. Stopping video recorder" + << std::endl; + } + else + { + // check if we need to catch up on recordiing + int lag = (s - this->dataPtr->event.startRecordTime) - + recorderStats.sec(); + if (!this->dataPtr->catchupOnRecording) + { + if (recorderStats.sec() != 0 && lag > 10) + { + this->dataPtr->eventManager->Emit(true); + this->dataPtr->catchupOnRecording = true; + ignmsg << "Recording Video: " << recorderStats.sec() + << "s (sim time)" << std::endl; + } + } + else + { + if (lag < 5) + { + this->dataPtr->eventManager->Emit(false); + this->dataPtr->catchupOnRecording = false; + } + } + } + + if (endOfPlayback || recorderStats.sec() >= + static_cast(this->dataPtr->event.endRecordTime - + this->dataPtr->event.startRecordTime)) + { + // stop recording + this->dataPtr->Record(false); + this->dataPtr->recordStopRequested = true; + this->dataPtr->recordStopTime = std::chrono::system_clock::now(); + ignmsg << "Recording stopped: " << recorderStats.sec() + << "s (sim time), " << rs << "s (real time)" << std::endl; + + // disable camera following + if (this->dataPtr->cameraFollow) + { + this->dataPtr->Follow(std::string()); + this->dataPtr->cameraFollowing = false; + this->dataPtr->cameraFollow = false; + } + + return; + } + } + + // Video recording stopped. We need to save a copy of the video file + if (this->dataPtr->recordStopRequested) + { + // give it some time for video encording to finiish encoding + std::chrono::time_point now = + std::chrono::system_clock::now(); + if (now - this->dataPtr->recordStopTime < std::chrono::seconds(5)) + return; + + if (common::exists(this->dataPtr->tmpVideoFilename)) + { + common::moveFile(this->dataPtr->tmpVideoFilename, + this->dataPtr->event.filename); + + ignmsg << "Saving video recording to: " + << this->dataPtr->event.filename << std::endl; + + auto now = std::chrono::steady_clock::now(); + this->dataPtr->statusLog << ignition::math::timePointToString(now) + << " Saving video recording to: " + << this->dataPtr->event.filename << std::endl; + + // Remove old temp file, if it exists. + std::remove(this->dataPtr->tmpVideoFilename.c_str()); + } + this->dataPtr->recordStopRequested = false; + + this->dataPtr->state = IDLE; + ignmsg << "State: Transitioning to IDLE" << std::endl; + return; + } + } +} + +////////////////////////////////////////////////// +void PlaybackEventRecorderPrivate::MoveTo(const math::Pose3d &_pose) +{ + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error sending move to request" << std::endl; + }; + + ignition::msgs::GUICamera req; + msgs::Set(req.mutable_pose(), _pose); + if (this->node.Request(this->moveToPoseService, req, cb)) + { + igndbg << "Move to pose: " << _pose << std::endl; + } +} + +////////////////////////////////////////////////// +void PlaybackEventRecorderPrivate::MoveTo(const std::string &_entity) +{ + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error sending move to request" << std::endl; + }; + + ignition::msgs::StringMsg req; + req.set_data(_entity); + if (this->node.Request(this->moveToService, req, cb)) + { + igndbg << "Move to entity: " << _entity << std::endl; + } +} + +////////////////////////////////////////////////// +void PlaybackEventRecorderPrivate::Follow(const std::string &_entity) +{ + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error sending follow request" << std::endl; + }; + + ignition::msgs::StringMsg req; + req.set_data(_entity); + if (this->node.Request(this->followService, req, cb)) + { + igndbg << "Follow entity: " << _entity << std::endl; + } +} + +////////////////////////////////////////////////// +void PlaybackEventRecorderPrivate::Seek(double _timeSec) +{ + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error sending playback control request" << std::endl; + }; + + msgs::LogPlaybackControl playbackMsg; + + playbackMsg.mutable_seek()->set_sec(_timeSec); + playbackMsg.mutable_seek()->set_nsec(0.0); + playbackMsg.set_pause(false); + if (this->node.Request( + "/world/default/playback/control", + playbackMsg, cb)) + { + igndbg << "Seek to time: " << _timeSec << std::endl; + } +} + +////////////////////////////////////////////////// +void PlaybackEventRecorderPrivate::Record(bool _record) +{ + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error sending record request" << std::endl; + }; + + ignition::msgs::VideoRecord req; + + if (_record) + { + std::string filename = this->tmpVideoFilename; + req.set_start(true); + req.set_format(this->videoFormat); + req.set_save_filename(filename); + igndbg << "Recording video " << filename << std::endl; + } + else + { + igndbg << "Stopping video recorder" << std::endl; + req.set_stop(true); + } + this->node.Request(this->videoRecordService, req, cb); +} + +////////////////////////////////////////////////// +void PlaybackEventRecorderPrivate::SpawnLight() +{ + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error sending record request" << std::endl; + }; + + ignition::msgs::EntityFactory req; + std::string spawnStr = "" \ + ""\ + ""\ + "0 0 10 0 0 0"\ + ""\ + ""; + req.set_sdf(spawnStr); + req.set_allow_renaming(false); + // for factory service requests + std::string createService = "/world/" + this->logWorldName + + "/create"; + this->node.Request(createService, req, cb); +} + +////////////////////////////////////////////////// +void PlaybackEventRecorderPrivate::OnRecorderStats(const msgs::Time &_msg) +{ + std::lock_guard lock(this->recorderStatsMutex); + this->recorderStatsMsg = _msg; +} diff --git a/subt_ign/src/PlaybackEventRecorder.hh b/subt_ign/src/PlaybackEventRecorder.hh new file mode 100644 index 00000000..1b528b3d --- /dev/null +++ b/subt_ign/src/PlaybackEventRecorder.hh @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef SUBT_IGN_PLAYBACKEVENTRECORDER_HH_ +#define SUBT_IGN_PLAYBACKEVENTRECORDER_HH_ + +#include +#include + +namespace subt +{ + class PlaybackEventRecorderPrivate; + /// \brief This plugin records videos on log playback for events that + /// occurred during simulation. + class PlaybackEventRecorder : + public ignition::gazebo::System, + public ignition::gazebo::ISystemConfigure, + public ignition::gazebo::ISystemPreUpdate, + public ignition::gazebo::ISystemPostUpdate + { + /// \brief Constructor + public: PlaybackEventRecorder(); + + /// \brief Destructor + public: ~PlaybackEventRecorder(); + + // Documentation inherited + public: void Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + // Documentation inherited + public: void PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; + }; +} + +#endif diff --git a/subt_ign/src/path_tracer.cc b/subt_ign/src/path_tracer.cc index 01235509..eac4ff05 100644 --- a/subt_ign/src/path_tracer.cc +++ b/subt_ign/src/path_tracer.cc @@ -68,8 +68,10 @@ MarkerColor::MarkerColor(const ignition::math::Color &_ambient, } ////////////////////////////////////////////////// -Processor::Processor(const std::string &_path, const std::string &_configPath) +Processor::Processor(const std::string &_path, const std::string &_configPath, const std::string &_partition, const std::string &_cameraPose, const std::string &_worldName, bool _onlyCheck) { + this->worldName = _worldName; + YAML::Node cfg; if (!_configPath.empty()) { @@ -179,15 +181,53 @@ Processor::Processor(const std::string &_path, const std::string &_configPath) {1.0, 0.761, 0.4, 1.0})); } - // Create the transport node with the partition used by simulation - // world. - ignition::transport::NodeOptions opts; - opts.SetPartition("PATH_TRACER"); - this->markerNode = std::make_unique(opts); - this->ClearMarkers(); + ignition::msgs::Boolean boolRep; + bool result = false; + bool executed = false; + + + if (!_onlyCheck) + { + // Create the transport node with the partition used by simulation + // world. + ignition::transport::NodeOptions opts; + opts.SetPartition(_partition); + this->markerNode = std::make_unique(opts); + this->ClearMarkers(); + this->Pause(true); + + // Move camera + std::vector camPoseParts = + ignition::common::split(_cameraPose, " "); + ignition::msgs::GUICamera guiCamReq; + guiCamReq.mutable_pose()->mutable_orientation()->set_x(0); + guiCamReq.mutable_pose()->mutable_orientation()->set_y(0.707); + guiCamReq.mutable_pose()->mutable_orientation()->set_z(0); + guiCamReq.mutable_pose()->mutable_orientation()->set_w(0.707); + guiCamReq.mutable_pose()->mutable_position()->set_x( + std::stof(camPoseParts[0])); + guiCamReq.mutable_pose()->mutable_position()->set_y( + std::stof(camPoseParts[1])); + guiCamReq.mutable_pose()->mutable_position()->set_z( + std::stof(camPoseParts[2])); + // Make sure we actually move + while (!result || !executed) + { + std::cout << "Moving camera to " << _cameraPose << std::endl; + executed = this->markerNode->Request("/gui/move_to/pose", guiCamReq, + 2000, boolRep, result); + std::this_thread::sleep_for(std::chrono::seconds(5)); + } + + this->markerNode->Subscribe("/clock", &Processor::ClockCb , this); - // Subscribe to the artifact poses. - this->SubscribeToArtifactPoseTopics(); + // recorder stats + this->markerNode->Subscribe("/gui/record_video/stats", + &Processor::RecorderStatsCb , this); + + // Subscribe to the artifact poses. + this->SubscribeToArtifactPoseTopics(); + } // Playback the log file. std::unique_lock lock(this->mutex); @@ -195,37 +235,8 @@ Processor::Processor(const std::string &_path, const std::string &_configPath) this->cv.wait(lock); - // Create a transport node that uses the default partition. - ignition::transport::Node node; - - // Subscribe to the robot pose topic - bool subscribed = false; - for (int i = 0; i < 5 && !subscribed; ++i) - { - std::vector topics; - node.TopicList(topics); - - // Subscribe to the first /dynamic_pose/info topic - for (auto const &topic : topics) - { - if (topic.find("/dynamic_pose/info") != std::string::npos) - { - // Subscribe to a topic by registering a callback. - if (!node.Subscribe(topic, &Processor::Cb, this)) - { - std::cerr << "Error subscribing to topic [" - << topic << "]" << std::endl; - return; - } - subscribed = true; - break; - } - } - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - - // Wait for log playback to end. - playbackThread.join(); + std::vector scoredArtifacts; + std::set artifactProximity; // Process the events log file. std::string eventsFilepath = _path + "/events.yml"; @@ -274,9 +285,41 @@ Processor::Processor(const std::string &_path, const std::string &_configPath) data->type = REPORT; data->pos = reportedPos; data->score = events[i]["points_scored"].as(); + if (data->score > 0) + { + this->scoreTimes.push_back(sec); + scoredArtifacts.push_back( + events[i]["closest_artifact_name"].as()); + } this->logData[sec].push_back(std::move(data)); } + else if (events[i]["type"].as() == "finished") + { + int sec = events[i]["time_sec"].as(); + std::unique_ptr data = std::make_unique(); + data->type = PHASE; + this->logData[sec].push_back(std::move(data)); + } + else if (events[i]["type"].as() == "started") + { + int sec = events[i]["time_sec"].as(); + std::unique_ptr data = std::make_unique(); + data->type = PHASE; + this->logData[sec].push_back(std::move(data)); + } + else if (events[i]["type"].as() == "detect") + { + if (events[i]["state"].as() == "enter") + { + if (events[i]["extra"] && events[i]["extra"]["type"] && + events[i]["extra"]["type"].as() == + "artifact_proximity") + { + artifactProximity.insert(events[i]["detector"].as()); + } + } + } } } else @@ -284,11 +327,141 @@ Processor::Processor(const std::string &_path, const std::string &_configPath) std::cerr << "Missing " << eventsFilepath << ". There will be no artifact report visualization.\n"; } + + // Make sure there is a zero second event. + std::unique_ptr data = std::make_unique(); + data->type = PHASE; + this->logData[0].push_back(std::move(data)); + + for (const std::string &artifact : scoredArtifacts) + { + if (artifactProximity.find(artifact) == artifactProximity.end()) + std::cerr << "Scored artifact not approached for " << artifact << ".\n"; + } + + // Create a transport node that uses the default partition. + ignition::transport::Node node; + + // Subscribe to the robot pose topic + bool subscribed = false; + for (int i = 0; i < 5 && !subscribed; ++i) + { + std::vector topics; + node.TopicList(topics); + + // Subscribe to the first /dynamic_pose/info topic + for (auto const &topic : topics) + { + if (topic.find("/dynamic_pose/info") != std::string::npos) + { + // Subscribe to a topic by registering a callback. + if (!node.Subscribe(topic, &Processor::Cb, this)) + { + std::cerr << "Error subscribing to topic [" + << topic << "]" << std::endl; + return; + } + subscribed = true; + break; + } + } + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + // Wait for log playback to end. + playbackThread.join(); + + // This block loop is for run validation. Safe to ignore for path + // visualization. + if (_onlyCheck) + { + for (std::map>>::iterator iter = + this->logData.begin(); iter != this->logData.end(); ++iter) + { + for (std::unique_ptr &data : iter->second) + { + if (data->type == REPORT) + { + const ReportData *report = static_cast(data.get()); + if (report->score > 0) + { + std::cout << "- scored:\n"; + + std::map scorePoses; + // Get the most recent poses of the robots. + for (auto iter2 = this->logData.begin(); + iter2 != this->logData.end() && iter2->first <= iter->first; + ++iter2) + { + for (std::unique_ptr &data2 : iter2->second) + { + if (data2->type == ROBOT) + { + const RobotPoseData *poseData = + static_cast(data2.get()); + for (std::map>::const_iterator poseIter = poseData->poses.begin(); poseIter != poseData->poses.end(); poseIter++) + { + //if (poseIter->first != "breadcrumb_node") + { + scorePoses[poseIter->first] = + (*(poseIter->second.rbegin())).Pos(); + } + } + } + } + } + + for (const auto &p : scorePoses) + { + std::cout << " " << p.first << ": " << p.second << std::endl; + } + } + + } + } + } + + return; + } + + // Display all of the artifacts using visual markers. this->DisplayArtifacts(); + // Start video recording + ignition::msgs::VideoRecord startRecordReq; + startRecordReq.set_start(true); + startRecordReq.set_format("mp4"); + startRecordReq.set_save_filename("/tmp/ign/output/0000-robot_paths.mp4"); + result = false; + executed = false; + + // Make sure we actually move + while (!result || !executed) + { + std::cout << "Start recording \n"; + executed = this->markerNode->Request("/gui/record_video", startRecordReq, + 2000, boolRep, result); + } + // Display all of the poses using visual markers. this->DisplayPoses(); + + ignition::msgs::VideoRecord endRecordReq; + endRecordReq.set_stop(true); + result = false; + executed = false; + + // Make sure we actually move + while (!result || !executed) + { + std::cout << "End recording \n"; + executed = this->markerNode->Request("/gui/record_video", endRecordReq, + 2000, boolRep, result); + } + + // Wait a bit for the video recorder to stop. + std::this_thread::sleep_for(std::chrono::seconds(60)); } ////////////////////////////////////////////////// @@ -367,6 +540,28 @@ void Processor::SubscribeToArtifactPoseTopics() } } +///////////////////////////////////////////////// +void Processor::ClockCb(const ignition::msgs::Clock &_msg) +{ + std::lock_guard lock(this->stepMutex); + //std::cout << "ClockCb[" << _msg.sim().sec() << "]\n"; + this->simTime = _msg.sim().sec(); + if (this->simTime >= this->nextSimTime) + this->stepCv.notify_one(); + + /*std::lock_guard lock(this->stepMutex); + if (this->nextSimTime > 0 && this->simTime.sim().sec() >= this->nextSimTime) + this->stepCv.notify_one(); + */ +} + +///////////////////////////////////////////////// +void Processor::RecorderStatsCb(const ignition::msgs::Time &_msg) +{ + std::lock_guard lock(this->recorderStatsMutex); + this->recorderStatsMsg = _msg; +} + ///////////////////////////////////////////////// void Processor::ArtifactCb(const ignition::msgs::Pose_V &_msg) { @@ -391,33 +586,93 @@ void Processor::ArtifactCb(const ignition::msgs::Pose_V &_msg) } } +////////////////////////////////////////////////// +void Processor::Pause(bool _pause) +{ + ignition::msgs::WorldControl msg; + ignition::msgs::Boolean boolRep; + bool result; + result = false; + + msg.set_pause(_pause); + bool executed = this->markerNode->Request( + "/world/" + this->worldName + "/control", msg, 2000, boolRep, result); + if (!executed || !result) + std::cerr << "\n!!! ERROR: Unable to pause simulation\n"; +} + +////////////////////////////////////////////////// +void Processor::StepUntil(int _sec) +{ + ignition::msgs::WorldControl msg; + ignition::msgs::Boolean boolRep; + bool result; + result = false; + + msg.mutable_run_to_sim_time()->set_sec(_sec); + + std::unique_lock lock(this->stepMutex); + this->markerNode->Request( + "/world/" + this->worldName + "/control", msg, 500, boolRep, result); + this->stepCv.wait(lock); +} + ////////////////////////////////////////////////// void Processor::DisplayPoses() { + this->startSimTime = this->simTime; + + // Iterate through all the stored poses. for (std::map>>::iterator iter = this->logData.begin(); iter != this->logData.end(); ++iter) { - auto start = std::chrono::steady_clock::now(); - printf("\r %ds/%ds (%06.2f%%)", iter->first, this->logData.rbegin()->first, - static_cast(iter->first) / this->logData.rbegin()->first * 100); - fflush(stdout); - + // Display new visual markers. for (std::unique_ptr &data : iter->second) { data->Render(this); } - // Get the next time stamp, and sleep the correct amount of time. + // Get the next time stamp, and run simulation to thattime. auto next = std::next(iter, 1); if (next != this->logData.end()) { - int sleepTime = (((next->first - iter->first) / this->rtf)*1000); - auto duration = std::chrono::steady_clock::now() - start; - std::this_thread::sleep_for(std::chrono::milliseconds(sleepTime) - - std::chrono::duration_cast( - duration)); + // This is the next sim time that contains new visual data. + this->nextSimTime = next->first + this->startSimTime; + + // Debug output + printf("%ds/%ds (%06.2f%%) start=%d currDelta=%d \ +nextDelta=%d next=%d curr=%ld\n", + iter->first, this->logData.rbegin()->first, + static_cast(iter->first) / this->logData.rbegin()->first*100, + this->startSimTime, iter->first, next->first, this->nextSimTime, + this->simTime); + // Step simulation to the new timestamp + this->StepUntil(this->nextSimTime); + } + + double dt = 0; + { + std::lock_guard lock(this->recorderStatsMutex); + dt = this->simTime - this->recorderStatsMsg.sec(); + // std::cerr << "simtime vs recorder stats time : " + // << this->simTime << " vs " << this->recorderStatsMsg.sec() + // << " " << dt << std::endl; + } + if (dt > 10) + { + std::cout << "Pausing to catch up\n"; + while (dt >= 5) + { + std::this_thread::sleep_for(std::chrono::seconds(1)); + std::lock_guard lock(this->recorderStatsMutex); + dt = this->simTime - this->recorderStatsMsg.sec(); + } + std::cout << "Resuming playback\n"; } } + + // Wait a bit for the video recorder to catch up. + std::this_thread::sleep_for(std::chrono::seconds(15)); } ///////////////////////////////////////////////// @@ -475,6 +730,7 @@ void Processor::SpawnMarker(MarkerColor &_color, void Processor::Cb(const ignition::msgs::Pose_V &_msg) { std::unique_ptr data(new RobotPoseData); + int sec = _msg.header().stamp().sec(); // Process each pose in the message. for (int i = 0; i < _msg.pose_size(); ++i) @@ -482,6 +738,7 @@ void Processor::Cb(const ignition::msgs::Pose_V &_msg) // Only conder robots. std::string name = _msg.pose(i).name(); if (name.find("_wheel") != std::string::npos || + name.find("Rock") != std::string::npos || name.find("rotor_") != std::string::npos || name == "base_link") { continue; } @@ -495,15 +752,26 @@ void Processor::Cb(const ignition::msgs::Pose_V &_msg) this->prevPose[name] = pose; } + // The following line is here to capture robot poses close to + // artifact report attempts. + bool forceRecord = false; + /*for (int t : scoreTimes) + { + if (std::abs(t-sec) < 4) + { + forceRecord = true; + break; + } + }*/ + // Filter poses. - if (this->prevPose[name].Pos().Distance(pose.Pos()) > 1.0) + if (this->prevPose[name].Pos().Distance(pose.Pos()) > 1.0 || forceRecord) { data->poses[name].push_back(pose); this->prevPose[name] = pose; } } - int sec = _msg.header().stamp().sec(); // Store data. if (!data->poses.empty()) this->logData[sec].push_back(std::move(data)); @@ -536,7 +804,7 @@ void ReportData::Render(Processor *_p) { _p->SpawnMarker(_p->artifactColors["correct_report_color"], this->pos, ignition::msgs::Marker::SPHERE, - ignition::math::Vector3d(4, 4, 4)); + ignition::math::Vector3d(10, 10, 10)); } else { @@ -546,14 +814,25 @@ void ReportData::Render(Processor *_p) } } +///////////////////////////////////////////////// +void PhaseData::Render(Processor *) +{ + // no-op +} + ///////////////////////////////////////////////// int main(int _argc, char **_argv) { // Create and run the processor. - if (_argc > 2) - Processor p(_argv[1], _argv[2]); + if (_argc == 7) + Processor p(_argv[1], _argv[2], _argv[3], _argv[4], _argv[5], true); + else if (_argc == 6) + Processor p(_argv[1], _argv[2], _argv[3], _argv[4], _argv[5], false); else - Processor p(_argv[1]); + std::cerr << "Invalid number of arguments. \n" + << "./path_tracer " + << " \"\" " << std::endl; + std::cout << "\nPlayback complete.\n"; return 0; } diff --git a/subt_ign/src/path_tracer.hh b/subt_ign/src/path_tracer.hh index 1fbf57a9..26edcb28 100644 --- a/subt_ign/src/path_tracer.hh +++ b/subt_ign/src/path_tracer.hh @@ -132,7 +132,8 @@ class Processor; enum DataType { ROBOT = 0, - REPORT = 1 + REPORT = 1, + PHASE = 2 }; /// \brief Color properties for a marker. @@ -202,6 +203,12 @@ class ReportData : public Data public: void Render(Processor *_p); }; +/// \brief Phase change data +class PhaseData : public Data +{ + public: void Render(Processor *_p); +}; + /// \brief The log file processor. class Processor { @@ -209,8 +216,16 @@ class Processor /// visualization. /// \param[in] _path Path to the directory containing the log files. /// \param[in] _configPath Path to the configuration file, or empty string. + /// \param[in] _partition Ignition transport partition. + /// \param[in] _cameraPose Initial camera pose. + /// \param[in] _worldName Name of the world. + /// \param[in] _onlyCheck True to only check, anot make a video. public: Processor(const std::string &_path, - const std::string &_configPath = ""); + const std::string &_configPath, + const std::string &_partition, + const std::string &_cameraPose, + const std::string &_worldName, + bool _onlyCheck); /// \brief Destructor public: ~Processor(); @@ -247,6 +262,22 @@ class Processor /// \brief This callback is triggered on every pose message in the log file. public: void Cb(const ignition::msgs::Pose_V &_msg); + /// \brief Callback for the clock message. + /// \param[in] _msg Clock message + public: void ClockCb(const ignition::msgs::Clock &_msg); + + /// \brief Recorder stats callback. + /// \param[in] _msg Recorder stats message. + public: void RecorderStatsCb(const ignition::msgs::Time &_msg); + + /// \brief Step simulation to a specific time. + /// \param[in] _sec Simulation seconds. + public: void StepUntil(int _sec); + + /// \brief Pause simulation. + /// \param[in] _pause True to pause simulation. + public: void Pause(bool _pause); + /// \brief Mapping of robot name to color public: std::map robots; @@ -279,4 +310,20 @@ class Processor /// \brief Realtime factor for playback. private: double rtf = 1.0; + + private: std::vector scoreTimes; + + private: int simTime; + private: int startSimTime = 0; + private: int nextSimTime = -1; + private: std::mutex stepMutex; + private: std::condition_variable stepCv; + private: std::string worldName; + + /// \brief Recorder stats msg. + public: ignition::msgs::Time recorderStatsMsg; + + + /// \brief Mutex to protect recorder stats msg. + public: std::mutex recorderStatsMutex; }; diff --git a/subt_ign/src/path_tracer.yml b/subt_ign/src/path_tracer.yml index 110baae3..252c9bd1 100644 --- a/subt_ign/src/path_tracer.yml +++ b/subt_ign/src/path_tracer.yml @@ -1,81 +1,146 @@ -incorrect_report_color: +rtf: 1.0 +incorrect_report_color: # medium grey ambient: - r: 1.0 - g: 0.0 - b: 0.0 + r: 0.490 + g: 0.431 + b: 0.373 a: 0.5 diffuse: - r: 1.0 - g: 0.0 - b: 0.0 + r: 0.490 + g: 0.431 + b: 0.373 a: 0.5 emissive: - r: 0.2 - g: 0.0 - b: 0.0 + r: 0.490 + g: 0.431 + b: 0.373 a: 0.1 -correct_report_color: +correct_report_color: # light green ambient: - r: 0.0 - g: 1.0 - b: 0.0 + r: 0.596 + g: 0.722 + b: 0.416 a: 1.0 diffuse: - r: 0.0 - g: 1.0 - b: 0.0 + r: 0.596 + g: 0.722 + b: 0.416 a: 1.0 emissive: - r: 0.0 - g: 1.0 - b: 0.0 + r: 0.596 + g: 0.722 + b: 0.416 a: 1.0 -artficat_location_color: +artifact_location_color: # medium blue ambient: - r: 0.0 - g: 1.0 - b: 1.0 - a: 0.5 + r: 0.059 + g: 0.369 + b: 0.565 + a: 1.0 diffuse: - r: 0.0 - g: 1.0 - b: 1.0 - a: 0.5 + r: 0.059 + g: 0.369 + b: 0.565 + a: 1.0 emissive: - r: 0.0 - g: 1.0 - b: 1.0 - a: 0.5 -robot_colors: - 1: + r: 0.059 + g: 0.369 + b: 0.565 + a: 1.0 +robot_colors: + 0: # medium orange ambient: - r: 0.0 - g: 1.0 - b: 1.0 + r: 1.0 + g: 0.275 + b: 0.0 a: 0.5 diffuse: - r: 0.0 - g: 1.0 - b: 1.0 + r: 1.0 + g: 0.275 + b: 0.0 a: 0.5 emissive: - r: 0.0 - g: 1.0 - b: 1.0 + r: 1.0 + g: 0.275 + b: 0.0 + a: 0.5 + 1: # light orange + ambient: + r: 0.898 + g: 0.451 + b: 0.0 + a: 0.5 + diffuse: + r: 0.898 + g: 0.451 + b: 0.0 + a: 0.5 + emissive: + r: 0.898 + g: 0.451 + b: 0.0 a: 0.5 2: ambient: - r: 0.0 - g: 1.0 - b: 1.0 + r: 0.5 + g: 0.138 + b: 0.0 + a: 0.5 + diffuse: + r: 0.5 + g: 0.138 + b: 0.0 + a: 0.5 + emissive: + r: 0.5 + g: 0.138 + b: 0.0 + a: 0.5 + 3: + ambient: + r: 0.449 + g: 0.226 + b: 0.0 + a: 0.5 + diffuse: + r: 0.449 + g: 0.226 + b: 0.0 + a: 0.5 + emissive: + r: 0.449 + g: 0.226 + b: 0.0 + a: 0.5 + 4: + ambient: + r: 0.75 + g: 0.206 + b: 0.0 + a: 0.5 + diffuse: + r: 0.75 + g: 0.206 + b: 0.0 + a: 0.5 + emissive: + r: 0.75 + g: 0.206 + b: 0.0 + a: 0.5 + 5: + ambient: + r: 0.674 + g: 0.338 + b: 0.0 a: 0.5 diffuse: - r: 0.0 - g: 1.0 - b: 1.0 + r: 0.674 + g: 0.338 + b: 0.0 a: 0.5 emissive: - r: 0.0 - g: 1.0 - b: 1.0 + r: 0.674 + g: 0.338 + b: 0.0 a: 0.5