diff --git a/RWR/src/read_event_pub/.vscode/c_cpp_properties.json b/RWR/src/read_event_pub/.vscode/c_cpp_properties.json new file mode 100644 index 0000000000000000000000000000000000000000..174f9f869e988ab28d5f5dac9d23bfee59ba9ee6 --- /dev/null +++ b/RWR/src/read_event_pub/.vscode/c_cpp_properties.json @@ -0,0 +1,49 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "", + "limitSymbolsToIncludedHeaders": true + }, + "includePath": [ + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/devel/include/**", + "/opt/ros/noetic/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_dvs_ros/davis_ros_driver/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_dvs_ros/dvs_calibration/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_dvs_ros/dvs_renderer/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_dvs_ros/dvs_ros_driver/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_dvs_ros/dvxplorer_ros_driver/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/eigen_checks/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_esim/event_camera_simulator/esim/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_esim/event_camera_simulator/esim_common/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_esim/event_camera_simulator/esim_rendering/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_esim/event_camera_simulator/esim_trajectory/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/ze_oss/imp_bridge_opencv/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/ze_oss/imp_bridge_ros/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/ze_oss/imp_core/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/minkindr/minkindr/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/minkindr_ros/minkindr_conversions/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/spot_micro_motion_cmd/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/ze_oss/ze_cameras/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/ze_oss/ze_common/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/ze_oss/ze_matplotlib/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/ze_oss/ze_pangolin/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/ze_oss/ze_ros/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/ze_oss/ze_splines/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/ze_oss/ze_vi_simulation/include/**", + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/src/ze_oss/ze_visualization/include/**", + "/usr/include/**" + ], + "name": "ROS" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/RWR/src/read_event_pub/.vscode/settings.json b/RWR/src/read_event_pub/.vscode/settings.json new file mode 100644 index 0000000000000000000000000000000000000000..3da6490dbf799af539196a6b4c338c68a9475450 --- /dev/null +++ b/RWR/src/read_event_pub/.vscode/settings.json @@ -0,0 +1,10 @@ +{ + "python.autoComplete.extraPaths": [ + "/home/chris/3rdYear/RWR/Robot-With-Reflexes/RWR/devel/lib/python3/dist-packages", + "/opt/ros/noetic/lib/python3/dist-packages" + ], + "files.associations": { + "*.ipp": "cpp" + }, + "C_Cpp.errorSquiggles": "Disabled" +} \ No newline at end of file diff --git a/RWR/src/read_event_pub/CMakeLists.txt b/RWR/src/read_event_pub/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..dabb2c5762e84858500ba73e103b74d5228ef10e --- /dev/null +++ b/RWR/src/read_event_pub/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.0.2) +project(read_event_pub) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + dvs_msgs + roscpp + rospy + std_msgs +) + +catkin_package() + +include_directories( + ${PROJECT_SOURCE_DIR}/include + ${catkin_INCLUDE_DIRS} +) + +add_executable(event_array_sub src/event_array_sub.cpp) + +add_dependencies(event_array_sub + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) \ No newline at end of file diff --git a/RWR/src/read_event_pub/include/event_array_sub.hpp b/RWR/src/read_event_pub/include/event_array_sub.hpp new file mode 100644 index 0000000000000000000000000000000000000000..a1caec74a59efcdd779245112ee17b873fd1930e --- /dev/null +++ b/RWR/src/read_event_pub/include/event_array_sub.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include <ros/ros.h> +#include <esim/common/utils.hpp> +#include <dvs_msgs/Event.h> +#include <dvs_msgs/EventArray.h> + +#define EVENT_ARRAY_PUBLISHERS 3 +#define EVENT_ARRAY_SUBSCRIBERS 1 + +class EventArraySub +{ + public: + EventArraySub(ros::NodeHandle & nh); + ~EventArraySub(); + + struct EventDetection { + ros::Publisher events_[2]; + double xy_means_[1]; + size_t msgSz; + }; + + + + private: + ros::NodeHandle nh_; + ros::Subscriber event_sub_; + + EventDetection event_detection_stats_; + + void publishEventDetection(); + void eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg); + +}; \ No newline at end of file diff --git a/RWR/src/read_event_pub/package.xml b/RWR/src/read_event_pub/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..6abe9d05c97bddfe95d3256e15aa5b78fcd8cb00 --- /dev/null +++ b/RWR/src/read_event_pub/package.xml @@ -0,0 +1,71 @@ +<?xml version="1.0"?> +<package format="2"> + <name>read_event_pub</name> + <version>0.0.0</version> + <description>The read_event_pub package</description> + + <!-- One maintainer tag required, multiple allowed, one person per tag --> + <!-- Example: --> + <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> + <maintainer email="chris@todo.todo">chris</maintainer> + + + <!-- One license tag required, multiple allowed, one license per tag --> + <!-- Commonly used license strings: --> + <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 --> + <license>TODO</license> + + + <!-- Url tags are optional, but multiple are allowed, one per tag --> + <!-- Optional attribute type can be: website, bugtracker, or repository --> + <!-- Example: --> + <!-- <url type="website">http://wiki.ros.org/read_event_pub</url> --> + + + <!-- Author tags are optional, multiple are allowed, one per tag --> + <!-- Authors do not have to be maintainers, but could be --> + <!-- Example: --> + <!-- <author email="jane.doe@example.com">Jane Doe</author> --> + + + <!-- The *depend tags are used to specify dependencies --> + <!-- Dependencies can be catkin packages or system dependencies --> + <!-- Examples: --> + <!-- Use depend as a shortcut for packages that are both build and exec dependencies --> + <!-- <depend>roscpp</depend> --> + <!-- Note that this is equivalent to the following: --> + <!-- <build_depend>roscpp</build_depend> --> + <!-- <exec_depend>roscpp</exec_depend> --> + <!-- Use build_depend for packages you need at compile time: --> + <!-- <build_depend>message_generation</build_depend> --> + <!-- Use build_export_depend for packages you need in order to build against this package: --> + <!-- <build_export_depend>message_generation</build_export_depend> --> + <!-- Use buildtool_depend for build tool packages: --> + <!-- <buildtool_depend>catkin</buildtool_depend> --> + <!-- Use exec_depend for packages you need at runtime: --> + <!-- <exec_depend>message_runtime</exec_depend> --> + <!-- Use test_depend for packages you need only for testing: --> + <!-- <test_depend>gtest</test_depend> --> + <!-- Use doc_depend for packages you need only for building documentation: --> + <!-- <doc_depend>doxygen</doc_depend> --> + <buildtool_depend>catkin</buildtool_depend> + <build_depend>dvs_msgs</build_depend> + <build_depend>roscpp</build_depend> + <build_depend>rospy</build_depend> + <build_depend>std_msgs</build_depend> + <build_export_depend>dvs_msgs</build_export_depend> + <build_export_depend>roscpp</build_export_depend> + <build_export_depend>rospy</build_export_depend> + <build_export_depend>std_msgs</build_export_depend> + <exec_depend>dvs_msgs</exec_depend> + <exec_depend>roscpp</exec_depend> + <exec_depend>rospy</exec_depend> + <exec_depend>std_msgs</exec_depend> + + + <!-- The export tag contains other, unspecified, tags --> + <export> + <!-- Other tools can request additional information be placed here --> + + </export> +</package> diff --git a/RWR/src/read_event_pub/src/event_array_sub.cpp b/RWR/src/read_event_pub/src/event_array_sub.cpp new file mode 100644 index 0000000000000000000000000000000000000000..83592c02b71ae789d4cd9d197fc96f3899a90a21 --- /dev/null +++ b/RWR/src/read_event_pub/src/event_array_sub.cpp @@ -0,0 +1,56 @@ +#include <ros/ros.h> +#include <std_msgs/Float32.h> +#include <std_msgs/UInt32.h> +#include "event_array_sub.hpp" + +namespace event_array_sub { +EventArraySub::EventArraySub(ros::NodeHandle & nh) : nh_(nh) +{ + event_detection_stats_.events_[0] = nh_.advertise<std_msgs::Float32>("event_avg_x", 1); + event_detection_stats_.events_[1] = nh_.advertise<std_msgs::Float32>("event_avg_y", 1); + event_detection_stats_.events_[2] = nh_.advertise<std_msgs::UInt32>("event_msg_sz", 1); + + event_sub_ = nh_.subscribe("events", 1, &EventArraySub::eventsCallback, this); +} + +EventArraySub::~EventArraySub() +{} + +void eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg) +{ + EventArraySub::EventDetection event_detection_stats_; + + size_t msgSz = msg->events.size(); + + ROS_INFO_STREAM("Number of particles="<<msgSz); + + double avg_x = 0; + double avg_y = 0; + for (int i(0); i < msgSz; ++i) + { + ROS_INFO_STREAM("X=" << msg->events[i].x); + ROS_INFO_STREAM("Y=" << msg->events[i].y;); + ROS_INFO_STREAM("pol=" << msg->events[i].polarity); + + avg_x = avg_x + msg->events[i].x; + avg_y = avg_y + msg->events[i].y; + } + + avg_x = avg_x / msgSz; + avg_y = avg_y / msgSz; + + event_detection_stats_.xy_means_[0] = avg_x; + event_detection_stats_.xy_means_[1] = avg_y; + event_detection_stats_.msgSz = msgSz; + + publishEventDetection(); //not sure why this fails +} + +void EventArraySub::publishEventDetection() +{ + event_detection_stats_.events_[0].publish(event_detection_stats_.xy_means_[0]); + event_detection_stats_.events_[1].publish(event_detection_stats_.xy_means_[1]); + event_detection_stats_.events_[2].publish(event_detection_stats_.msgSz); +} + +} //namespace \ No newline at end of file