void TiltLaserListener::processTiltHalfDown(
const ros::Time& stamp, const double& joint_angle)
{
double velocity = joint_angle - prev_angle_;
if (velocity < 0 && prev_velocity_ >= 0) {
start_time_ = stamp;
}
else if (velocity > 0 && prev_velocity_ <= 0) {
publishTimeRange(stamp, start_time_, stamp);
}
prev_angle_ = joint_angle;
prev_velocity_ = velocity;
}
void TiltLaserListener::processTilt(
const ros::Time& stamp, const double& joint_angle)
{
if (buffer_.size() > 3) {
double direction = buffer_[buffer_.size() - 1]->getValue() - joint_angle;
int change_count = 0;
for (size_t i = buffer_.size() - 1; i > 0; i--) {
double current_direction = buffer_[i-1]->getValue() - buffer_[i]->getValue();
if (direction * current_direction < 0) {
++change_count;
}
if (change_count == 2) {
ros::Time start_time = buffer_[i]->header.stamp;
publishTimeRange(stamp, start_time, stamp);
if (clear_assembled_scans_) {
buffer_.removeBefore(stamp);
}
else {
buffer_.removeBefore(buffer_[i-1]->header.stamp);
}
break;
}
direction = current_direction;
}
}
std_msgs::Header header;
header.stamp = stamp;
StampedJointAngle::Ptr j(new StampedJointAngle(header, joint_angle));
if (buffer_.size() > 0 && buffer_[buffer_.size() - 1]->getValue() == joint_angle) {
// do nothing, duplicate value
}
else {
buffer_.push_back(j);
}
}