bool LaserAggregationServiceCB(hubo_sensor_msgs::LidarAggregation::Request& req, hubo_sensor_msgs::LidarAggregation::Response& res)
{
ROS_INFO("Attempting to aggregate %ld laser scans into a pointcloud", req.Scans.size());
sensor_msgs::PointCloud2 full_cloud;
if (req.Scans.size() > 1)
{
g_laser_projector.transformLaserScanToPointCloud(g_fixed_frame, req.Scans[0], full_cloud, *g_transformer);
for (unsigned int index = 1; index < req.Scans.size(); index++)
{
sensor_msgs::PointCloud2 scan_cloud;
g_laser_projector.transformLaserScanToPointCloud(g_fixed_frame, req.Scans[index], scan_cloud, *g_transformer);
bool succeded = pcl::concatenatePointCloud(full_cloud, scan_cloud, full_cloud);
if (!succeded)
{
ROS_ERROR("PCL could not concatenate pointclouds");
}
}
}
else if (req.Scans.size() == 1)
{
g_laser_projector.transformLaserScanToPointCloud(g_fixed_frame, req.Scans[0], full_cloud, *g_transformer);
}
else
{
ROS_WARN("No laser scans to aggregate");
}
full_cloud.header.frame_id = g_fixed_frame;
full_cloud.header.stamp = ros::Time::now();
res.Cloud = full_cloud;
g_cloud_publisher.publish(full_cloud);
return true;
}
void My_Filter::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan) {
sensor_msgs::PointCloud2 cloud;
// scan->header.stamp += ros::Duration(0.5)
try {
projector.transformLaserScanToPointCloud("/base_link", *scan, cloud, tfListener); // 1st arg was "base_link"
header = cloud.header;
} catch(tf::TransformException ex) {
ROS_WARN("TransformException!");
ROS_ERROR("%s", ex.what());
return;
}
if (b_2d) {
pcl::PointCloud<pcl::PointXYZ> pc_2d;
pcl::fromROSMsg(cloud, pc_2d);
int points = pc_2d.width;
for (int i = 0; i < points; i++) {
pc_2d.points[i].z = 0;
}
pc_comb_2d = pc_comb_2d + pc_2d;
}
if (b_3d) {
pcl::PointCloud<pcl::PointXYZ> pc_3d;
pcl::fromROSMsg(cloud, pc_3d);
pc_comb_3d = pc_comb_3d + pc_3d;
}
}
void Node::laser_1_cb(const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
if(!listener_.waitForTransform("/laser1", "/world", scan_in->header.stamp + ros::Duration().fromSec(scan_in->ranges.size()*scan_in->time_increment), ros::Duration(1.0))) {
return;
}
sensor_msgs::PointCloud2 cloud;
projector_.transformLaserScanToPointCloud("/world", *scan_in, cloud, listener_);
pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
pcl::fromROSMsg(cloud, pcl_cloud);
crop_cloud(pcl_cloud, 1, scan_in->header.stamp);
if (!starting_config && fabs(angle) < 0.1)
{
starting_config = 1;
ROS_INFO("Ready to Scan.");
}
if (starting_config)
cloud1 += pcl_cloud;
sensor_msgs::PointCloud2 cloud_out;
pcl::toROSMsg(cloud1, cloud_out);
cloud_out.header = cloud.header;
pc_1_pub_.publish(cloud_out);
}
void scanCallback (const sensor_msgs::LaserScan scan_in) {
ROS_DEBUG("LaserScanReceiver: I have %d ranges", scan_in.ranges.size());
sensor_msgs::PointCloud cloud;
try {
projector.transformLaserScanToPointCloud(
base_tf.c_str(),scan_in, cloud,listener);
}
catch (tf::TransformException& e) {
ROS_ERROR("%s", e.what());
return;
}
obstacle_x.clear();
obstacle_y.clear();
obstacle_x.reserve(cloud.points.size());
obstacle_y.reserve(cloud.points.size());
for (std::vector<geometry_msgs::Point32>::iterator i=cloud.points.begin(); i!=cloud.points.end(); i++) {
obstacle_x.push_back(i->x);
obstacle_y.push_back(i->y);
}
laser_count = obstacle_x.size();
pcl_pub.publish(cloud);
}
//receive a sensor_msgs::laserscan from the robot laser
//translate it to the right frame and project to a sensor_msgs::PointCloud2.
void subLaserScanCallback(const LaserScan::ConstPtr& scan_in)
{
//transform to the right frame
if(!mTransformListener->waitForTransform(scan_in->header.frame_id, "/map", scan_in->header.stamp + ros::Duration().fromSec(scan_in->ranges.size()*scan_in->time_increment), ros::Duration(1.0)))
{
return;
}
//project to a sensor_msgs::PointCloud2 message
//for usage of the pcl library and forward compatibility
PointCloud2 pointCloud;
try
{
mLaserProjector.transformLaserScanToPointCloud("/map", *scan_in, pointCloud, *mTransformListener);
}
catch (tf::TransformException& e)
{
ROS_ERROR("obstacle_detector - %s", e.what());
}
if (pointCloud.width * pointCloud.height >0)
{
pointCloud.header.stamp = ros::Time::now();
pointCloud.header.frame_id = "/map";
pubCloud.publish(pointCloud);
}
}
void Match_Filter::scanCB(const sensor_msgs::LaserScan::ConstPtr& scan){
sensor_msgs::PointCloud2 cloud_in,cloud_out;
projector_.transformLaserScanToPointCloud("/lidar_link",*scan,cloud_in,tfListener_);
rot_tf.header.frame_id = "/ChestLidar";
// lidar mx28 axis aligned mode
// rot_tf.transform.rotation.x = enc_tf_.getRotation().x();
// rot_tf.transform.rotation.y = enc_tf_.getRotation().y();
// rot_tf.transform.rotation.z = enc_tf_.getRotation().z();
// rot_tf.transform.rotation.w = enc_tf_.getRotation().w();
// lidar mx28 axis perpendicular modeg
tf::Quaternion q1;
q1.setRPY(-M_PI/2,0,0);
tf::Transform m1(q1);
tf::Quaternion q2(enc_tf_.getRotation().x(),enc_tf_.getRotation().y(),enc_tf_.getRotation().z(),enc_tf_.getRotation().w());
tf::Transform m2(q2);
tf::Transform m4;
m4 = m2*m1; // rotate lidar axis and revolute mx28 axis
rot_tf.transform.rotation.x = m4.getRotation().x();
rot_tf.transform.rotation.y = m4.getRotation().y();
rot_tf.transform.rotation.z = m4.getRotation().z();
rot_tf.transform.rotation.w = m4.getRotation().w();
tf2::doTransform(cloud_in,cloud_out,rot_tf);
point_cloud_pub_.publish(cloud_out);
moving_now.data = dxl_ismove_;
dxl_ismove_pub_.publish(moving_now);
}
void LaserscanMerger::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan, std::string topic)
{
sensor_msgs::PointCloud tmpCloud1,tmpCloud2;
sensor_msgs::PointCloud2 tmpCloud3;
// Verify that TF knows how to transform from the received scan to the destination scan frame
tfListener_.waitForTransform(scan->header.frame_id.c_str(), destination_frame.c_str(), scan->header.stamp, ros::Duration(1));
projector_.transformLaserScanToPointCloud(scan->header.frame_id, *scan, tmpCloud1, tfListener_);
try
{
tfListener_.transformPointCloud(destination_frame.c_str(), tmpCloud1, tmpCloud2);
}catch (tf::TransformException ex){ROS_ERROR("%s",ex.what());return;}
for(int i=0; i<input_topics.size(); ++i)
{
if(topic.compare(input_topics[i]) == 0)
{
sensor_msgs::convertPointCloudToPointCloud2(tmpCloud2,tmpCloud3);
pcl_conversions::toPCL(tmpCloud3, clouds[i]);
clouds_modified[i] = true;
}
}
// Count how many scans we have
int totalClouds = 0;
for(int i=0; i<clouds_modified.size(); ++i)
if(clouds_modified[i])
++totalClouds;
// Go ahead only if all subscribed scans have arrived
if(totalClouds == clouds_modified.size())
{
pcl::PCLPointCloud2 merged_cloud = clouds[0];
clouds_modified[0] = false;
for(int i=1; i<clouds_modified.size(); ++i)
{
pcl::concatenatePointCloud(merged_cloud, clouds[i], merged_cloud);
clouds_modified[i] = false;
}
point_cloud_publisher_.publish(merged_cloud);
Eigen::MatrixXf points;
getPointCloudAsEigen(merged_cloud,points);
pointcloud_to_laserscan(points, &merged_cloud);
}
}
void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
if(!listener_.waitForTransform(
scan_in->header.frame_id,
base_frame,
scan_in->header.stamp + ros::Duration().fromSec(scan_in->ranges.size()*scan_in->time_increment),
ros::Duration(1.0))){
return;
}
sensor_msgs::PointCloud2 cloud2;
projector_.transformLaserScanToPointCloud(base_frame,*scan_in,
cloud2,listener_);
scanPub.publish(cloud2);
}
void LaserMerger::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan, std::string topic)
{
sensor_msgs::PointCloud tmpCloud1,tmpCloud2;
tfListener_.waitForTransform(scan->header.frame_id.c_str(), destination_frame.c_str(), scan->header.stamp, ros::Duration(1));
projector_.transformLaserScanToPointCloud(scan->header.frame_id, *scan, tmpCloud1, tfListener_);
try
{
tfListener_.transformPointCloud(destination_frame.c_str(), tmpCloud1, tmpCloud2);
}catch (tf::TransformException ex){ROS_ERROR("%s",ex.what());return;}
for(int i=0; i<input_topics.size(); ++i)
{
if(topic.compare(input_topics[i]) == 0)
{
sensor_msgs::convertPointCloudToPointCloud2(tmpCloud2,clouds[i]);
clouds_modified[i] = true;
}
}
int totalClouds = 0;
for(int i=0; i<clouds_modified.size(); ++i)
if(clouds_modified[i])
++totalClouds;
if(totalClouds == clouds_modified.size())
{
sensor_msgs::PointCloud2 merged_cloud = clouds[0];
clouds_modified[0] = false;
for(int i=1; i<clouds_modified.size(); ++i)
{
pcl::concatenatePointCloud(merged_cloud, clouds[i], merged_cloud);
clouds_modified[i] = false;
}
point_cloud_publisher_.publish(merged_cloud);
Eigen::MatrixXf points;
getPointCloudAsEigen(merged_cloud,points);
pointcloud_to_laserscan(points, &merged_cloud);
}
}
void ScanCallback (const sensor_msgs::LaserScan::ConstPtr& msg){
for(int i=0; i<1080; i=i+4){
histlaser[(int)floor(i/4)]=msg->ranges[i];
if(histlaser[i/4]>msg->range_max || histlaser[i/4]<msg->range_min) histlaser[i/4]=0.0;
}
static tf::TransformListener listener;
static laser_geometry::LaserProjection projector;
try{
sensor_msgs::PointCloud cloud;
projector.transformLaserScanToPointCloud("map",*msg, cloud, listener);
// Do something with cloud.
drawData.laserScan.clear();
for(unsigned int i=0; i<cloud.points.size(); i=i+4) drawData.laserScan.push_back(toimage(cloud.points[i].x,cloud.points[i].y));
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
}
void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
sensor_msgs::PointCloud cloud;
try
{
projector_.transformLaserScanToPointCloud(
"base_link",*scan_in, cloud,listener_);
}
catch (tf::TransformException& e)
{
std::cout << e.what();
return;
}
// Do something with cloud.
scan_pub_.publish(cloud);
}
void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan, tf::TransformListener* listener) {
ROS_DEBUG("get scan data");
string fid = scan->header.frame_id;
ROS_DEBUG_STREAM("scan->frame_id:" << fid << " reference frame_id:" << ref_frame_id);
try {
// LRFの最後の光線の時刻で変換できるようになるまで待つ
bool is_timeout = !listener->waitForTransform(
ref_frame_id, fid,
scan->header.stamp + ros::Duration().fromSec(scan->ranges.size() * scan->time_increment),
ros::Duration(3.0));
if(is_timeout){
ROS_WARN("waitForTransform timeout");
return;
}
} catch (tf::TransformException& ex) {
ROS_WARN("%s", ex.what());
return;
}
sensor_msgs::PointCloud cloud;
static laser_geometry::LaserProjection projector_;
try{
projector_.transformLaserScanToPointCloud(ref_frame_id, *scan, cloud, *listener);
} catch (tf::TransformException& ex) {
ROS_WARN("%s", ex.what());
return;
}
// pclに変換できた点のLaserScanでのインデックス配列が格納されている,channelsインデックス
int ch_i = -1;
for (int i = 0; i < cloud.channels.size(); i++) {
if ("index" == cloud.channels[i].name) {
ch_i = i;
break;
}
}
if (-1 == ch_i) {
ROS_ERROR("There is not 'index' channel in PointCloud converted from LaserScan");
}
// 四角形内のインデックスを記録
vector<int> rip_idx;
for (int i = 0; i < cloud.points.size(); i++) {
float x = cloud.points.at(i).x;
float y = cloud.points.at(i).y;
if (xmin < x && x < xmax && ymin < y && y < ymax) {
int tmp = (int)cloud.channels[ch_i].values[i];
rip_idx.push_back(tmp);
}
}
// インデックスに登録された点を削除，無効な距離を設定
sensor_msgs::LaserScan scan_out = *scan;
for (vector<int>::iterator it = rip_idx.begin(); it < rip_idx.end(); ++it) {
int i = (int)*it;
scan_out.ranges.at(i) = scan_out.range_max + 1.0;
}
pub.publish(scan_out);
}
void ConvertLaser(const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
sensor_msgs::LaserScan temp = *scan_in;
sensor_msgs::LaserScan* scan = &temp;
// Remove points that are to close and or the boat itself
// The LaserProjection class only removes points that are to far away
// It does not remove points that are to close
// To workaround this we iterate through the raw laser scan and change all values
// that are minimum value to above maximum value
float angle = scan->angle_min;
for(std::vector<float>::iterator it = scan->ranges.begin(); it != scan->ranges.end(); ++it)
{
// 1 cm epsilon
if (*it <= scan->range_min + 0.01 || isSelf(*it, angle))
{
*it = scan->range_max + 0.01;
}
// Increment
angle += scan->angle_increment;
}
//Convert to point cloud
sensor_msgs::PointCloud2 cloud; //Define a new pc msg
bool transform_ready = false;
try
{
//Wait until available
if(transform_.waitForTransform(
scan->header.frame_id, //Transform from lidar
frame_.c_str(), //to frame_
//Most recent time (start_time + number_of_points * time_increment)
scan->header.stamp + ros::Duration().fromSec(scan->ranges.size() * scan->time_increment),
ros::Duration(2))) //Timeout after 2 seconds
{
transform_ready = true;
}
else
{
ROS_WARN("Transform from %s to %s timed out", scan->header.frame_id.c_str(), frame_.c_str());
}
}
catch (tf::TransformException& e)
{
ROS_ERROR("Transform error from %s to %s: %s", scan->header.frame_id.c_str(), frame_.c_str(), e.what());
}
//If we can transform then do it!
if(transform_ready)
{
//Project laser scan into a point cloud in frame_
try // It is possible that some of the transforms are to far into the past
{
projector_.transformLaserScanToPointCloud(frame_.c_str(), *scan, cloud, transform_);
}
catch(tf::TransformException& e)
{
ROS_ERROR("Transform error from %s to %s: %s", scan->header.frame_id.c_str(), frame_.c_str(), e.what());
}
//Publish the cloud
pc_pub_.publish(cloud);
}
}
/***
*\brief Callback function that collects all scans and converts to pointcloud2
*\param LaserScan
*
*/
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_ptr) {
sensor_msgs::LaserScan _scan_in = *scan_ptr;
if (_scan_in.header.frame_id.compare("/laser") != 0) {
ROS_WARN("An unknown source: %s", _scan_in.header.frame_id);
return;
}
if (maxToProducePcdCount > 0 && producedPcdCount >= maxToProducePcdCount) {
//ROS_WARN("Already %d point cloud produced", maxToProducePcdCount);
bRunning = false;
return;
}
ros::Time originalTime = _scan_in.header.stamp;
_scan_in.header.stamp = originalTime - ros::Duration(0.05); // hack: Verzoegerung
if (!_tfListener.waitForTransform( /* wait for possible TF */
target_frame,
_scan_in.header.frame_id,
_scan_in.header.stamp, ros::Duration(0.01)))
{
resetCloudCollectionOnError();
ROS_WARN("Timeout with waitForTransform");
return;
}
sensor_msgs::PointCloud2 sub_cloud;
tf::StampedTransform transform;
try { /* transform laser scan to a sub_cloud */
_laserProjector.transformLaserScanToPointCloud(
target_frame,
_scan_in,
sub_cloud,
_tfListener,
-1.0, laser_geometry::channel_option::Default);
} catch (tf::TransformException& exc) {
resetCloudCollectionOnError();
ROS_ERROR("Failed with transformLaserScanToPointCloud: %s", exc.what());
return;
}
try {
//Get the searched TF. Throws exception on failure.
_tfListener.lookupTransform(_scan_in.header.frame_id,
target_frame, _scan_in.header.stamp, transform);
} catch (tf::TransformException &ex) {
resetCloudCollectionOnError();
ROS_ERROR("Failed with lookupTransform %s\n", ex.what()); //Print exception which was caught
return;
}
if (!isSkipThisCloud) {
sensor_msgs::PointCloud2 tmp_cloud = _resulting_cloud;
pcl::concatenatePointCloud(tmp_cloud, sub_cloud, _resulting_cloud);
}
float roll,pitch,yaw;
getRPY(transform.getRotation(),roll,pitch,yaw);
const double current_yaw = yaw;
bool isCrossZeroIndicator = false;
if ((0 < _the_last_2nd_yaw
&& 0 < _the_last_1st_yaw
&& 0 > current_yaw) ||
(0 > _the_last_2nd_yaw
&& 0 > _the_last_1st_yaw
&& 0 < current_yaw)) {
isCrossZeroIndicator = true;
}
if (isCrossZeroIndicator) {
// we have now a possible complete scan !
if(!isSkipThisCloud)
{
producedPcdCount++;
ROS_INFO("Collected %d cloud with size [%d,%d]", producedPcdCount, _resulting_cloud.width, _resulting_cloud.height);
_resulting_cloud.header.stamp = _scan_in.header.stamp;
_scan_pub.publish(_resulting_cloud);
pcl::PointCloud<pcl::PointXYZI> cloud;
pcl::fromROSMsg(_resulting_cloud, cloud);
time_t rawtime;
time(&rawtime);
std::stringstream newtimestamp;
newtimestamp << pcd_prefix << "_" << rawtime;
//pcd/" + newtimestamp.str() + "
std::string strStamp = "pcd/" + newtimestamp.str() + "_lms111.pcd";
pcl::io::savePCDFileASCII (strStamp, cloud);
} else {
ROS_WARN("Skipped incomplete cloud due to some error data");
}
isSkipThisCloud = false;
_start_yaw = current_yaw;
_resulting_cloud = sub_cloud; // reset
}
_the_last_2nd_yaw = _the_last_1st_yaw;
_the_last_1st_yaw = current_yaw;
}