Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/timestamp from phaselock angle #481

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Feature/timestamp from phaselock angle #481

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
dda64b3
Add timestamp from phase_lock angle
Oct 25, 2022
105ee42
Add parameters to configure timestamp at phase_lock angle to launch f…
Oct 25, 2022
502037a
Redo timestamp from phase lock to be more generic. timestamp from angle.
Oct 26, 2022
8093567
fix leftovers in launch file
PhilippSchmaelzle Nov 9, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions velodyne_driver/include/velodyne_driver/driver.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Could you please include a description somewhere of what this actually means/does? It's definitely unclear to me.

double time_offset; // time in seconds added to each velodyne time stamp
bool enabled; // polling is enabled
bool timestamp_first_packet;
Expand Down
2 changes: 2 additions & 0 deletions velodyne_driver/launch/nodelet_manager.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
<arg name="gps_time" default="false" />
<arg name="pcap_time" default="false" />
<arg name="cut_angle" default="-0.01" />
<arg name="timestamp_angle" default="-0.01" />
<arg name="timestamp_first_packet" default="false" />
<arg name="diagnostic_frequency_tolerance" default="0.1" />

Expand All @@ -37,6 +38,7 @@
<param name="gps_time" value="$(arg gps_time)"/>
<param name="pcap_time" value="$(arg pcap_time)"/>
<param name="cut_angle" value="$(arg cut_angle)"/>
<param name="timestamp_angle" value="$(arg timestamp_angle)"/>
<param name="timestamp_first_packet" value="$(arg timestamp_first_packet)"/>
<param name="diagnostic_frequency_tolerance" value="$(arg diagnostic_frequency_tolerance)"/>
</node>
Expand Down
38 changes: 38 additions & 0 deletions velodyne_driver/src/driver/driver.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,6 +147,31 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node,
// which is used in velodyne packets
config_.cut_angle = int((cut_angle*360/(2*M_PI))*100);

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);
Comment on lines +150 to +173
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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


int udp_port;
private_nh.param("port", udp_port, (int) DATA_PORT_NUMBER);

Expand Down Expand Up @@ -213,6 +238,8 @@ bool VelodyneDriver::poll(void)
// Allocate a new shared pointer for zero-copy sharing with other nodelets.
velodyne_msgs::VelodyneScanPtr scan(new velodyne_msgs::VelodyneScan);

ros::Time time_angle;

if( config_.cut_angle >= 0) //Cut at specific angle feature enabled
{
scan->packets.reserve(config_.npackets);
Expand All @@ -237,6 +264,14 @@ bool VelodyneDriver::poll(void)
last_azimuth_ = azimuth;
continue;
}

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))
Comment on lines +268 to +270
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

{
time_angle = tmp_packet.stamp;
}

if((last_azimuth_ < config_.cut_angle && config_.cut_angle <= azimuth)
|| ( config_.cut_angle <= azimuth && azimuth < last_azimuth_)
|| (azimuth < last_azimuth_ && last_azimuth_ < config_.cut_angle))
Expand Down Expand Up @@ -270,6 +305,9 @@ bool VelodyneDriver::poll(void)
if (config_.timestamp_first_packet){
scan->header.stamp = scan->packets.front().stamp;
}
else if(config_.timestamp_angle >= 0.0){
scan->header.stamp = time_angle;
}
else{
scan->header.stamp = scan->packets.back().stamp;
}
Expand Down
3 changes: 3 additions & 0 deletions velodyne_pointcloud/launch/32e_points.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
<arg name="gps_time" default="false" />
<arg name="pcap_time" default="false" />
<arg name="cut_angle" default="-0.01" />
<arg name="timestamp_angle" default="-0.01" />
<arg name="timestamp_first_packet" default="false" />
<arg name="laserscan_ring" default="-1" />
<arg name="laserscan_resolution" default="0.007" />
Expand All @@ -39,6 +40,8 @@
<arg name="gps_time" value="$(arg gps_time)"/>
<arg name="pcap_time" value="$(arg pcap_time)"/>
<arg name="cut_angle" value="$(arg cut_angle)"/>
<arg name="phase_lock_angle" value="$(arg phase_lock_angle)"/>
<arg name="timestamp_angle" value="$(arg timestamp_angle)"/>
<arg name="timestamp_first_packet" value="$(arg timestamp_first_packet)"/>
</include>

