door_angle_detector.cpp

Go to the documentation of this file.

#include "door_angle_detector.h"

#include "polygon_filter.h"
#include "visualize_polygon.hpp"

#include <std_msgs/Float64.h>
#include <std_msgs/ColorRGBA.h>

#include <pcl/filters/passthrough.h>
#include <pcl/ros/conversions.h>

#include <limits> //quiet_nan
#include <cmath> //atan2


DoorAngleDetector::DoorAngleDetector() : door_frame_("/door")
{
    ros::NodeHandle nh("~");
    nh.param<std::string>("door_frame", door_frame_, door_frame_);
    ROS_DEBUG("Using door frame: %s", door_frame_.c_str());

    scan_sub_.subscribe(node_, "/base_scan", 10);
    tf_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(scan_sub_, tfListener_, door_frame_, 10);
    tf_filter_->registerCallback( boost::bind(&DoorAngleDetector::scanCallback, this, _1) );

    point_cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud2> ("door_scan_cloud", 10, false);
    door_state_publisher_  = node_.advertise<door_perception_msgs::DoorState> ("door_state", 10, false);
    vis_pub_ = node_.advertise<visualization_msgs::Marker>( "door_state_visualization", 2 );
    angle_pub_ = node_.advertise<std_msgs::Float64>( "door_angle", 2 );
    service = node_.advertiseService("detect_door_state", &DoorAngleDetector::serviceCallback, this);
    intercept = slope = std::numeric_limits<typeof(intercept)>::quiet_NaN();
    angle = std::numeric_limits<typeof(angle)>::quiet_NaN();

    cropping_polygon_.header.frame_id = door_frame_;
    PointType vertex;
    vertex.x =  0.20; vertex.y =  0.15; cropping_polygon_.points.push_back(vertex);//  +----------------+
    vertex.x = -0.05; vertex.y =  0.15; cropping_polygon_.points.push_back(vertex);//  |               /
    vertex.x = -0.50; vertex.y =  1.00; cropping_polygon_.points.push_back(vertex);//  |              /
    vertex.x =  0.95; vertex.y =  1.00; cropping_polygon_.points.push_back(vertex);//  |             /
    vertex.x =  0.95; vertex.y = -0.10; cropping_polygon_.points.push_back(vertex);//  |         +--+
    vertex.x =  0.20; vertex.y = -0.10; cropping_polygon_.points.push_back(vertex);//  +---------+
}


void DoorAngleDetector::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan){
        //scan->header.stamp.setNow(ros::Time::now());
    sensor_msgs::PointCloud2::Ptr cloud(new sensor_msgs::PointCloud2);
    last_scan_ = *scan;

    try {
      ros::Time end_time = scan->header.stamp + ros::Duration ().fromSec (scan->ranges.size () * scan->time_increment);
      tfListener_.waitForTransform("/door", scan->header.frame_id, end_time, ros::Duration(0.1), ros::Duration(0.02));
      projector_.transformLaserScanToPointCloud(door_frame_,*scan, *cloud, tfListener_);
      //Now 0,0,0 is at the door axis, 1,0,0 at tha handle in closed position, 0,1,0 at the handle in opened position
      //projector_.projectLaser(*scan, *cloud);
      //Extract the 1.5m in front of the laser scanner
      PointCloudType pcl_cloud;
      pcl::fromROSMsg(*cloud,pcl_cloud);
      cropping_polygon_.header = cloud->header;
      
      PointCloudType inside_cloud = crop_by_polygon(cropping_polygon_, pcl_cloud, 0, 1);
      if(vis_pub_.getNumSubscribers() > 0){
        cropping_polygon_.header.stamp = scan->header.stamp;//Important for visualization
        visualization_msgs::Marker polygon = visualize_polygon(cropping_polygon_);
        vis_pub_.publish(polygon);
      }
      //inside_cloud.header 

      if(inside_cloud.size() == 0){
        door_found = false;
      }
      else
      {
        geometry_msgs::Point door_axis, handle_approx;
        estimateDoorState(inside_cloud, angle, door_axis);
        //Angle wraps at +90 to -90 degrees, make it -20 to 160
        if(angle < -20.0/180.0*3.1415927){
          angle += 3.1415927;
        }
        door_found = true;
        door_axis.x = 0.0;
        door_axis.y = 0.0;
        handle_approx.x = door_axis.x + cos(angle);
        handle_approx.y = door_axis.y + sin(angle);
        //Send Door state msg
        if(door_state_publisher_.getNumSubscribers() > 0){
          door_perception_msgs::DoorState ds;
          ds.header = cloud->header;; 
          ds.door_found = true;
          ds.angle = angle;
          door_state_publisher_.publish(ds);
        }

        ROS_DEBUG_THROTTLE(1, "Computed door state: %.1f degree to X axis (of %s)", angle*180.0/3.1415927, scan->header.frame_id.c_str());
        ROS_DEBUG_THROTTLE(1, "Current door axis estimate: (%.2f, %.2f)", door_axis.x, door_axis.y);
        ROS_DEBUG_THROTTLE(1, "Current door handle estimate: (%.2f, %.2f)", handle_approx.x, handle_approx.y);
        //Visualize
        if(vis_pub_.getNumSubscribers() > 0){
          visualization_msgs::Marker door = visualize_door(door_axis, handle_approx, cloud->header);
          vis_pub_.publish(door);
        }
        //For debugging, send cropped cloud
        if(point_cloud_publisher_.getNumSubscribers() > 0){
          pcl::toROSMsg(inside_cloud, *cloud);
          point_cloud_publisher_.publish(*cloud);
        }
      }
    }
    catch (tf::TransformException ex){
      ROS_WARN("%s",ex.what());
    }
}

