diff --git a/.gitignore b/.gitignore index 376edffeb9..39d3ba8f9f 100644 --- a/.gitignore +++ b/.gitignore @@ -36,3 +36,4 @@ tags* **.swp **.DS_Store **.ccls* +**compile_commands.json diff --git a/CMakeLists.txt b/CMakeLists.txt index 1b7c5bf277..5764945dca 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/include/scrimmage/plugins/autonomy/PubSub/PubSub.h b/include/scrimmage/plugins/autonomy/PubSub/PubSub.h new file mode 100644 index 0000000000..218f0b5db5 --- /dev/null +++ b/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 . + * + * @author Christopher Richardson + * @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 +#include +#include +#include + +#include +#include +#include +#include + +namespace scrimmage { +namespace autonomy { +class PubSub : public scrimmage::Autonomy { + public: + void init(std::map ¶ms) override; + bool step_autonomy(double t, double dt) override; + + protected: + scrimmage::PublisherPtr pub_logged_; + void callback_logged(scrimmage::MessagePtr> msg); + + std::ofstream ofs_; +}; +} // namespace autonomy +} // namespace scrimmage +#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_PUBSUB_PUBSUB_H_ diff --git a/include/scrimmage/plugins/autonomy/PubSub/PubSub.xml b/include/scrimmage/plugins/autonomy/PubSub/PubSub.xml new file mode 100644 index 0000000000..8960bf5557 --- /dev/null +++ b/include/scrimmage/plugins/autonomy/PubSub/PubSub.xml @@ -0,0 +1,5 @@ + + + + PubSub_plugin + diff --git a/include/scrimmage/pubsub/Serialization.h b/include/scrimmage/pubsub/Serialization.h new file mode 100644 index 0000000000..c6a5df138f --- /dev/null +++ b/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 . + * + * @author Christopher Richardson + * @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 +#include + +#include +#include + +#include +#include + +namespace boost { +namespace serialization { + +template +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 +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 +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_ diff --git a/missions/test/test_pubsub.xml b/missions/test/test_pubsub.xml new file mode 100644 index 0000000000..a06972e541 --- /dev/null +++ b/missions/test/test_pubsub.xml @@ -0,0 +1,88 @@ + + + + + + + + 50051 + localhost + + time + + 10 + 1000 + + mcmillan + 191 191 191 + 10 + + false + all + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + true + + 35.721025 + -120.767925 + 300 + true + 10 + + false + + GlobalNetwork + LocalNetwork + + + 2147483648 + + + publisher + 1 + 77 77 255 + + 1 + 1 + 1 + + 0 + 0 + 100 + 0 + + PubSub + Straight + SimpleAircraftControllerPID + SimpleAircraft + zephyr-blue + + + + subscriber + 1 + 77 77 255 + ${count=1} + 1 + 1 + + 0 + 0 + 200 + 0 + + Straight + SimpleAircraftControllerPID + SimpleAircraft + zephyr-red + + + diff --git a/src/plugins/autonomy/PubSub/CMakeLists.txt b/src/plugins/autonomy/PubSub/CMakeLists.txt new file mode 100644 index 0000000000..5254ecc862 --- /dev/null +++ b/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(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) diff --git a/src/plugins/autonomy/PubSub/PubSub.cpp b/src/plugins/autonomy/PubSub/PubSub.cpp new file mode 100644 index 0000000000..5128988471 --- /dev/null +++ b/src/plugins/autonomy/PubSub/PubSub.cpp @@ -0,0 +1,94 @@ +/*! + * @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 . + * + * @author Christopher Richardson + * @date 31 July 2017 + * @version 0.1.0 + * @brief Brief file description. + * @section DESCRIPTION + * A Long description goes here. + * + */ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +using std::cout; +using std::endl; + +namespace sc = scrimmage; + +REGISTER_PLUGIN(scrimmage::Autonomy, + scrimmage::autonomy::PubSub, + PubSub_plugin) + +namespace scrimmage { +namespace autonomy { + +void PubSub::init(std::map ¶ms) { + pub_logged_ = advertise("GlobalNetwork", "LoggedTopic", 1); + + // sub + subscribe>( + "GlobalNetwork", "LoggedTopic", + std::bind(&PubSub::callback_logged, this, std::placeholders::_1)); + + ofs_.open("test_boost_serial.xml"); +} + +bool PubSub::step_autonomy(double t, double dt) { + + // pub + std::vector vec; + scrimmage::State st = *state_; + for (int i=0; i<3; ++i) { + Eigen::Vector3d curpos = st.pos(); + st.pos() = curpos + Eigen::Vector3d(100, 0, 0); + vec.push_back(st); + } + auto msg = std::make_shared>>(); + msg->data = vec; + pub_logged_->publish(msg); + + return true; +} + +void PubSub::callback_logged(scrimmage::MessagePtr> msg) { + std::cout << "PubSub: received state vector. Serializing to xml..." << std::endl; + boost::archive::xml_oarchive oa(ofs_); + oa << boost::serialization::make_nvp("state_vec", msg->data); +} +} // namespace autonomy +} // namespace scrimmage