void TiltLaserListener::publishTimeRange(
const ros::Time& stamp,
const ros::Time& start,
const ros::Time& end)
{
jsk_recognition_msgs::TimeRange range;
range.header.stamp = stamp;
range.start = start;
range.end = end;
trigger_pub_.publish(range);
if (use_laser_assembler_) {
if (skip_counter_++ % skip_number_ == 0) {
laser_assembler::AssembleScans2 srv;
srv.request.begin = start;
srv.request.end = end;
try {
if (!not_use_laser_assembler_service_) {
if (assemble_cloud_srv_.call(srv)) {
sensor_msgs::PointCloud2 output_cloud = srv.response.cloud;
output_cloud.header.stamp = stamp;
cloud_pub_.publish(output_cloud);
}
else {
JSK_NODELET_ERROR("Failed to call assemble cloud service");
}
}
else {
// Assemble cloud from local buffer
std::vector<sensor_msgs::PointCloud2::ConstPtr> target_clouds;
{
boost::mutex::scoped_lock lock(cloud_mutex_);
for (size_t i = 0; i < cloud_buffer_.size(); i++) {
ros::Time the_stamp = cloud_buffer_[i]->header.stamp;
if (the_stamp > start && the_stamp < end) {
target_clouds.push_back(cloud_buffer_[i]);
}
}
cloud_buffer_.removeBefore(start);
}
sensor_msgs::PointCloud2 output_cloud;
getPointCloudFromLocalBuffer(target_clouds, output_cloud);
output_cloud.header.stamp = stamp;
cloud_pub_.publish(output_cloud);
}
}
catch (...) {
JSK_NODELET_ERROR("Exception in calling assemble cloud service");
}
}
}
}
void TiltLaserListener::publishTimeRange(
const ros::Time& stamp,
const ros::Time& start,
const ros::Time& end)
{
jsk_recognition_msgs::TimeRange range;
range.header.stamp = stamp;
range.start = start;
range.end = end;
trigger_pub_.publish(range);
// publish velocity
// only publish if twist_frame_id is not empty
if (!twist_frame_id_.empty()) {
// simply compute from the latest velocity
if (buffer_.size() >= 2) {
// at least, we need two joint angles to
// compute velocity
StampedJointAngle::Ptr latest = buffer_[buffer_.size() - 1];
StampedJointAngle::Ptr last_second = buffer_[buffer_.size() - 2];
double value_diff = latest->getValue() - last_second->getValue();
double time_diff = (latest->header.stamp - last_second->header.stamp).toSec();
double velocity = value_diff / time_diff;
geometry_msgs::TwistStamped twist;
twist.header.frame_id = twist_frame_id_;
twist.header.stamp = latest->header.stamp;
if (laser_type_ == INFINITE_SPINDLE_HALF || // roll laser
laser_type_ == INFINITE_SPINDLE) {
twist.twist.angular.x = velocity;
}
else { // tilt laser
twist.twist.angular.y = velocity;
}
twist_pub_.publish(twist);
}
}
if (use_laser_assembler_) {
if (skip_counter_++ % skip_number_ == 0) {
laser_assembler::AssembleScans2 srv;
srv.request.begin = start;
srv.request.end = end;
try {
if (!not_use_laser_assembler_service_) {
if (assemble_cloud_srv_.call(srv)) {
sensor_msgs::PointCloud2 output_cloud = srv.response.cloud;
output_cloud.header.stamp = stamp;
cloud_pub_.publish(output_cloud);
}
else {
JSK_NODELET_ERROR("Failed to call assemble cloud service");
}
}
else {
// Assemble cloud from local buffer
std::vector<sensor_msgs::PointCloud2::ConstPtr> target_clouds;
{
boost::mutex::scoped_lock lock(cloud_mutex_);
if (cloud_buffer_.size() == 0) {
return;
}
for (size_t i = 0; i < cloud_buffer_.size(); i++) {
ros::Time the_stamp = cloud_buffer_[i]->header.stamp;
if (the_stamp > start && the_stamp < end) {
target_clouds.push_back(cloud_buffer_[i]);
}
}
if (clear_assembled_scans_) {
cloud_buffer_.removeBefore(end);
}
else {
cloud_buffer_.removeBefore(start);
}
}
sensor_msgs::PointCloud2 output_cloud;
getPointCloudFromLocalBuffer(target_clouds, output_cloud);
output_cloud.header.stamp = stamp;
cloud_pub_.publish(output_cloud);
}
}
catch (...) {
JSK_NODELET_ERROR("Exception in calling assemble cloud service");
}
}
}
}
