Feature/timestamp from phaselock angle #481
Conversation
There was a problem hiding this comment.
I was checking GitHub and noticed my issue was mentioned in this MR. I came up with a hack back then but this looks like a proper way to add the feature. I have some comments to reduce redundant code and make it a bit easier to understand for future people who are new to the codebase.
| double timestamp_angle; | ||
| private_nh.param("timestamp_angle", timestamp_angle, -0.01); | ||
| if (timestamp_angle < 0.0) | ||
| { | ||
| ROS_INFO_STREAM("Time at specific angle feature deactivated."); | ||
| } | ||
| else if (timestamp_angle < (2*M_PI)) | ||
| { | ||
| ROS_INFO_STREAM("Time at specific angle feature activated. " | ||
| "Set to " << timestamp_angle << " rad."); | ||
| if (config_.timestamp_first_packet){ | ||
| ROS_ERROR_STREAM("timestamp_first_packet AND timestamp_angle configured! timestamp_first_packet will be used!"); | ||
| } | ||
| } | ||
| else | ||
| { | ||
| ROS_ERROR_STREAM("timestamp_angle is out of range. Allowed range is " | ||
| << "between 0.0 and 2*PI or negative values to deactivate this feature."); | ||
| timestamp_angle = -0.01; | ||
| } | ||
|
|
||
| // Convert timestamp_angle from radian to one-hundredth degree, | ||
| // which is used in velodyne packets | ||
| config_.timestamp_angle = int((timestamp_angle*360/(2*M_PI))*100); |
There was a problem hiding this comment.
This whole initialization function is getting long. I think the Radian to hundredth degree could be a function since it is now used twice. This timestamp validation code could be in its own function. I'm also concerned about the timestamp_angle param being overwritten
| if((last_azimuth_ < config_.timestamp_angle && config_.timestamp_angle <= azimuth) | ||
| || ( config_.timestamp_angle <= azimuth && azimuth < last_azimuth_) | ||
| || (azimuth < last_azimuth_ && last_azimuth_ < config_.timestamp_angle)) |
There was a problem hiding this comment.
The logic in this if statement is the same logic as line 275 and should really be placed in its own function to be used on both lines
if (angleIsBetween(last_azimuth_, azimuth, config_.timestamp_angle))
Within the function, I think you need a comment just explaining the logic since it was not immediately obvious to me.
// Case 1: 0° < last azimuth < angle < azimuth < 360°
// Case 2: last azimuth < 360° < 0° < angle < azimuth
// Case 3: last azimuth < angle < 360° < 0° < azimuth
I'm not even completely sure what is going on since there are other cases listed in the issue here but I don't know how those show up #174
|
Please address comments by @mpitropov. |
| @@ -74,6 +74,7 @@ class VelodyneDriver | |||
| int npackets; // number of packets to collect | |||
| double rpm; // device rotation rate (RPMs) | |||
| int cut_angle; // cutting angle in 1/100° | |||
| int timestamp_angle; // configured phase lock angle | |||
There was a problem hiding this comment.
Could you please include a description somewhere of what this actually means/does? It's definitely unclear to me.
|
Thanks for the feedback! |
Adds the possibility to set the point cloud time stamp to the stamp of the package received when passing the phase_lock angle.
It is implemented similar to the cut_off angle and makes probably only sense to use both together.
I tested it live with an VLP32-c
Addresses also #242