Fix/rewrite sync #324

ghost · 2017-03-08T10:54:45Z

Use message filters to sync the scans.

fix https://github.com/ipa320/cob_driver/issues/268

mgruhler · 2017-03-08T12:35:20Z

cob_scan_unifier/src/scan_unifier_node.cpp

+    }
+    default:
+      ROS_ERROR_STREAM(config_.number_input_scans << " topics have been set as input, but scan_unifier doesn't support this.");
+      return;


Hmm, I don't like that this can only be done for 2 or 3 scans.
This was better in the old implementation. Is there a way to achieve the same here?

I don't like this too, but message_filters::TimeSynchronizer is implemented for a max of 9 scans, with a separate impl for every number-of-scanners. I tried but I didn't find a way to make it number-of-scanners-independent. I talked to ipa-fxm about this and he didn't know a better solution either.

To me, the documentation on the wiki seems to state that we can sync up to 9 topics:

C++: Up to 9 separate filters, each of which is of the form void callback(const boost::shared_ptr&). The number of filters supported is determined by the number of template arguments the class was created with.

I'd say, this should be fine then...

@ipa-fxm fyi
@mig-em please note if this is superseeding another PR and cross-referene.

Well, shouldn't we make it in a way then to be able to handle the nine scanners then? Use a vector of synchronizers, maybe?

alright, ignore this one: discussed in person.

For the sake of completeness: Synchronizer-API is implemented in the same (ugly) way...

mgruhler · 2017-03-09T07:45:00Z

cob_scan_unifier/src/scan_unifier_node.cpp

+        else
+        {
+          ROS_WARN_STREAM("Scan unifier skipped scan with " << current_scans.at(i)->header.stamp << " stamp.");
+        }


Is this correct? We continue when waitForTransform times out and throw a warning if not. Or did I mix that up?

I am confused as well....
With the Synchronizer we never should need to skip a scan any more...that's the whole point....

Synchronizer does only sync the messages, so it doesn't make any difference for the tf problem...

Okay, this is fine.

But: Isn't this still the wrong way around?

Well, now that I'm looking at it:
We are handing over the tf::TransformListener object to the projector_. So in case all the waitForTransform magic we do in order to make the transformLaserScanToPointCloud work....maybe we just need to make our listener_ object work!

Because currently it's using the default constructor (http://docs.ros.org/indigo/api/tf/html/c++/classtf_1_1TransformListener.html), i.e. default values for max_cache_time...maybe we can tweak something here...what also helps qutie often is "wait a little" at the end of the constructor to let the listener_ fill its cache...as we are seeing this problem only in the very beginning...

Also, is this ROS_WARN_STREAM still seen in the log? Because maybe the better time synchronization now does not give us messy timestamps as the "old" synchronization might have...

Last not least, in case the !waitForTranform case cannot be avoided by any means...should we return, i.e. skip all scans of current cycle or only skip the scan that failed, i.e. incomplete scan unified

Is this correct? We continue when waitForTransform times out and throw a warning if not. Or did I mix that up?

ja is wrong!

mgruhler · 2017-03-09T07:45:44Z

cob_scan_unifier/src/scan_unifier_node.cpp

+      typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::LaserScan, sensor_msgs::LaserScan> SyncPolicy;
+      synchronizer2_ = new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(2), *message_filter_subscribers_.at(0), *message_filter_subscribers_.at(1));
+      synchronizer2_->setInterMessageLowerBound(0, ros::Duration(0.167));
+      synchronizer2_->setInterMessageLowerBound(1, ros::Duration(0.167));


How did you choose this number?

=0.5*(1/12) // half of expected scanner rate at 12Hz (I guess)

Hmm, is it safe to assume this is the lower bound? Or would we want to merge scans with a frequency <12Hz as well?

Is 12Hz a configurable parameter for the scanners?
Then we at least should make the computation of the bounds based on a parameter as we might unify scans from other sensors with different publish rate as well...

To be honest, I'm not sure. There is a parameter about the publish rate in the S300 driver. But: afaik, this is not respected. The new S300 Expert do have an output of about 25Hz (phyiscal max of the scanner) with our driver.

The frequency of the data output depends on the configuration in the scanner itself.

Also, other scanners have other frequencies... And we could actually try to merge scanners with different frequencies. But this gets ugly.

let's make it work for our use-case....either 2 s300 (raw) or 3 s300 (cob4)...then test it...eventually it's cob_scan_unifier anyway
(but add a comment about where the formula comes from, i.e. relate it to expected s300 publish rate)

fine for me. Then we can stick to 12Hz (even though the scanner itself could be configured to send out with less frequency without respecting anything in the driver. we just should not do that).

Then we can stick with the 0.5° spacing in my comment below as well...

Well, I meant after successful testing we could at least introduce parameters to make the scan_unifier node configurable...but those parameters would be scan_unifier parameters and not related to parameters (of the same meaning) of the scanner driver, though

Yes, I fully understand this. I just wanted to point out that this is in the end not independent (high expected frequency in the unifier + low frequency in the scanner = no unified scan).

But I'm sure I'm preaching to the choir here :-)

mgruhler · 2017-03-09T07:47:12Z

cob_scan_unifier/src/scan_unifier_node.cpp


-  initLaserScanStructs();
+//  synchronizer_->registerCallback(&ScanUnifierNode::messageFilterCallback);


remove, this is not used.

mgruhler · 2017-03-09T07:52:15Z

cob_scan_unifier/src/scan_unifier_node.cpp

