Ask Your Question
3

pointcloud to laserscan with transform?

asked 2011-04-02 06:35:30 -0600

Bart gravatar image

updated 2016-10-24 08:33:42 -0600

ngrennan gravatar image

Has anyone written a nodelet that can be applied between the cloud_throttle nodelet and the cloud_to_scan nodelet in the pointcloud_to_laserscan package in the turtlebot stack? This nodelet would be used to provide a horizontal laserscan relative to the base_footprint frame when the Kinect is tilted downwards to provide a better view of the ground area just in front of the robot. (or is there a better way to accomplish the same objective?)

edit retag flag offensive close merge delete

3 Answers

Sort by » oldest newest most voted
2

answered 2011-04-07 16:51:38 -0600

Bart gravatar image

updated 2011-05-30 17:31:45 -0600

With the help of others I have developed a nodelet to display a horizonal projection of a laserscan with a pan/tilt Kinect camera. This nodelet is intended to work with the other pointcloud_to_laserscan nodelets, providing a choice of two laserscan projection types. Here is the source code with detailed instructions appended at the end ...


SOURCE cloud_to_scanHoriz.cpp: /*
* Copyright (c) 2010, 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, Inc. 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 "ros/ros.h"
#include "pluginlib/class_list_macros.h"
#include "nodelet/nodelet.h"
#include "sensor_msgs/LaserScan.h"
#include "pcl/point_cloud.h"
#include "pcl_ros/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl/ros/conversions.h"
#include "pcl_ros/transforms.h"
#include "tf/transform_listener.h"
#include "tf/message_filter.h"
#include "message_filters/subscriber.h"


namespace pointcloud_to_laserscan
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

class CloudToScanHoriz : public nodelet::Nodelet
{
public:
  //Constructor
  CloudToScanHoriz(): min_height_(0.10), max_height_(0.75), baseFrame("/base_footprint"), laserFrame("/camera_tower")
  {
  };

private:
  double min_height_, max_height_;
  std::string baseFrame;  //ground plane referenced by laser
  std::string laserFrame; //pan frame simulating projected laser
  bool result;

  tf::TransformListener tfListener;
  message_filters::Subscriber<PointCloud> cloudSubscriber;
  tf::MessageFilter<PointCloud> *tfFilter;

  ros::Publisher laserPublisher;

  //Nodelet initialization
  virtual void onInit()
  {
    ros::NodeHandle& nh = getNodeHandle();
    ros::NodeHandle& private_nh = getPrivateNodeHandle();

    private_nh.getParam("min_height", min_height_);
    private_nh.getParam("max_height", max_height_);
    private_nh.getParam("base_frame", baseFrame);
    private_nh.getParam("laser_frame", laserFrame);
    NODELET_INFO("CloudToScanHoriz min_height: %f, max_height: %f", min_height_, max_height_);
    NODELET_INFO("CloudToScanHoriz baseFrame: %s, laserFrame: %s", baseFrame.c_str(), laserFrame.c_str());

    //Set up to process new pointCloud and tf data together
    cloudSubscriber.subscribe(nh, "cloud_in", 5);
    tfFilter = new tf::MessageFilter<PointCloud>(cloudSubscriber, tfListener, laserFrame, 1);
    tfFilter->registerCallback(boost::bind(&CloudToScanHoriz::callback, this, _1));
    tfFilter->setTolerance(ros::Duration(0.01));

    laserPublisher = nh.advertise<sensor_msgs::LaserScan>("laserScanHoriz", 10);
  };

  //Pointcloud and tf transform received
  void callback(const PointCloud::ConstPtr& cloud_in)
  {
    PointCloud cloudTransformed;
    sensor_msgs::LaserScanPtr ...
(more)
edit flag offensive delete link more
2

answered 2011-04-03 23:19:23 -0600

I made a modified version of pointcloud_to_laserscan to do this. It can be found at:

http://bazaar.launchpad.net/~fuzzgun/grok2/trunk/files/head:/fakelaser/

It seems better to do the transform at the throttling stage, so that the resulting point cloud can be visualised using X, Y or Z color filters within Rviz.

Some example launch file entries:

<!-- throttling: limit the point cloud data update rate to produce a fake laser scan -->
<node pkg="nodelet" type="nodelet" name="pointcloud_throttle" args="load fakelaser/CloudThrottle openni_manager" respawn="true">
    <param name="max_rate" value="$(arg fake_laser_frequency)"/>
    <param name="frame_id" value="base_link"/>
    <remap from="cloud_in" to="/camera/depth/points"/>
    <remap from="cloud_out" to="cloud_throttled"/>
</node>

<!-- Fake Laser -->
<node pkg="nodelet" type="nodelet" name="kinect_laser" args="load fakelaser/CloudToScan openni_manager" respawn="true">
    <param name="output_frame_id" value="/kinect_fake_laser"/>
    <remap from="cloud" to="/cloud_throttled"/>
    <param name="min_height" value="0.2"/>
    <param name="max_height" value="2.0"/>
</node>
edit flag offensive delete link more

Comments

Thanks for the link to your source. Where is the output frame "/kinect_fake_laser" on your robot. (does it matter?) Is the second transform in cloud_to_scan referencing an imaginary laserScanner? I get a laserscan representation in rviz, but it is not contained in the pointCloud as expected.
Bart gravatar imageBart ( 2011-04-04 14:35:45 -0600 )edit
Further inspection reveals that the laserscan is flipped left to right, as in rotate 180 degrees about the axis moving forward away from the scanner/robot.
Bart gravatar imageBart ( 2011-04-04 14:44:36 -0600 )edit
I forgot to mention that your code does a great job of maintaining the laserscan when I tilt my Kinect downwards using the base (my original question) I expect that I can just reverse a variable in cloud_to_scan, but I will try to understand first why the scan is inverted.
Bart gravatar imageBart ( 2011-04-04 14:53:32 -0600 )edit
Don't know why my version is inverted, but in cloud_to_scan the easy fix is changing "double angle = -atan2(side,fwd);" to "double angle = atan2(side,fwd);" (removed minus sign). Everything is now working.
Bart gravatar imageBart ( 2011-04-04 15:18:27 -0600 )edit
Likewise what I'm doing is panning and tilting the Kinect, whilst the fake laser simulated a laser scanner in a fixed position at the base of the robot. You can deal with inversions by changing the joint rpy information to your fake laser frame within your URDF model.
JediHamster gravatar imageJediHamster ( 2011-04-04 22:30:00 -0600 )edit
0

answered 2011-04-02 17:47:44 -0600

Bart gravatar image

Okay, I'm getting closer as I have added a third nodelet to the turtlebot pointcloud_to_laserscan package for the pointCloud tilt function, but I am stuck on a boost shared pointer that I can't pass directly into the transformPointCloud function in the PointCloud subscriber callback ...

  void callback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud_in)
  {
    pcl::PointCloud<pcl::PointXYZ> cloud_out;
    //pcl::PointCloud<pcl::PointXYZ> cloud_test;
    std::string tgtFrame = "base_footprint";
    NODELET_DEBUG("CloudTilt callback");
    try
     {
       result = pcl_ros::transformPointCloud(tgtFrame, cloud_in, cloud_out, tfListener);
     }
     catch (tf::TransformException& e)
     {
        std::cout << e.what();
        return;
     }
     //Publish transformed pointCloud
     pub_.publish(cloud_out);
  } //callback

Error message: /opt/ros/dturtle/turtlebot/pointcloud_to_laserscan/src/cloud_tilt.cpp: In member function ‘void pointcloud_to_laserscan::CloudTilt::callback(const boost::shared_ptr<const pcl::pointcloud<pcl::pointxyz=""> >&)’: /opt/ros/dturtle/turtlebot/pointcloud_to_laserscan/src/cloud_tilt.cpp:87: error: no matching function for call to ‘transformPointCloud(std::string&, const boost::shared_ptr<const pcl::pointcloud<pcl::pointxyz=""> >&, pcl::PointCloud<pcl::pointxyz>&, tf::TransformListener&)’

Any suggestions on dealing with the boost pointer to reference conversions would be appreciated.

I also read that for this to be successful I need to use message_filters to buffer the tf data?

edit flag offensive delete link more

Comments

How about dereferencing the boost shared pointer: transformPointCloud(tgtFrame, *cloud_in, cloud_out, tfListener)?
fergs gravatar imagefergs ( 2011-04-03 09:08:16 -0600 )edit
Yes, that works, but I get into message timing problems as suspected. I have added the tf::MessageFilter code and roscomm message_filter code, but my callback is not getting called.
Bart gravatar imageBart ( 2011-04-03 10:27:14 -0600 )edit
message_filters::Subscriber<PointCloud> cloudSubscriber(nh, "cloud_in", 5); tf::MessageFilter<PointCloud> tfFilter(cloudSubscriber, tfListener, "/base_footprint", 10); tfFilter.registerCallback(&CloudTilt::callback, this);
Bart gravatar imageBart ( 2011-04-03 10:28:13 -0600 )edit
The example in the tf::MessageFilter documentation doesn't use a boost::Bind for the registerCallback call, the example in laser_pipeline/Tutorials LaserScanToPointCloud does. More study required.
Bart gravatar imageBart ( 2011-04-03 10:35:30 -0600 )edit
I have solved all of the message filtering/timing problems and now transformPointCloud is functioning. My next problem is confusion regarding the appropriate tf frame to specify for transforming the throttled Kinect pointcloud before "vertical thinning" in the cloud_to_scan nodelet.
Bart gravatar imageBart ( 2011-04-03 16:23:26 -0600 )edit
I will post the code and launch file when I get this working properly.
Bart gravatar imageBart ( 2011-04-03 16:24:28 -0600 )edit
Bart, this is great. The frame into which you want to transform is usually something like base_link, but it depends on your robot setup. Also, the existing implementation is working in the optical link which is Z axis forward. You probably want to change the code to work in REP103 compliant directions. http://www.ros.org/reps/rep-0103.html
tfoote gravatar imagetfoote ( 2011-04-03 17:06:44 -0600 )edit
I think my problem is that cloud_to_scan works in camera coordinates (y is down), but after transformation my tilted pointcloud is working in ROS coordinates (z is up). Changing the x,y,z indexes in cloud_to_scan may correct, but I've run out of time (and clear thinking) for today.
Bart gravatar imageBart ( 2011-04-03 17:54:15 -0600 )edit

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-04-02 06:35:30 -0600

Seen: 3,906 times

Last updated: May 30 '11