Revision history [back]

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 output(new sensor_msgs::LaserScan());

    try
    {
      //Transform pointcloud to new reference frame
      result = pcl_ros::transformPointCloud(baseFrame, *cloud_in, cloudTransformed, tfListener);
    }
    catch (tf::TransformException& e)
    {
      NODELET_INFO("CloudToScanHoriz failed");
      std::cout << e.what();
      return;
    }
    //NODELET_DEBUG("Got cloud");

    //Setup laserscan message
    output->header = cloud_in->header;
    output->header.frame_id = laserFrame;
    output->angle_min = -M_PI/2;
    output->angle_max = M_PI/2;
    output->angle_increment = M_PI/180.0/2.0;
    output->time_increment = 0.0;
    output->scan_time = 1.0/30.0;
    output->range_min = 0.45;
    output->range_max = 10.0;

    uint32_t ranges_size = std::ceil((output->angle_max - output->angle_min) / output->angle_increment);
    output->ranges.assign(ranges_size, output->range_max + 1.0);

    //"Thin" laser height from pointcloud
    for (PointCloud::const_iterator it = cloudTransformed.begin(); it != cloudTransformed.end(); ++it)
    {
      const float &x = it->x;
      const float &y = it->y;
      const float &z = it->z;

      if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )
      {
        continue;  
      }
      if (z > max_height_ || z < min_height_)
      {
        continue;
      }
      double angle = atan2(y, x);
      if (angle < output->angle_min || angle > output->angle_max)
      {
        continue;
      }
      int index = (angle - output->angle_min) / output->angle_increment;
      //Calculate hypoteneuse distance to point
      double range_sq = y*y+x*x;
      if (output->ranges[index] * output->ranges[index] > range_sq)
        output->ranges[index] = sqrt(range_sq);
    } //for it
    laserPublisher.publish(output);
  } //callback

};

PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, CloudToScanHoriz, pointcloud_to_laserscan::CloudToScanHoriz, nodelet::Nodelet);
}

INSTRUCTIONS:

Load turtlebot stack to provide original pointcloud_to_laserscan source ... hg clone https://kforge.ros.org/turtlebot/turtlebot /opt/ros/dturtle/turtlebot Copy attached source code for cloud_to_scanHoriz.cpp to turtlebot/pointcloud_to_laserscan/src/cloud_to_scanHoriz.cpp

Modify turtlebot/pointcloud_to_laserscan/nodelets.xml to include: <class name="pointcloud_to_laserscan/CloudToScanHoriz" type="pointcloud_to_laserscan::CloudToScanHoriz" base_class_type="nodelet::Nodelet"> <description> A nodelet to transform a point cloud to a base frame and then thin it to a laser scan. </description> </class>

Modify last line of turtlebot/pointcloud_to_laserscan/CMakeList.txt: rosbuild_add_library(cloud_to_scan src/cloud_to_scan.cpp src/cloud_throttle.cpp src/cloud_to_scanHoriz.cpp)

rosmake pointcloud_to_laserscan

Modify turtlebot/pointcloud_to_laserscan/launch/kinect_laser.launch: <launch> <include file="$(find openni_camera)/launch/openni_node.launch"/> <node pkg="nodelet" type="nodelet" name="openni_manager" output="screen" respawn="true" args="manager"/> <node pkg="nodelet" type="nodelet" name="pointcloud_throttle" args="load pointcloud_to_laserscan/CloudThrottle openni_manager"> <remap from="cloud_in" to="/camera/depth/points"/> <remap from="cloud_out" to="cloud_throttled"/> </node> <node pkg="nodelet" type="nodelet" name="kinect_laser" args="load pointcloud_to_laserscan/CloudToScan openni_manager" output="screen"> <remap from="cloud" to="cloud_throttled"/> </node> <node pkg="nodelet" type="nodelet" name="tilt_laser" args="load pointcloud_to_laserscan/CloudToScanHoriz openni_manager"> <remap from="cloud_in" to="cloud_throttled"/> </node> </launch>

Notes: 1) throttling nodelet cloud_in remapped to /camera/depth/points 2) base_frame link is a fixed URDF link at the front of the robot, exact name is not important. 3) The min and max height are set cover most of the robot height, relative to the base_frame link elevation. 4) laser_frame link is a fixed URDF link which will be the origin of the calculated laserscan projection, robot dependent. 5) Robot program must provide a complete tf link/joint path from the /openni optical frames to the base_frame link. 6) Kinect camera can be mounted on a pan/tilt mechanism or simply use the Kinect tilting base as long as the pan and tilt angles are exported to tf by the robot program. (add kinect_aux package to openni_kinect stack to use Kinect tilting base)

roslaunch pointcloud_to_laserscan kinect_laser.launch

