plane_tracker.cc

Go to the documentation of this file.

/*
 * plane_tracker.cc
 * Mac Mason <mmason@willowgarage.com>
 *
 * Fundamentally, this is a gadget for tracking a set of planes. The basic
 * operation is "listen for planes, and integrate them into the set". However,
 * we do this in a slightly goofy manner. We provide a service, which takes in
 * a set of planes ("integrate these") and returns a new set of planes ("once
 * integrated, those planes are now these planes"). The idea is for the
 * segmenter to find some planes, ship them over, and get back the planes it
 * should really be working with. 
 *
 * At the moment, this just sends back all of the planes each time; that's a
 * fairly small number.
 */

#include <vector>
#include <boost/scoped_ptr.hpp>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
#include "ros/ros.h"
#include "tf/transform_listener.h"
#include "pcl16/point_types.h"
#include "pcl16_ros/point_cloud.h"
#include "visualization_msgs/Marker.h"
#include "semanticmodel/detailedplane.hh"
#include "semanticmodel/PlaneExchange.h"
#include "semanticmodel/Planes.h"
#include "mongo_ros/message_collection.h"
#include "semanticmodel/GetCollectionNamespace.h"

namespace mr=mongo_ros;
namespace vm=visualization_msgs;

typedef pcl16::PointXYZRGB Point;
typedef pcl16::PointCloud<Point> PointCloud;

namespace semanticmodel
{

class Node
{
public:

  void visualize(const ros::WallTimerEvent& e = ros::WallTimerEvent());
  void publishPlanes();
  bool mergePlanes();
  void savePlanesToDB();
  bool planeSrvCB(PlaneExchange::Request& req, PlaneExchange::Response& resp);
  void dropSmallAreaPolygons(double min_area);

  Node();
  ~Node();

private:

  ros::NodeHandle nh;
  ros::NodeHandle p_nh;
  
  // I/O
  ros::ServiceServer merge_srv;

  // Push our visualization messages.
  ros::Publisher vis_pub;

  // Push one cloud containing all of the plane points.
  ros::Publisher all_planes_pub;

  // The planes themselves.
  std::vector<DetailedPlane::Ptr> planes;

  // We need to be able to transform between frames in DetailedPlane. This
  // needs to be a pointer so that we don't get get a NodeHandle before
  // ros::init.
  boost::scoped_ptr<tf::TransformListener> listener;

  // Interface to db
  typedef mr::MessageCollection<Planes> PlaneCollection;
  boost::scoped_ptr<PlaneCollection> plane_coll;

  // How many planes have been saved to the db so far
  unsigned num_saved_planes;
  