Expand Down
3 changes: 3 additions & 0 deletions velodyne_pointcloud/launch/64e_S3.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
<arg name="gps_time" default="false" />
<arg name="pcap_time" default="false" />
<arg name="cut_angle" default="-0.01" />
<arg name="timestamp_angle" default="-0.01" />
<arg name="timestamp_first_packet" default="false" />
<arg name="laserscan_ring" default="-1" />
<arg name="laserscan_resolution" default="0.007" />
Expand All @@ -39,6 +40,8 @@
<arg name="gps_time" value="$(arg gps_time)"/>
<arg name="pcap_time" value="$(arg pcap_time)"/>
<arg name="cut_angle" value="$(arg cut_angle)"/>
<arg name="phase_lock_angle" value="$(arg phase_lock_angle)"/>
<arg name="timestamp_angle" value="$(arg timestamp_angle)"/>
<arg name="timestamp_first_packet" value="$(arg timestamp_first_packet)"/>
</include>

Expand Down
2 changes: 2 additions & 0 deletions velodyne_pointcloud/launch/VLP-32C_points.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
<arg name="gps_time" default="false" />
<arg name="pcap_time" default="false" />
<arg name="cut_angle" default="-0.01" />
<arg name="timestamp_angle" default="-0.01" />
<arg name="timestamp_first_packet" default="false" />
<arg name="laserscan_ring" default="-1" />
<arg name="laserscan_resolution" default="0.007" />
Expand All @@ -39,6 +40,7 @@
<arg name="gps_time" value="$(arg gps_time)"/>
<arg name="pcap_time" value="$(arg pcap_time)"/>
<arg name="cut_angle" value="$(arg cut_angle)"/>
<arg name="timestamp_angle" value="$(arg timestamp_angle)"/>
<arg name="timestamp_first_packet" value="$(arg timestamp_first_packet)"/>
</include>

Expand Down
2 changes: 2 additions & 0 deletions velodyne_pointcloud/launch/VLP16_points.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
<arg name="gps_time" default="false" />
<arg name="pcap_time" default="false" />
<arg name="cut_angle" default="-0.01" />
<arg name="timestamp_angle" default="-0.01" />
<arg name="timestamp_first_packet" default="false" />
<arg name="laserscan_ring" default="-1" />
<arg name="laserscan_resolution" default="0.007" />
Expand All @@ -39,6 +40,7 @@
<arg name="gps_time" value="$(arg gps_time)"/>
<arg name="pcap_time" value="$(arg pcap_time)"/>
<arg name="cut_angle" value="$(arg cut_angle)"/>
<arg name="timestamp_angle" value="$(arg timestamp_angle)"/>
<arg name="timestamp_first_packet" value="$(arg timestamp_first_packet)"/>
</include>

Expand Down
3 changes: 3 additions & 0 deletions velodyne_pointcloud/launch/VLS128_points.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
<arg name="rpm" default="600.0" />
<arg name="gps_time" default="false" />
<arg name="cut_angle" default="-0.01" />
<arg name="timestamp_angle" default="-0.01" />
<arg name="timestamp_first_packet" default="false" />
<arg name="laserscan_ring" default="-1" />
<arg name="laserscan_resolution" default="0.007" />
Expand All @@ -37,6 +38,8 @@
<arg name="rpm" value="$(arg rpm)"/>
<arg name="gps_time" value="$(arg gps_time)"/>
<arg name="cut_angle" value="$(arg cut_angle)"/>
<arg name="phase_lock_angle" value="$(arg phase_lock_angle)"/>
<arg name="timestamp_angle" value="$(arg timestamp_angle)"/>
<arg name="timestamp_first_packet" value="$(arg timestamp_first_packet)"/>
</include>

Expand Down