//Computes the (smaller) angle to the Y axis
void DoorAngleDetector::estimateDoorState(const PointCloudType & pcl_cloud , double& angle_out, geometry_msgs::Point& axis_position_out){
    if(pcl_cloud.size() == 0){
      ROS_INFO("Cannot estimate door angle from empty cloud. Open?");
      return;
    }

    //Numerically accurate line fitting code from: www.johndcook.com/blog/2008/10/20/comparing-two-ways-to-fit-a-line-to-data/
    double sumx = 0.0, sumy = 0.0, sum_delta_sq = 0.0, sts = 0.0;
    int n = pcl_cloud.size();
    for (int i = 0; i < n; ++i)
    {
      sumx += pcl_cloud.points[i].x;
      sumy += pcl_cloud.points[i].y;
    }
    for (int i = 0; i < n; ++i)
    {
      double delta_x_from_mean = pcl_cloud.points[i].x - sumx/n;
      sum_delta_sq += delta_x_from_mean*delta_x_from_mean;
      sts += delta_x_from_mean*pcl_cloud.points[i].y;
    }

    angle_out = atan2(sts, sum_delta_sq);//y and x equivalent, see below
    
    //Compute axis position if previous door
    double new_slope = sts/sum_delta_sq; //Slope is also: y/x
    double new_intercept = (sumy - sumx*new_slope)/n;
    if(!isnan(slope) && !isnan(intercept)){
      double dslope = slope - new_slope;
      double dintercept = new_intercept - intercept;
      if(dslope != 0){
        axis_position_out.x = dintercept/dslope;
        axis_position_out.y = new_slope*axis_position_out.x + new_intercept;
      }
    }
    else{
      slope = new_slope;
      intercept = new_intercept;
    }
}

//Send out last result
bool DoorAngleDetector::serviceCallback(door_perception_msgs::DoorStateSrv::Request& request, door_perception_msgs::DoorStateSrv::Response& response){
    response.state.door_found =door_found;      
    response.state.angle = angle;       
    return true;
}

visualization_msgs::Marker visualize_door(geometry_msgs::Point p1, geometry_msgs::Point p2, std_msgs::Header header)
{
  visualization_msgs::Marker door;
  door.header = header;
  door.id = 2;
  door.type = visualization_msgs::Marker::LINE_LIST;
  door.action = visualization_msgs::Marker::ADD;
  door.pose.orientation.w = 1.0;
  door.scale.x = 0.02;
  door.color.a = 0.5;
  door.color.r = 0.5;
  door.color.g = 0.5;

  std_msgs::ColorRGBA c1, c2;
  c1.a = c2.a = 0.5;
  c1.r = 1.0;
  c2.b = 1.0;
  door.colors.push_back(c1);
  door.colors.push_back(c2);

  door.points.push_back(p1);
  door.points.push_back(p2);
  return door;
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "door_state");
    DoorAngleDetector doorAngleDetector;
    ros::Duration(1.0).sleep();//get tf data
    ros::spin();
    return 0;
}