diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml new file mode 100644 index 0000000..36709f7 --- /dev/null +++ b/.github/workflows/build.yaml @@ -0,0 +1,30 @@ +name: Build ROS package + +on: + push: + branches: [ main ] + +jobs: + build-package: + runs-on: ubuntu-20.04 + + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: noetic + - run: | + sudo add-apt-repository ppa:inivation-ppa/inivation + sudo apt update + sudo apt install libcaer-dev + git clone https://github.com/raultapia/openev + mkdir -p openev/build + cd openev/build + cmake .. + make + sudo make install + - uses: ros-tooling/action-ros-ci@v0.3 + with: + package-name: asap + target-ros1-distro: noetic + vcs-repo-file-url: https://raw.githubusercontent.com/raultapia/asap/main/dependencies.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index 5aa06d8..8c383a6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,10 +13,10 @@ add_service_files(FILES Configuration.srv) add_service_files(FILES Roi.srv) generate_messages(DEPENDENCIES sensor_msgs std_msgs) generate_dynamic_reconfigure_options(config/parameters.cfg) -catkin_package(CATKIN_DEPENDS message_runtime sensor_msgs std_msgs) +catkin_package(CATKIN_DEPENDS dynamic_reconfigure message_runtime sensor_msgs std_msgs) add_executable(asap src/asap_node.cpp src/configuration.cpp src/main_thread.cpp src/publishers.cpp src/services.cpp) -add_dependencies(asap asap_gencfg) +add_dependencies(asap ${asap_EXPORTED_TARGETS}) target_link_libraries(asap ${catkin_LIBRARIES}) target_link_libraries(asap ${OpenCV_LIBRARIES}) target_link_libraries(asap openev) diff --git a/config/parameters.cfg b/config/parameters.cfg index f849525..729dfe7 100644 --- a/config/parameters.cfg +++ b/config/parameters.cfg @@ -7,6 +7,9 @@ from dynamic_reconfigure.parameter_generator_catkin import double_t from dynamic_reconfigure.parameter_generator_catkin import int_t from dynamic_reconfigure.parameter_generator_catkin import str_t +PACKAGE_NAME = "asap" +NODE_NAME = "asap" +PREFIX = "parameters" INIT_YAML = os.path.realpath(__file__).replace('parameters.cfg', 'asap.yaml') try: @@ -35,4 +38,4 @@ else: dvs.add("dvs_size", int_t, 0, "help", data['dvs']['size'], 1, 10000) imu.add("imu_enabled", bool_t, 0, "help", data['imu']['enabled']) - exit(gen.generate("asap", "asap", "asap")) + exit(gen.generate(PACKAGE_NAME, NODE_NAME, PREFIX)) diff --git a/dependencies.yaml b/dependencies.yaml new file mode 100644 index 0000000..a523f08 --- /dev/null +++ b/dependencies.yaml @@ -0,0 +1,5 @@ +repositories: + rpg_dvs_ros: + type: git + url: https://github.com/uzh-rpg/rpg_dvs_ros + version: master diff --git a/include/asap/asap.hpp b/include/asap/asap.hpp index fc0dbaf..8ecbe9d 100644 --- a/include/asap/asap.hpp +++ b/include/asap/asap.hpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include #include #include @@ -28,6 +28,7 @@ constexpr size_t EVENT_RATE_QUEUE_MAX_SIZE = 500; constexpr size_t DVS_BUFFER_WARNING_SIZE = 1e8; constexpr size_t APS_BUFFER_WARNING_SIZE = 50; constexpr size_t IMU_BUFFER_WARNING_SIZE = 300; +constexpr int THREAD_SLEEP_TIME_NSEC = 10; constexpr int APS_DEFAULT_EXPOSURE = 6500; constexpr double APS_DEFAULT_RATE = 50; @@ -36,34 +37,29 @@ constexpr double DVS_DEFAULT_RATE = 50; constexpr int DVS_DEFAULT_SIZE = 1000; constexpr Mode DVS_DEFAULT_MODE = Mode::SIZE; -constexpr int DVS_CONFIGURATION_ALIGNED = 32; -constexpr int APS_CONFIGURATION_ALIGNED = 16; -constexpr int CONFIGURATION_ALIGNED = 128; -constexpr int THREAD_SLEEP_TIME_NSEC = 10; - -using DvsConfiguration = struct { +struct DvsConfiguration { bool enabled{true}; Mode mode{DVS_DEFAULT_MODE}; int size{DVS_DEFAULT_SIZE}; double rate{DVS_DEFAULT_RATE}; double gamma{DVS_DEFAULT_GAMMA}; -} __attribute__((aligned(DVS_CONFIGURATION_ALIGNED))); +} __attribute__((aligned(32))); -using ApsConfiguration = struct { +struct ApsConfiguration { bool enabled{true}; double rate{APS_DEFAULT_RATE}; int exposure{APS_DEFAULT_EXPOSURE}; -} __attribute__((aligned(APS_CONFIGURATION_ALIGNED))); +} __attribute__((aligned(16))); -using ImuConfiguration = struct { +struct ImuConfiguration { bool enabled{true}; }; -using Configuration = struct { +struct Configuration { DvsConfiguration dvs; ApsConfiguration aps; ImuConfiguration imu; -} __attribute__((aligned(CONFIGURATION_ALIGNED))); +} __attribute__((aligned(128))); class Asap { public: @@ -87,7 +83,7 @@ class Asap { std::thread dvsThread_, apsThread_, imuThread_, cameraInfoThread_; ros::Publisher dvsPub_{}, apsPub_{}, imuPub_{}, cameraInfoPub_{}; ros::ServiceServer feedbackSrv_{}, configurationSrv_{}, roiSrv_{}; - dynamic_reconfigure::Server server_{}; + dynamic_reconfigure::Server server_{}; std::array autoModeRecall_{0, 0, 0}; std::queue timeQueue_{}; double gammaMax_{}; @@ -111,7 +107,7 @@ class Asap { bool feedbackCallback(asap::Feedback::Request &, asap::Feedback::Response &); bool setConfigurationCallback(asap::Configuration::Request &, asap::Configuration::Response &); bool setRoiCallback(asap::Roi::Request &, asap::Roi::Response &); - void dynamicReconfigureCallback(asap::asapConfig &config, uint32_t level); + void dynamicReconfigureCallback(asap::parametersConfig &config, uint32_t level); }; inline double saturation(const double x, const double xmax, const double xmin) { diff --git a/package.xml b/package.xml index 690ab76..89342bd 100644 --- a/package.xml +++ b/package.xml @@ -7,6 +7,7 @@ GPLv3 catkin dvs_msgs + dynamic_reconfigure roscpp sensor_msgs std_msgs diff --git a/src/services.cpp b/src/services.cpp index 38add06..15c6efa 100644 --- a/src/services.cpp +++ b/src/services.cpp @@ -62,7 +62,7 @@ bool Asap::setRoiCallback(asap::Roi::Request &req, asap::Roi::Response &res) { return res.success; } -void Asap::dynamicReconfigureCallback(asap::asapConfig &config, uint32_t level) { +void Asap::dynamicReconfigureCallback(asap::parametersConfig &config, uint32_t level) { if(static_cast(level) < 0) { return; }