WIP: Message logging

.gitignore

@@ -36,3 +36,4 @@ tags*
 **.swp
 **.DS_Store
 **.ccls*
+**compile_commands.json

CMakeLists.txt

@@ -196,7 +196,7 @@ if (${VTK_FOUND})
         set(OPENGL_MODULE vtkRenderingOpenGL2)
     endif()
 
-    set(VTK_MODULES 
+    set(VTK_MODULES
         vtkFiltersSources
         vtkFiltersTexture
         vtkInteractionStyle
@@ -208,7 +208,7 @@ if (${VTK_FOUND})
         ${OPENGL_MODULE})
 
     find_package(VTK QUIET NO_MODULE COMPONENTS ${VTK_MODULES})
-endif() 
+endif()
 
 if (NOT EXTERNAL AND ${VTK_FOUND})
   message(STATUS "Found VTK Version: ${VTK_MAJOR_VERSION}.${VTK_MINOR_VERSION}.${VTK_BUILD_VERSION}")
@@ -248,7 +248,7 @@ endif()
 ########################################################
 #set(Boost_DEBUG ON)
 find_package(Boost 1.55 COMPONENTS thread date_time iostreams program_options regex
-  filesystem system graph REQUIRED)
+  filesystem system graph serialization REQUIRED)
 add_library(scrimmage-boost INTERFACE)
 target_include_directories(scrimmage-boost INTERFACE ${Boost_INCLUDE_DIRS})
 target_link_libraries(scrimmage-boost INTERFACE ${Boost_LIBRARIES})

include/scrimmage/plugins/autonomy/PubSub/PubSub.h

@@ -0,0 +1,59 @@
+/*!
+ * @file
+ *
+ * @section LICENSE
+ *
+ * Copyright (C) 2017 by the Georgia Tech Research Institute (GTRI)
+ *
+ * This file is part of SCRIMMAGE.
+ *
+ *   SCRIMMAGE is free software: you can redistribute it and/or modify it under
+ *   the terms of the GNU Lesser General Public License as published by the
+ *   Free Software Foundation, either version 3 of the License, or (at your
+ *   option) any later version.
+ *
+ *   SCRIMMAGE is distributed in the hope that it will be useful, but WITHOUT
+ *   ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ *   FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
+ *   License for more details.
+ *
+ *   You should have received a copy of the GNU Lesser General Public License
+ *   along with SCRIMMAGE.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Christopher Richardson <christopher.richardson@gtri.gatech.edu>
+ * @date 31 July 2017
+ * @version 0.1.0
+ * @brief Brief file description.
+ * @section DESCRIPTION
+ * A Long description goes here.
+ *
+ */
+
+#ifndef INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_PUBSUB_PUBSUB_H_
+#define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_PUBSUB_PUBSUB_H_
+#include <scrimmage/autonomy/Autonomy.h>
+#include <scrimmage/pubsub/Message.h>
+#include <scrimmage/pubsub/Publisher.h>
+#include <scrimmage/pubsub/Serialization.h>
+
+#include <string>
+#include <map>
+#include <vector>
+#include <fstream>
+
+namespace scrimmage {
+namespace autonomy {
+class PubSub : public scrimmage::Autonomy {
+ public:
+    void init(std::map<std::string, std::string> &params) override;
+    bool step_autonomy(double t, double dt) override;
+
+ protected:
+    scrimmage::PublisherPtr pub_logged_;
+    void callback_logged(scrimmage::MessagePtr<std::vector<scrimmage::State>> msg);
+
+    std::ofstream ofs_;
+};
+} // namespace autonomy
+} // namespace scrimmage
+#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_PUBSUB_PUBSUB_H_

include/scrimmage/plugins/autonomy/PubSub/PubSub.xml

@@ -0,0 +1,5 @@
+<?xml version="1.0"?>
+<?xml-stylesheet type="text/xsl" href="http://gtri.gatech.edu"?>
+<params>
+  <library>PubSub_plugin</library>
+</params>

