C++ laser_geometry::LaserProjection类代码示例

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 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);
		}
	}

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);
}

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 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);
	 }

void My_Filter::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan){
    sensor_msgs::PointCloud2 raw_cloud_msg;
    sensor_msgs::PointCloud2 local_cloud_msg;
    sensor_msgs::PointCloud2 global_cloud_msg;
    PointCloudPtr_t scale_cloud(new PointCloud_t());
    PointCloudPtr_t local_cloud(new PointCloud_t());
    PointCloudPtr_t global_cloud(new PointCloud_t());
    projector_.projectLaser(*scan, raw_cloud_msg);
    pcl::fromROSMsg(raw_cloud_msg, *scale_cloud);
    
    for(int i=0; i<scale_cloud->size(); i++)
    {
      pcl::PointXYZ& point = scale_cloud->at(i);
      point.z = std::floor(point.z);
    }
    
    this->filterGlobal(scale_cloud, global_cloud);
    this->filterLocal(scale_cloud,  local_cloud);
    
    pcl::toROSMsg(*global_cloud, global_cloud_msg);
    pcl::toROSMsg(*local_cloud,  local_cloud_msg);
    
    this->local_cloud_publisher_.publish(local_cloud_msg);
    this->global_cloud_publisher_.publish(global_cloud_msg);
    
}

//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 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::ConstPtr& msg)
{
  sensor_msgs::LaserScan scanData = *msg;

  auto rangeIsValid = [&scanData](float range) {
    return !std::isnan(range) && range > scanData.range_min && range < scanData.range_max;
  };

  for (unsigned int i = 0; i < scanData.ranges.size(); i++)
  {
    float& range = scanData.ranges[i];
    if (range > scanData.range_max || range < scanData.range_min)
      continue;
    // Too close
    if (range < min_dist)
      range = scanData.range_max + 1;
    // No valid neighbors
    else if ((i == 0 || !rangeIsValid(scanData.ranges[i - 1])) &&
             (i == (scanData.ranges.size() - 1) || !rangeIsValid(scanData.ranges[i + 1])))
      range = scanData.range_max + 1;
    // No close neighbors
    else if ((i == 0 || abs(scanData.ranges[i - 1] - range) > neighbor_dist) &&
             (i == (scanData.ranges.size() - 1) || abs(scanData.ranges[i + 1] - range) > neighbor_dist))
      range = scanData.range_max + 1;
  }

  sensor_msgs::PointCloud2 cloud;
  projection.projectLaser(scanData, cloud);
  cloud.header.frame_id = "/lidar";

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_for_pub(new pcl::PointCloud<pcl::PointXYZ>());
  fromROSMsg(cloud, *cloud_for_pub);
  _pointcloud_pub.publish(*cloud_for_pub);
}

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 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 laser_receive(const sensor_msgs::LaserScan::ConstPtr& reading) {
    sensor_msgs::PointCloud cloud;
    projector_.projectLaser(*reading, cloud);
	adjustByIMUData(cloud);

	std::vector<point2d_t> target;
    
	for(int i = 0; i < cloud.points.size(); ++i) {
		point2d_t pt;
		pt.x = cloud.points[i].x;
		pt.y = cloud.points[i].y;
		target.push_back(pt);
	} 

	// build tree for search
	if(reference.size() != 0) {
		tree2d_t *tree = tree2d_build(reference.begin(), reference.end(), X, depth_of_tree);
		if (!tree) {
			std::cout << "failed!" << std::endl;
			return;
		}

		reference = target;

		// find pose
		pose_t pose;
		pose.p.x = 0;
		pose.p.y = 0;
		pose.yaw = 0;
		pose = icp_align(tree, target, pose, iterations);
		std::cout << pose << std::endl;
		
		pose_estimator::pose_estimator_msg outMsg;
		outMsg.x = pose.p.x;
		outMsg.y = pose.p.y;
		outMsg.yaw = pose.yaw;
		
		pub.publish(outMsg);
		
		// free tree
		tree2d_free(&tree);
		if (tree) {
			std::cout << "failed!" << std::endl;
			return;
		}
	} else {
		reference = target;
	}
}

void callback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
	sensor_msgs::LaserScan scan_filtered=*scan_in;
	int total_points=(scan_filtered.angle_max-scan_filtered.angle_min)/scan_filtered.angle_increment;
	for (int i=0;i<total_points;i++)
	{
		if(scan_filtered.ranges[i]==0)
		{		
			scan_filtered.ranges[i]=5;
		}
	}
	projector_.projectLaser(scan_filtered,cloud);
	pcl_pub.publish(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);
        }