  // Timer to visualize.  Make it a WallTimer so it'll run even if a 
  // bag is paused.
  ros::WallTimer visualize_timer;
};

// Send out our visualization message.
void Node::visualize(const ros::WallTimerEvent& e)
{
  if (planes.size() == 0)
    return;

  visualization_msgs::Marker marker;
  marker.header.frame_id = planes[0]->cloud->header.frame_id;
  marker.header.stamp = planes[0]->cloud->header.stamp;
  
  marker.id = 0;
  marker.ns = "planes";
  marker.type = visualization_msgs::Marker::LINE_LIST;
  marker.action = visualization_msgs::Marker::ADD;
  marker.scale.x = 0.01;
  marker.color.r = marker.color.g = marker.color.b = marker.color.a = 1.0;

  foreach(DetailedPlane::Ptr plane, planes)
  {
    size_t n = plane->hull->points.size();
    for (size_t i = 0 ; i < n ; ++i)
    {
      size_t j;
      if (i > 0)
        j = i - 1;
      else
        j = n - 1;

      marker.points.push_back(geometry_msgs::Point());
      marker.points.back().x = plane->hull->points[j].x;
      marker.points.back().y = plane->hull->points[j].y;
      marker.points.back().z = plane->hull->points[j].z;

      marker.points.push_back(geometry_msgs::Point());
      marker.points.back().x = plane->hull->points[i].x;
      marker.points.back().y = plane->hull->points[i].y;
      marker.points.back().z = plane->hull->points[i].z;

      marker.colors.push_back(plane->color);
      marker.colors.push_back(plane->color);
    }
  }
  vis_pub.publish(marker);
}

// Push a message of all of our plane points.
void Node::publishPlanes()
{
  if (planes.size() == 0)
    return;

  PointCloud::Ptr all(new PointCloud());
  all->header.frame_id = planes[0]->cloud->header.frame_id;
  foreach(DetailedPlane::Ptr plane, planes)
  {
    *all += *plane->cloud;
  }
  all_planes_pub.publish(all);
}

// This works like our object-merging algorithm from the object tracker.
// Basically, here's the plan: the plane-to-be-merged is the last element of
// planes. We check for things we need to merge; if we merge something, we put
// it at the back, and return true. Otherwise, we return false. See
// plane_srv_cb for how to use this.
bool Node::mergePlanes()
{
  // Skip the last one.
  for (size_t i = 0 ; i < planes.size() - 1 ; ++i)
  {
    if (planes[i]->overlaps(*planes.back()))
    {
      planes[i]->mergeFrom(*planes.back());
      // Our new one has been absorbed.
      planes.pop_back();
      // Restore our invariant.
      std::swap(planes[i], planes.back());
      return true;
    }
  }
  return false;
}

Plane convertToMsg (DetailedPlane::Ptr dp)
{
  return dp->toPlaneMsg();
}

void Node::savePlanesToDB ()
{
  // First get the existing set so we can delete it later
  typedef mr::MessageWithMetadata<Planes>::ConstPtr PlanesPtr;
  std::vector<PlanesPtr> existing = plane_coll->pullAllResults(mr::Query());

  // Next, generate a ros message for the current set and save it
  Planes planes_msg;
  transform(planes.begin(), planes.end(), back_inserter(planes_msg.planes),
            convertToMsg);
  plane_coll->insert(planes_msg);

  // Remove the old one if necessary
  if (existing.size()>0)
  {
    ROS_ASSERT(existing.size()==1);
    plane_coll->removeMessages(existing[0]->metadata);
  }
}

bool Node::planeSrvCB(PlaneExchange::Request& req,
                PlaneExchange::Response& res)
{
  ROS_DEBUG_NAMED("plane_tracker",
                  "Received plane update request with %zu planes",
                  req.in_planes.planes.size());
  // Here's the copy. We do a push_back because our merging algorithm assumes
  // the plane-to-be-merged is at the back.
  foreach(Plane& p, req.in_planes.planes)
  {
    planes.push_back(
        DetailedPlane::Ptr(new DetailedPlane(p, listener.get())));
    // Merge them together.
    while (mergePlanes());
  }
  
  ROS_DEBUG_NAMED("plane_tracker", "After merging, have %zu planes",
                  planes.size());

  // Now, do some culling.
  dropSmallAreaPolygons(0.10);

  ROS_DEBUG_NAMED("plane_tracker", "After dropping small planes, %zu are left",
                  planes.size());

  res.out_planes.planes.resize(planes.size());
  for (size_t i = 0 ; i < planes.size() ; ++i)
  {
    planes[i]->toHullAndEquationPlaneMsg(res.out_planes.planes[i]);
  }

  visualize();
  publishPlanes();
  savePlanesToDB();
  
  ROS_DEBUG_NAMED("plane_tracker", "Plane update complete; now have %zu planes",
                  planes.size());
  return true;
}

void Node::dropSmallAreaPolygons(double min_area)
{
  std::vector<DetailedPlane::Ptr> bigplanes;
  for (size_t i = 0 ; i < planes.size() ; ++i)
  {
    if (planes[i]->area() >= min_area)
      bigplanes.push_back(planes[i]);
  }
  std::swap(planes, bigplanes);
}

Node::Node () :
  p_nh("~"), num_saved_planes(0)
{
  // Ok, to figure out which collection to store the planes in, we have
  // to call a service
  ROS_INFO_STREAM("Waiting for service get_db_name");
  GetCollectionNamespace coll_ns_srv;
  ros::service::waitForService("get_collection_namespace");
  ros::service::call("get_collection_namespace", coll_ns_srv);
  const std::string cname = coll_ns_srv.response.collection_namespace+"_planes";
  ROS_INFO_STREAM ("Db for plane tracker is " << coll_ns_srv.response.db_name
                   << "/" << cname);
  plane_coll.reset(new PlaneCollection(coll_ns_srv.response.db_name, cname));

  listener.reset(new tf::TransformListener(ros::Duration(600)));
  merge_srv = p_nh.advertiseService("plane_srv", &Node::planeSrvCB, this);
  vis_pub = p_nh.advertise<vm::Marker>("/visualization_marker", 100);

  all_planes_pub = p_nh.advertise<PointCloud>("all_planes", 100, true);

  visualize_timer = 
    nh.createWallTimer(ros::WallDuration(1.0), &Node::visualize, this);
  ROS_INFO ("Plane tracker initialized");
}

Node::~Node ()
{
}

} // namespace

int main(int argc, char** argv)
{
  ros::init(argc, argv, "filter");
  semanticmodel::Node node;
  ros::spin();
  return 0;
}