Run Rviz and your robot program, including the tf broadcaster. In Rviz, set Laser Scan display to laserScan, tilting pan/tilt downwards should project a perpidicular line at floor elevation In Rviz, set Laser Scan display to laserScanHoriz, tilting pan/tilt downwards should project laser points for obstacles at the laser_frame height. Adjust actual Kinect pan control for robot, laserscan points should remain at same projected elevation, but follow the panning changes.

#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 output(new sensor_msgs::LaserScan());

    try
    {
      //Transform pointcloud to new reference frame
      result = pcl_ros::transformPointCloud(baseFrame, *cloud_in, cloudTransformed, tfListener);
    }
    catch (tf::TransformException& e)
    {
      NODELET_INFO("CloudToScanHoriz failed");
      std::cout << e.what();
      return;
    }
    //NODELET_DEBUG("Got cloud");

    //Setup laserscan message
    output->header = cloud_in->header;
    output->header.frame_id = laserFrame;
    output->angle_min = -M_PI/2;
    output->angle_max = M_PI/2;
    output->angle_increment = M_PI/180.0/2.0;
    output->time_increment = 0.0;
    output->scan_time = 1.0/30.0;
    output->range_min = 0.45;
    output->range_max = 10.0;

    uint32_t ranges_size = std::ceil((output->angle_max - output->angle_min) / output->angle_increment);
    output->ranges.assign(ranges_size, output->range_max + 1.0);

    //"Thin" laser height from pointcloud
    for (PointCloud::const_iterator it = cloudTransformed.begin(); it != cloudTransformed.end(); ++it)
    {
      const float &x = it->x;
      const float &y = it->y;
      const float &z = it->z;

      if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )
      {
        continue;  
      }
      if (z > max_height_ || z < min_height_)
      {
        continue;
      }
      double angle = atan2(y, x);
      if (angle < output->angle_min || angle > output->angle_max)
      {
        continue;
      }
      int index = (angle - output->angle_min) / output->angle_increment;
      //Calculate hypoteneuse distance to point
      double range_sq = y*y+x*x;
      if (output->ranges[index] * output->ranges[index] > range_sq)
        output->ranges[index] = sqrt(range_sq);
    } //for it
    laserPublisher.publish(output);
  } //callback

};

PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, CloudToScanHoriz, pointcloud_to_laserscan::CloudToScanHoriz, nodelet::Nodelet);
}

INSTRUCTIONS:

Modify turtlebot/pointcloud_to_laserscan/nodelets.xml to ~~include:~~ include:

  <class name="pointcloud_to_laserscan/CloudToScanHoriz" type="pointcloud_to_laserscan::CloudToScanHoriz" base_class_type="nodelet::Nodelet">
    <description>
      A nodelet to transform a point cloud to a base frame and then thin it to a laser scan.
    </description>
  </class></class>

Modify last line of turtlebot/pointcloud_to_laserscan/CMakeList.txt: rosbuild_add_library(cloud_to_scan src/cloud_to_scan.cpp src/cloud_throttle.cpp src/cloud_to_scanHoriz.cpp)

rosmake pointcloud_to_laserscan

Modify ~~turtlebot/pointcloud_to_laserscan/launch/kinect_laser.launch:~~ turtlebot/pointcloud_to_laserscan/launch/kinect_laser.launch:

<launch>
   <!-- kinect and frame ids -->
  <include file="$(find openni_camera)/launch/openni_node.launch"/>
   <!-- openni manager -->
  <node pkg="nodelet" type="nodelet" name="openni_manager" output="screen" respawn="true" args="manager"/>
   <!-- throttling -->
  <node pkg="nodelet" type="nodelet" name="pointcloud_throttle" args="load pointcloud_to_laserscan/CloudThrottle openni_manager">
    <remap from="cloud_in" to="/camera/depth/points"/>
    <remap from="cloud_out" to="cloud_throttled"/>
     <param name="max_rate" value="2"/>
  </node>
   <!-- normal laser -->
  <node pkg="nodelet" type="nodelet" name="kinect_laser" args="load pointcloud_to_laserscan/CloudToScan openni_manager" output="screen">
    <remap from="cloud" to="cloud_throttled"/>
     <param name="output_frame_id" value="/openni_depth_frame"/>
  </node>
   <!-- tilting laser -->
  <node pkg="nodelet" type="nodelet" name="tilt_laser" args="load  pointcloud_to_laserscan/CloudToScanHoriz openni_manager">
    <remap from="cloud_in" to="cloud_throttled"/>
        <param name="base_frame" value="/camera_tower"/>
    <param name="laser_frame" value="/camera_tower"/>
    <param name="min_height" value="-0.2"/>
    <param name="max_height" value="0.50"/>
  </node>
</launch></launch>

roslaunch pointcloud_to_laserscan kinect_laser.launch