include/scrimmage/pubsub/Serialization.h

@@ -0,0 +1,76 @@
+/*!
+ * @file
+ *
+ * @section LICENSE
+ *
+ * Copyright (C) 2017 by the Georgia Tech Research Institute (GTRI)
+ *
+ * This file is part of SCRIMMAGE.
+ *
+ *   SCRIMMAGE is free software: you can redistribute it and/or modify it under
+ *   the terms of the GNU Lesser General Public License as published by the
+ *   Free Software Foundation, either version 3 of the License, or (at your
+ *   option) any later version.
+ *
+ *   SCRIMMAGE is distributed in the hope that it will be useful, but WITHOUT
+ *   ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ *   FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
+ *   License for more details.
+ *
+ *   You should have received a copy of the GNU Lesser General Public License
+ *   along with SCRIMMAGE.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Christopher Richardson <christopher.richardson@gtri.gatech.edu>
+ * @date 31 July 2017
+ * @version 0.1.0
+ * @brief Brief file description.
+ * @section DESCRIPTION
+ * A Long description goes here.
+ *
+ */
+
+#ifndef INCLUDE_SCRIMMAGE_PUBSUB_SERIALIZATION_H_
+#define INCLUDE_SCRIMMAGE_PUBSUB_SERIALIZATION_H_
+
+#include <scrimmage/math/State.h>
+#include <scrimmage/math/Quaternion.h>
+
+#include <string>
+#include <memory>
+
+#include <boost/archive/xml_oarchive.hpp>
+#include <boost/archive/xml_iarchive.hpp>
+
+namespace boost {
+namespace serialization {
+
+template<class Archive>
+void serialize(Archive & ar, Eigen::Vector3d & vec, const unsigned int version)
+{
+    ar & boost::serialization::make_nvp("x", vec[0]);
+    ar & boost::serialization::make_nvp("y", vec[1]);
+    ar & boost::serialization::make_nvp("z", vec[2]);
+}
+
+template<class Archive>
+void serialize(Archive & ar, scrimmage::Quaternion & quat, const unsigned int version)
+{
+    ar & boost::serialization::make_nvp("w", quat.w());
+    ar & boost::serialization::make_nvp("x", quat.vec()[0]);
+    ar & boost::serialization::make_nvp("y", quat.vec()[1]);
+    ar & boost::serialization::make_nvp("z", quat.vec()[2]);
+}
+
+template<class Archive>
+void serialize(Archive & ar, scrimmage::State & state, const unsigned int version)
+{
+    ar & boost::serialization::make_nvp("pos", state.pos());
+    ar & boost::serialization::make_nvp("vel", state.vel());
+    ar & boost::serialization::make_nvp("quat", state.quat());
+    ar & boost::serialization::make_nvp("ang_vel", state.ang_vel());
+}
+
+} // namespace serialization
+} // namespace boost
+
+#endif // INCLUDE_SCRIMMAGE_PUBSUB_SERIALIZATION_H_

missions/test/test_pubsub.xml

