Skip to content
Snippets Groups Projects
Commit ccf483ab authored by thirgy4's avatar thirgy4
Browse files

Python script fix and new issue with event sim sub

parent 96d2ff21
No related branches found
No related tags found
1 merge request!3Resolve "Create a simulation in Gazebo of the quadruped robot and intergrate the event camera"
......@@ -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
#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
#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
#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
......@@ -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()
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment