Ask Your Question
0

Hokuyo laser data to composite 3D map

asked 2011-10-14 06:04:44 -0600

tyler258 gravatar image

Hello,

I've just finished all of the laser pipeline tutorials using the laser.bag data provided by ROS. Now I want to use my Hokuyo laser to read in data and then turn that data into a 3D composite map. When I record data both the Fixed Frame and Target Frame are "/laser". What do I need to change in the source code or launch file to get this data to read in correctly to Rviz. I will provide the source code and launch file below.

Launch File

<launch>

<node type="laser_scan_assembler" pkg="assembler_test" name="my_assembler">

<remap from="scan" to="tilt_scan"/>

<param name="max_scans" type="int" value="400" />

<param name="fixed_frame" type="string" value="base_link" />

</node>

</launch>

Laser_scan_assembler.cpp

#include "laser_geometry/laser_geometry.h"
#include "sensor_msgs/LaserScan.h"
#include "laser_assembler/base_assembler.h"
#include "filters/filter_chain.h"

using namespace laser_geometry;
using namespace std ;

namespace laser_assembler
{


class LaserScanAssembler : public BaseAssembler<sensor_msgs::LaserScan>
{  
public:
  LaserScanAssembler() : BaseAssembler<sensor_msgs::LaserScan>("max_scans"), filter_chain_("sensor_msgs::LaserScan")
  {
    // ***** Set Laser Projection Method *****
    private_ns_.param("ignore_laser_skew", ignore_laser_skew_, true);

    // configure the filter chain from the parameter server
    filter_chain_.configure("filters", private_ns_);

    // Have different callbacks, depending on whether or not we want to ignore laser skews.
    if (ignore_laser_skew_)
      start("scan");
    else
    {
      start();
      skew_scan_sub_ = n_.subscribe("scan", 10, &LaserScanAssembler::scanCallback, this);
    }
  }

  ~LaserScanAssembler()
  {

  }

  unsigned int GetPointsInScan(const sensor_msgs::LaserScan& scan)
  {
    return (scan.ranges.size ());
  }

  void ConvertToCloud(const string& fixed_frame_id, const  sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud& cloud_out)
  {
    // apply filters on laser scan
    filter_chain_.update (scan_in, scan_filtered_);

    // convert laser scan to point cloud
    if (ignore_laser_skew_)  // Do it the fast (approximate) way
    {
      projector_.projectLaser(scan_filtered_, cloud_out);
      if (cloud_out.header.frame_id != fixed_frame_id)
        tf_->transformPointCloud(fixed_frame_id, cloud_out, cloud_out);
    }
    else                     // Do it the slower (more accurate) way
    {
      int mask = laser_geometry::channel_option::Intensity +
                 laser_geometry::channel_option::Distance +
                 laser_geometry::channel_option::Index +
                 laser_geometry::channel_option::Timestamp;
      projector_.transformLaserScanToPointCloud (fixed_frame_id, scan_filtered_, cloud_out, *tf_, mask);
    }
    return;
  }

  void scanCallback(const sensor_msgs::LaserScanConstPtr& laser_scan)
  {
    if (!ignore_laser_skew_)
    {
      ros::Duration cur_tolerance = ros::Duration(laser_scan->time_increment * laser_scan->ranges.size());
      if (cur_tolerance > max_tolerance_)
      {
        ROS_DEBUG("Upping tf tolerance from [%.4fs] to [%.4fs]", max_tolerance_.toSec(), cur_tolerance.toSec());
        assert(tf_filter_);
        tf_filter_->setTolerance(cur_tolerance);
        max_tolerance_ = cur_tolerance;
      }
      tf_filter_->add(laser_scan);
    }
  }

private:
  bool ignore_laser_skew_;
  laser_geometry::LaserProjection projector_;

  ros::Subscriber skew_scan_sub_;
  ros::Duration max_tolerance_;   // The longest tolerance we've needed on a scan so far

  filters::FilterChain<sensor_msgs::LaserScan> filter_chain_;
  mutable sensor_msgs::LaserScan scan_filtered_;

};

}

using namespace laser_assembler ;

int main(int argc, char **argv)
{
  ros::init(argc, argv, "laser_scan_assembler");
  LaserScanAssembler pc_assembler;
  ros::spin();

  return 0;
}
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2011-10-14 09:18:57 -0600

DimitriProsser gravatar image

You need to change the launch file from the tilt_scan value in the line <remap from="scan" to="tilt_scan"/> to the topic that the hokuyo publishes (base_scan/scan?). Also, in the launch file, it specifies the "fixed_frame". What this means is that the data coming from the scanner will be transformed into the robot's base_link frame. If you change this frame to "/laser", all of the assembled data will be displayed with respect to the laser, rather than with respect to the main body of the robot. You might want this to happen. If so, change the value of "fixed_frame" to "/laser". Otherwise, you can leave it as "base_link"

edit flag offensive delete link more
0

answered 2016-06-08 21:52:06 -0600

Shantnu gravatar image

Please see my answer at http://answers.ros.org/question/23649... .

If you still have problem, please message me..

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2011-10-14 06:04:44 -0600

Seen: 2,210 times

Last updated: Jun 08 '16