Start roscore
and rosbag
(I'm assuming that you are playing a pre-recorded bagfile)
Then use the laser_scan_assembler.launch
file below. Of course, you can change the time buffer and maximum number of scans. I use the odom
frame, but you can use map
, if you prefer that!
<launch>
<node pkg="laser_assembler" type="laser_scan_assembler" output="screen" name="laser_scan_assembler">
<param name="tf_cache_time_secs" type="double" value="475.0" />
<param name="max_scans" type="int" value="4000000" />
<param name="ignore_laser_skew" type="bool" value="true" />
<param name="fixed_frame" type="string" value="/odom" />
</node>
</launch>
Follow this with periodic snapshotter (rosrun periodic_snapshotter periodic_snapshotter
) I use the periodic_snapshotter.cpp
file below. You can change the time buffer for the snapshotter in the timerCallback
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <cstdio>
#include <ros/ros.h>
// Services
#include "laser_assembler/AssembleScans.h"
// Messages
#include "sensor_msgs/PointCloud.h"
/***
* This a simple test app that requests a point cloud from the
* point_cloud_assembler every 80 seconds, and then publishes the
* resulting data
*/
namespace laser_assembler
{
class PeriodicSnapshotter
{
public:
PeriodicSnapshotter()
{
// Create a publisher for the clouds that we assemble
pub_ = n_.advertise<sensor_msgs::PointCloud> ("assembled_cloud", 1);
// Create the service client for calling the assembler
client_ = n_.serviceClient<AssembleScans>("assemble_scans");
// Start the timer that will trigger the processing loop (timerCallback)
timer_ = n_.createTimer(ros::Duration(80,0), &PeriodicSnapshotter::timerCallback, this);
// Need to track if we've called the timerCallback at least once
first_time_ = true;
}
void timerCallback(const ros::TimerEvent& e)
{
// We don't want to build a cloud the first callback, since we we
// don't have a start and end time yet
if (first_time_)
{
first_time_ = false;
return;
}
// Populate our service request based on our timer callback times
AssembleScans srv;
srv.request.begin ...
(more)