@@ -0,0 +1,88 @@
+<?xml version="1.0"?>
+<?xml-stylesheet type="text/xsl" href="http://gtri.gatech.edu"?>
+<runscript xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+           name="Straight flying">
+
+  <run start="0.0" end="1000" dt="0.1"
+       time_warp="10"
+       enable_gui="true"
+       network_gui="false"
+       start_paused="true"/>
+
+  <!-- <multi_threaded num_threads="8">false</multi_threaded> -->
+  <stream_port>50051</stream_port>
+  <stream_ip>localhost</stream_ip>
+
+  <end_condition>time</end_condition> <!-- time, one_team, none-->
+
+  <grid_spacing>10</grid_spacing>
+  <grid_size>1000</grid_size>
+
+  <terrain>mcmillan</terrain>
+  <background_color>191 191 191</background_color> <!-- Red Green Blue -->
+  <gui_update_period>10</gui_update_period> <!-- milliseconds -->
+
+  <plot_tracks>false</plot_tracks>
+  <output_type>all</output_type>
+  <show_plugins>false</show_plugins>
+
+  <metrics>SimpleCollisionMetrics</metrics>
+
+  <log_dir>~/.scrimmage/logs</log_dir>
+  <create_latest_dir>true</create_latest_dir>
+
+  <latitude_origin>35.721025</latitude_origin>
+  <longitude_origin>-120.767925</longitude_origin>
+  <altitude_origin>300</altitude_origin>
+  <show_origin>true</show_origin>
+  <origin_length>10</origin_length>
+
+  <enable_screenshots min_period="1.0" start="8.3" end="15.3">false</enable_screenshots>
+
+  <network>GlobalNetwork</network>
+  <network>LocalNetwork</network>
+
+  <!-- uncomment "seed" and use integer for deterministic results -->
+  <seed>2147483648</seed>
+
+  <entity>
+    <name>publisher</name>
+    <team_id>1</team_id>
+    <color>77 77 255</color>
+    <!-- <count>${count=1}</count> -->
+    <count>1</count>
+    <health>1</health>
+    <radius>1</radius>
+
+    <x>0</x>
+    <y>0</y>
+    <z>100</z>
+    <heading>0</heading>
+
+    <autonomy>PubSub</autonomy>
+    <autonomy>Straight</autonomy>
+    <controller>SimpleAircraftControllerPID</controller>
+    <motion_model>SimpleAircraft</motion_model>
+    <visual_model>zephyr-blue</visual_model>
+  </entity>
+
+  <entity>
+    <name>subscriber</name>
+    <team_id>1</team_id>
+    <color>77 77 255</color>
+    <count>${count=1}</count>
+    <health>1</health>
+    <radius>1</radius>
+
+    <x>0</x>
+    <y>0</y>
+    <z>200</z>
+    <heading>0</heading>
+
+    <autonomy>Straight</autonomy>
+    <controller>SimpleAircraftControllerPID</controller>
+    <motion_model>SimpleAircraft</motion_model>
+    <visual_model>zephyr-red</visual_model>
+  </entity>
+
+</runscript>

src/plugins/autonomy/PubSub/CMakeLists.txt

@@ -0,0 +1,49 @@
+#--------------------------------------------------------
+# Library Creation
+#--------------------------------------------------------
+set(LIBRARY_NAME PubSub_plugin)
+set(LIB_MAJOR 0)
+set(LIB_MINOR 0)
+set(LIB_RELEASE 1)
+
+file(GLOB SRCS *.cpp)
+
+add_library(${LIBRARY_NAME} SHARED
+  ${SRCS}
+)
+
+target_compile_options(${LIBRARY_NAME}
+  PRIVATE -Wall -Wno-return-type-c-linkage -std=c++14
+)
+
+target_link_libraries(${LIBRARY_NAME}
+  PUBLIC
+      scrimmage-core
+  PRIVATE
+      Boost::boost
+      Boost::serialization
+)
+
+set(_soversion ${LIB_MAJOR}.${LIB_MINOR}.${LIB_RELEASE})
+
+set_target_properties(${LIBRARY_NAME} PROPERTIES
+  SOVERSION ${LIB_MAJOR}
+  VERSION ${_soversion}
+  LIBRARY_OUTPUT_DIRECTORY ${PROJECT_PLUGIN_LIBS_DIR}
+)
+
+target_include_directories(${LIBRARY_NAME}
+  PUBLIC
+    $<INSTALL_INTERFACE:include>
+    $<BUILD_INTERFACE:${PROJECT_INC_DIR}>
+)
+
+install(TARGETS ${LIBRARY_NAME}
+  # IMPORTANT: Add the library to the "export-set"
+  EXPORT ${PROJECT_NAME}-targets
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION lib/${PROJECT_NAME}/plugin_libs
+)
+
+# Push up the PROJECT_PLUGINS variable
+set(PROJECT_PLUGINS ${PROJECT_PLUGINS} ${LIBRARY_NAME} PARENT_SCOPE)