18 #include <message_filters/subscriber.h> 19 #include <message_filters/time_synchronizer.h> 20 #include <opencv2/opencv.hpp> 22 #include <object_msgs/ObjectInBox.h> 23 #include <object_msgs/ObjectsInBoxes.h> 25 #define LINESPACING 20 30 static boost::posix_time::ptime duration_start = boost::posix_time::microsec_clock::local_time();
31 static int frame_cnt = 0;
35 boost::posix_time::ptime current = boost::posix_time::microsec_clock::local_time();
36 boost::posix_time::time_duration msdiff = current - duration_start;
38 if (msdiff.total_milliseconds() > 1000)
42 duration_start = current;
48 void syncCb(
const sensor_msgs::ImageConstPtr& img,
const object_msgs::ObjectsInBoxes::ConstPtr& objs_in_boxes)
51 int width = img->width;
52 int height = img->height;
54 for (
auto obj : objs_in_boxes->objects_vector)
57 ss << obj.object.object_name <<
": " << obj.object.probability * 100 <<
'%';
59 int xmin = obj.roi.x_offset;
60 int ymin = obj.roi.y_offset;
61 int w = obj.roi.width;
62 int h = obj.roi.height;
64 int xmax = ((xmin + w) < width) ? (xmin + w) : width;
65 int ymax = ((ymin + h) < height) ? (ymin + h) : height;
67 cv::Point left_top = cv::Point(xmin, ymin);
68 cv::Point right_bottom = cv::Point(xmax, ymax);
69 cv::rectangle(cvImage, left_top, right_bottom, cv::Scalar(0, 255, 0), 1, cv::LINE_8, 0);
70 cv::rectangle(cvImage, cvPoint(xmin, ymin), cvPoint(xmax, ymin + 20), cv::Scalar(0, 255, 0), -1);
71 cv::putText(cvImage, ss.str(), cvPoint(xmin + 5, ymin + 20), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 0, 255), 1);
78 cv::putText(cvImage, ss.str(), cvPoint(
LINESPACING,
LINESPACING), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0));
79 cv::imshow(
"image_viewer", cvImage);
81 int key = cv::waitKey(5);
82 if (key == 13 || key == 27 || key == 32 || key == 113)
88 int main(
int argc,
char** argv)
90 ros::init(argc, argv,
"movidius_ncs_example_stream");
93 message_filters::Subscriber<sensor_msgs::Image> camSub(nh,
"/camera/color/image_raw", 1);
94 message_filters::Subscriber<object_msgs::ObjectsInBoxes> objSub(nh,
"/movidius_ncs_nodelet/detected_objects", 1);
95 message_filters::TimeSynchronizer<sensor_msgs::Image, object_msgs::ObjectsInBoxes> sync(camSub, objSub, 60);
96 sync.registerCallback(boost::bind(&
syncCb, _1, _2));
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
void syncCb(const sensor_msgs::ImageConstPtr &img, const object_msgs::ObjectsInBoxes::ConstPtr &objs_in_boxes)
ROSCPP_DECL void shutdown()