-    unified_scan.header = vec_laser_struct_.at(0).current_scan_msg.header;
-    unified_scan.header.frame_id = "base_link";
+    unified_scan.header = current_scans.at(0)->header;
+    unified_scan.header.frame_id = frame_;
    unified_scan.angle_increment = M_PI/180.0/2.0;
    unified_scan.angle_min = -M_PI + unified_scan.angle_increment*0.01;
    unified_scan.angle_max =  M_PI - unified_scan.angle_increment*0.01;


This might actually be the reason for #249

Also, the above restricts this unifier to be used with a spacing fo 0.5°. This should probably be adapted as well.
Something like "biggest common denominator" but in float ;-)
Added it to #249

fmessmer · 2017-03-09T08:05:02Z

cob_scan_unifier/src/scan_unifier_node.cpp

+      delete(synchronizer3_);
+      break;
+    }
+  }


I guess there is actually no need to switch...you could just delete both...

fmessmer · 2017-03-09T08:06:43Z

cob_scan_unifier/src/scan_unifier_node.cpp

  XmlRpc::XmlRpcValue topicList;

+  // TODO short parsing


Link to list retrieval: http://wiki.ros.org/roscpp/Overview/Parameter%20Server#Retrieving_Lists

fmessmer · 2017-03-09T08:07:39Z

cob_scan_unifier/src/scan_unifier_node.cpp

+  }
+  else
+  {
+    pnh_.getParam("frame", frame_);


just use "default version" (nh.param<std::string>("default_param", default_param, "default_value");) ?

fmessmer · 2017-03-09T08:09:23Z

cob_scan_unifier/src/scan_unifier_node.cpp

- * output: -
- */
-void scan_unifier_node::initLaserScanStructs()
+void ScanUnifierNode::messageFilterCallback(const sensor_msgs::LaserScan::ConstPtr& first_scanner, const sensor_msgs::LaserScan::ConstPtr& second_scanner)


I'd prefer scan1 instead of first_scanner, scan2 instead of second_scanner....

mgruhler · 2017-03-15T07:42:50Z

cob_scan_unifier/src/scan_unifier_node.cpp

+//
+//      config_.number_input_scans = config_.input_scan_topics.size();
+//    }
+//  }


This could/should be removed...

… handling.

Elias Marks added 2 commits March 1, 2017 16:38

Use message_filter::Synchronizer (there is still a bug)

d1156a2

Cleanup

3646521

mgruhler suggested changes Mar 8, 2017

View reviewed changes

mgruhler suggested changes Mar 9, 2017

View reviewed changes

mgruhler mentioned this pull request Mar 9, 2017

[cob_scan_unifier] cob_scan_unifier is not merging complete scans in simulation #249

Open

fmessmer reviewed Mar 9, 2017

View reviewed changes

fmessmer mentioned this pull request Mar 9, 2017

Skip laserscans with no corresponding tf transform #313

Closed

Some small fixes

45efde8

mgruhler suggested changes Mar 15, 2017

View reviewed changes

Added sleep in constructor, new topic parameter parsing, better error…

e4aed56

… handling.

mgruhler approved these changes Mar 16, 2017

View reviewed changes

remove commented line

3fd0cf5

fmessmer merged commit 6b57771 into 4am-robotics:indigo_dev Mar 16, 2017

This was referenced Mar 16, 2017

[cob_scan_unifier] Update rate #248

Closed

[cob_scan_unifier] LookupError on start up #268

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix/rewrite sync #324

Fix/rewrite sync #324

ghost commented Mar 8, 2017 •

edited by fmessmer

Loading

mgruhler Mar 8, 2017

ghost Mar 8, 2017 •

edited by ghost

Loading

mgruhler Mar 8, 2017

mgruhler Mar 8, 2017

mgruhler Mar 8, 2017

mgruhler Mar 9, 2017

fmessmer Mar 9, 2017

mgruhler Mar 9, 2017

fmessmer Mar 9, 2017

ghost Mar 14, 2017

mgruhler Mar 15, 2017

fmessmer Mar 15, 2017 •

edited

Loading

fmessmer Mar 15, 2017

mgruhler Mar 9, 2017

fmessmer Mar 9, 2017

mgruhler Mar 9, 2017

fmessmer Mar 9, 2017

mgruhler Mar 9, 2017 •

edited

Loading

fmessmer Mar 9, 2017 •

edited

Loading

mgruhler Mar 9, 2017

fmessmer Mar 9, 2017 •

edited

Loading

mgruhler Mar 9, 2017

mgruhler Mar 9, 2017

mgruhler Mar 9, 2017

fmessmer Mar 9, 2017

fmessmer Mar 9, 2017

fmessmer Mar 9, 2017

fmessmer Mar 9, 2017 •

edited

Loading

mgruhler Mar 15, 2017


		initLaserScanStructs();
		// synchronizer_->registerCallback(&ScanUnifierNode::messageFilterCallback);

Fix/rewrite sync #324

Fix/rewrite sync #324

Conversation

ghost commented Mar 8, 2017 • edited by fmessmer Loading

Choose a reason for hiding this comment

ghost Mar 8, 2017 • edited by ghost Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

fmessmer Mar 15, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mgruhler Mar 9, 2017 • edited Loading

Choose a reason for hiding this comment

fmessmer Mar 9, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

fmessmer Mar 9, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

fmessmer Mar 9, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ghost commented Mar 8, 2017 •

edited by fmessmer

Loading

ghost Mar 8, 2017 •

edited by ghost

Loading

fmessmer Mar 15, 2017 •

edited

Loading

mgruhler Mar 9, 2017 •

edited

Loading

fmessmer Mar 9, 2017 •

edited

Loading

fmessmer Mar 9, 2017 •

edited

Loading

fmessmer Mar 9, 2017 •

edited

Loading