diff --git a/RWR/src/read_event_pub/CMakeLists.txt b/RWR/src/read_event_pub/CMakeLists.txt index dabb2c5762e84858500ba73e103b74d5228ef10e..77ec53025751763bcb77436d59bc49a6ae2069d1 100644 --- a/RWR/src/read_event_pub/CMakeLists.txt +++ b/RWR/src/read_event_pub/CMakeLists.txt @@ -8,10 +8,10 @@ add_compile_options(-std=c++11) ## 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 + dvs_msgs ) catkin_package() @@ -22,8 +22,21 @@ include_directories( ) add_executable(event_array_sub src/event_array_sub.cpp) +add_executable(event_array_pub_node src/read_event_pub_node.cpp) add_dependencies(event_array_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} +) +add_dependencies(event_array_pub_node + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +target_link_libraries(event_array_sub + ${catkin_LIBRARIES} +) + +target_link_libraries(event_array_pub_node + ${catkin_LIBRARIES} ) \ 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.h similarity index 88% rename from RWR/src/read_event_pub/include/event_array_sub.hpp rename to RWR/src/read_event_pub/include/event_array_sub.h index a1caec74a59efcdd779245112ee17b873fd1930e..b29355c340920a62d5b843dea94896970c0a18b2 100644 --- a/RWR/src/read_event_pub/include/event_array_sub.hpp +++ b/RWR/src/read_event_pub/include/event_array_sub.h @@ -1,12 +1,12 @@ #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 +namespace event_array_sub { class EventArraySub { @@ -19,16 +19,17 @@ class EventArraySub double xy_means_[1]; size_t msgSz; }; - - + //void publishEventDetection(); private: ros::NodeHandle nh_; ros::Subscriber event_sub_; EventDetection event_detection_stats_; - - void publishEventDetection(); + void eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg); + void publishEventDetection(); -}; \ No newline at end of file +}; + +} //namespace \ No newline at end of file diff --git a/RWR/src/read_event_pub/src/event_array_sub.cpp b/RWR/src/read_event_pub/src/event_array_sub.cpp index 83592c02b71ae789d4cd9d197fc96f3899a90a21..e8c92f3731a3b085200ec348c188e11fd827ff9d 100644 --- a/RWR/src/read_event_pub/src/event_array_sub.cpp +++ b/RWR/src/read_event_pub/src/event_array_sub.cpp @@ -1,9 +1,10 @@ #include <ros/ros.h> #include <std_msgs/Float32.h> #include <std_msgs/UInt32.h> -#include "event_array_sub.hpp" +#include "event_array_sub.h" 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); @@ -28,12 +29,12 @@ void eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg) 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; + 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; @@ -46,11 +47,15 @@ void eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg) publishEventDetection(); //not sure why this fails } -void EventArraySub::publishEventDetection() +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); + std_msgs::Float32 msg; + msg.data = event_detection_stats_.xy_means_[0]; + event_detection_stats_.events_[0].publish(msg); + msg.data = event_detection_stats_.xy_means_[1]; + event_detection_stats_.events_[1].publish(msg); + msg.data = event_detection_stats_.msgSz; + event_detection_stats_.events_[2].publish(msg); } } //namespace \ No newline at end of file diff --git a/RWR/src/read_event_pub/src/read_event_pub_node.cpp b/RWR/src/read_event_pub/src/read_event_pub_node.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4b220cd336041f873ecc58edaacf6a6710e27821 --- /dev/null +++ b/RWR/src/read_event_pub/src/read_event_pub_node.cpp @@ -0,0 +1,13 @@ +#include "event_array_sub.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "read_event_pub"); + + ros::NodeHandle nh; + event_array_sub::EventArraySub eventArraySub(nh); + //EventArraySub eventArraySub(nh); + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/RWR/src/spot_micro_keyboard_command/scripts/postObjDetect.py b/RWR/src/spot_micro_keyboard_command/scripts/postObjDetect.py old mode 100644 new mode 100755 index 9fd1402b9ed890a913333448549aae8497554086..da1ebebec89359ba5dce579ad6b8f23aaf42e61e --- a/RWR/src/spot_micro_keyboard_command/scripts/postObjDetect.py +++ b/RWR/src/spot_micro_keyboard_command/scripts/postObjDetect.py @@ -38,7 +38,7 @@ def objCallback(obj_det): def listener(): rospy.init_node('obj_det_sub') rospy.Subscriber('/obj_det', Bool, objCallback, queue_size=1) - rospy.Spin() + rospy.spin() if __name__ == '__main__': listener()