Skip to content

Commit baa0419

Browse files
MoscowskyAntonMoscowskyAnton
committed
depth also can be synchronized
1 parent 935e84e commit baa0419

File tree

2 files changed

+38
-26
lines changed

2 files changed

+38
-26
lines changed

‎src/extended_object_detection_node/eod_node.cpp‎

Lines changed: 32 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -70,6 +70,7 @@ EOD_ROS::EOD_ROS(ros::NodeHandle nh, ros::NodeHandle nh_p){
7070
nh_p_.param("publish_markers", publish_markers, false);
7171
nh_p_.param("broadcast_tf", broadcast_tf, false);
7272
nh_p_.param("allowed_lag_sec", allowed_lag_sec, 0.0);
73+
nh_p_.param("subs_queue_size", subs_queue_size, 10);
7374

7475
std::string object_base_path;
7576
nh_p_.getParam("object_base",object_base_path);
@@ -138,42 +139,52 @@ EOD_ROS::EOD_ROS(ros::NodeHandle nh, ros::NodeHandle nh_p){
138139

139140
for( size_t i = 0 ; i < rgb_image_topics.size() ; i++ ){
140141

141-
ROS_INFO("Bounding %s and %s...",rgb_image_topics[i].c_str(), rgb_info_topics[i].c_str());
142-
142+
//ROS_INFO("Bounding %s and %s...",rgb_image_topics[i].c_str(), rgb_info_topics[i].c_str());
143+
143144
rgb_it_.push_back(new image_transport::ImageTransport(nh_));
144145
sub_rgb_.push_back(new image_transport::SubscriberFilter());
145146
sub_info_.push_back(new message_filters::Subscriber<sensor_msgs::CameraInfo>());
146147

147-
sub_rgb_[i]->subscribe(*rgb_it_[i], rgb_image_topics[i], 10);
148-
sub_info_[i]->subscribe(nh_, rgb_info_topics[i], 10);
148+
sub_rgb_[i]->subscribe(*rgb_it_[i], rgb_image_topics[i], subs_queue_size);
149+
sub_info_[i]->subscribe(nh_, rgb_info_topics[i], subs_queue_size);
149150

150151
// set up message filters
151152
if( depth_image_topics.size() == 0 ){
152153

153154

154155
rgb_sync_.push_back(new boost::shared_ptr<RGBSynchronizer>());
155-
rgb_sync_[i]->reset(new RGBSynchronizer(RGBInfoSyncPolicy(10), *sub_rgb_[i], *sub_info_[i]) );
156+
rgb_sync_[i]->reset(new RGBSynchronizer(RGBInfoSyncPolicy(subs_queue_size), *sub_rgb_[i], *sub_info_[i]) );
156157

157158
(*rgb_sync_[i])->registerCallback(boost::bind(&EOD_ROS::rgb_info_cb, this, boost::placeholders::_1, boost::placeholders::_2));
158159
}
159-
/* else{
160-
//ROS_INFO("Configuring filter on rgbd images and infos...");
161-
sub_depth_.subscribe(*rgb_it_, "depth/image_raw", 10);
162-
sub_depth_info_.subscribe(nh_, "depth/info", 10);
160+
else{
161+
depth_it_.push_back(new image_transport::ImageTransport(nh_));
162+
//sub_depth_.subscribe(*rgb_it_, "depth/image_raw", 10);
163+
//sub_depth_info_.subscribe(nh_, "depth/info", 10);
164+
//depth_it_
165+
sub_depth_.push_back(new image_transport::SubscriberFilter());
166+
sub_depth_info_.push_back(new message_filters::Subscriber<sensor_msgs::CameraInfo>);
167+
168+
sub_depth_[i]->subscribe(*depth_it_[i], depth_image_topics[i], subs_queue_size);
169+
sub_depth_info_[i]->subscribe(nh_, depth_info_topics[i], subs_queue_size);
163170

164-
rgbd_sync_.reset( new RGBDSynchronizer(RGBDInfoSyncPolicy(10), sub_rgb_, sub_info_, sub_depth_, sub_depth_info_) );
165-
rgbd_sync_->registerCallback(boost::bind(&EOD_ROS::rgbd_info_cb, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
166-
}*/
171+
rgbd_sync_.push_back(new boost::shared_ptr<RGBDSynchronizer>);
172+
rgbd_sync_[i]->reset( new RGBDSynchronizer(RGBDInfoSyncPolicy(subs_queue_size), *sub_rgb_[i], *sub_info_[i], *sub_depth_[i], *sub_depth_info_[i]) );
173+
174+
(*rgbd_sync_[i])->registerCallback(boost::bind(&EOD_ROS::rgbd_info_cb, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
175+
}
167176

168177
}
169178
//ROS_INFO("Configured!");
170179
}
171180

172181

173-
bool EOD_ROS::check_time(const ros::Time& stamp){
174-
if( frame_sequence == 0)
182+
bool EOD_ROS::check_time(const ros::Time& stamp, std::string frame_id){
183+
if( frame_sequence == 0){
184+
prev_detected_time[frame_id] = stamp;
175185
return true;
176-
return (stamp - prev_detected_time).toSec() > rate_limit_sec;
186+
}
187+
return (stamp - prev_detected_time[frame_id]).toSec() > rate_limit_sec;
177188
}
178189

179190

@@ -208,7 +219,7 @@ cv::Mat EOD_ROS::getD(const sensor_msgs::CameraInfoConstPtr& info_msg){
208219
void EOD_ROS::rgb_info_cb(const sensor_msgs::ImageConstPtr& rgb_image, const sensor_msgs::CameraInfoConstPtr& rgb_info){
209220
//ROS_INFO("Got Image!");
210221
// CHECK RATE
211-
if( !check_time(ros::Time::now()) ) {
222+
if( !check_time(ros::Time::now(), rgb_image->header.frame_id) ) {
212223
//ROS_WARN("Skipped frame");
213224
return;
214225
}
@@ -233,7 +244,7 @@ void EOD_ROS::rgb_info_cb(const sensor_msgs::ImageConstPtr& rgb_image, const sen
233244
void EOD_ROS::rgbd_info_cb(const sensor_msgs::ImageConstPtr& rgb_image, const sensor_msgs::CameraInfoConstPtr& rgb_info, const sensor_msgs::ImageConstPtr& depth_image, const sensor_msgs::CameraInfoConstPtr& depth_info){
234245
//ROS_INFO("Got RGBD!");
235246
// CHECK RATE
236-
if( !check_time(ros::Time::now()) ) {
247+
if( !check_time(ros::Time::now(), rgb_image->header.frame_id) ) {
237248
//ROS_WARN("Skipped frame");
238249
return;
239250
}
@@ -273,11 +284,11 @@ void EOD_ROS::rgbd_info_cb(const sensor_msgs::ImageConstPtr& rgb_image, const se
273284

274285

275286
void EOD_ROS::detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std_msgs::Header header){
276-
ROS_INFO("Detecting...");
287+
//ROS_INFO("Detecting...");
277288
if( frame_sequence != 0 ){
278-
detect_rate_values->push_back((ros::Time::now() - prev_detected_time).toSec());
289+
detect_rate_values->push_back((ros::Time::now() - prev_detected_time[header.frame_id]).toSec());
279290
}
280-
prev_detected_time = ros::Time::now();
291+
prev_detected_time[header.frame_id] = ros::Time::now();
281292

282293
if( header.frame_id[0] == '/')
283294
header.frame_id.erase(0,1);
@@ -553,7 +564,7 @@ bool EOD_ROS::set_simple_objects_cb(extended_object_detection::SetObjects::Reque
553564
else{
554565
// already removed
555566
ROS_WARN("[set_simple_objects srv] object with id %i is already removed", object_id);
556-
}
567+
}
557568
}
558569
}
559570
else{

‎src/extended_object_detection_node/eod_node.h‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -49,9 +49,9 @@ class EOD_ROS{
4949
std::vector<boost::shared_ptr<RGBSynchronizer>* > rgb_sync_;
5050

5151
std::vector<image_transport::ImageTransport*> depth_it_;
52-
std::vector<image_transport::SubscriberFilter> sub_depth_;
53-
std::vector<message_filters::Subscriber<sensor_msgs::CameraInfo> > sub_depth_info_;
54-
std::vector<boost::shared_ptr<RGBDSynchronizer> > rgbd_sync_;
52+
std::vector<image_transport::SubscriberFilter*> sub_depth_;
53+
std::vector<message_filters::Subscriber<sensor_msgs::CameraInfo>* > sub_depth_info_;
54+
std::vector<boost::shared_ptr<RGBDSynchronizer>* > rgbd_sync_;
5555

5656
ros::Publisher simple_objects_pub_;
5757
ros::Publisher simple_objects_markers_pub_;
@@ -76,10 +76,11 @@ class EOD_ROS{
7676
bool publish_markers;
7777
bool broadcast_tf;
7878
double allowed_lag_sec;
79+
int subs_queue_size;
7980

8081
// vars
8182
int frame_sequence;
82-
ros::Time prev_detected_time;
83+
std::map<std::string, ros::Time> prev_detected_time;
8384
boost::circular_buffer<double>* detect_rate_values;
8485

8586
// callbacks
@@ -94,7 +95,7 @@ class EOD_ROS{
9495

9596
// functions
9697
void detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std_msgs::Header header);
97-
bool check_time(const ros::Time& stamp);
98+
bool check_time(const ros::Time& stamp, std::string frame_id);
9899
bool check_lag(const ros::Time& stamp, double &lag);
99100
extended_object_detection::BaseObject eoi_to_base_object(std::string name, int id, eod::ExtendedObjectInfo* eoi, const cv::Mat& K);
100101
cv::Mat getK(const sensor_msgs::CameraInfoConstPtr& info_msg);

0 commit comments

Comments
 (0)