ClearpathDemoExplore.cpp

Go to the documentation of this file.

// Includes
#include <algorithm>
#include <iostream>
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include "pcl/io/pcd_io.h"
#include "ClearpathDemoTools.h"
#include "geometry_msgs/Twist.h"

// Slowest permissible is 10Hz
#define MAX_RUNTIME_SECONDS 0.1f

// Typedef PCL Types
typedef pcl::PointXYZRGB PointType;
typedef pcl::PointCloud<PointType> PCLPointCloud;

// Publishers
ros::Publisher pub;
ros::Publisher pubvel;

// Variables to get from ROS Param
double lin_speed;
double ang_speed;
double cam_height;
double cam_z_trans;
double wall_buffer;
double angular_vel_correction;
int publish_visualization = 1;
int random_walk = 0;

// ROS-Independant Demo Tool
ClearpathDemoTools demo;

// PointCloud for drawing
PCLPointCloud::Ptr final(new PCLPointCloud);

// Callback from Kinect
void callback(const PCLPointCloud::ConstPtr& cloud)
{
    // Clear Result
    final->points.clear();

    // Start Timer
    ros::Time begin = ros::Time::now();

    // Convert PCL Point Cloud to our basic version
    // TODO This should be able to be done by a resize + memcpy
    PointCloud c, f;
    for (int i = 0; i < cloud->points.size(); i++)
    {
        Point p;  p.x = cloud->points[i].x; p.y = cloud->points[i].y; p.z = cloud->points[i].z; p.rgb = cloud->points[i].rgb;
        c.points.push_back(p);
    }

    // Call Demo Update and get Twist
    Twist tw2 = demo.Update(&c, &f);

    // Convert Twist to ROS-Twist msg
    geometry_msgs::Twist tw;
    tw.linear.x = tw2.linear;
    tw.angular.z = angular_vel_correction*tw2.angular;
    pubvel.publish(tw);        

    if (publish_visualization == 1)
    {
        // Convert our basic PointCloud to PCL Type
        // TODO This should be able to be done by a resize + memcpy
        for (int i = 0; i < f.points.size(); i++)
        {
            PointType p;  p.x = f.points[i].x; p.y = f.points[i].y; p.z = f.points[i].z; p.rgb = f.points[i].rgb;
            final->points.push_back(p);
        }
        
        // Publish Visualization Cloud
        pub.publish(*final);
    }

    ros::Duration duration = ros::Time::now() - begin;
    if (duration.toSec() > MAX_RUNTIME_SECONDS) {
      ROS_WARN("Iteration of vision loop took %f seconds (max is %f)", duration.toSec(), MAX_RUNTIME_SECONDS);
    }
}

// Main
int main(int argc, char** argv)
{
    ros::init (argc, argv, "idfloor_default");    

    // Get ROS Params
    ros::NodeHandle nh = ros::NodeHandle(ros::this_node::getName());
    std::string in, out, out_vel;
    nh.param<std::string>("in", in, "idfloor_in");
    nh.param<std::string>("out", out, "idfloor_out");
    nh.param<std::string>("out_vel", out_vel, "idfloor_out_vel");

    nh.param<double>("angular_vel_correction", angular_vel_correction, 1.0);
    nh.param<double>("lin_speed", lin_speed, 0.3);
    nh.param<double>("ang_speed", ang_speed, 0.3);
    nh.param<double>("cam_height", cam_height, 0.55);
    nh.param<double>("cam_z_trans", cam_z_trans, 0.0);
    nh.param<double>("wall_buffer", wall_buffer, 0.6);

    nh.param<int>("publish_visualization", publish_visualization, 1);
    nh.param<int>("random_walk", random_walk, 0);

    // Init Demo Tool
    demo.InitExplore(lin_speed,
                     ang_speed,
                     cam_height,
                     cam_z_trans,
                     wall_buffer,
                     random_walk);

    // Init Subscribers
    ros::NodeHandle nx;
    ros::Subscriber sub = nx.subscribe<PCLPointCloud>(in, 1, callback);

    // Init Publishers
    pub = nx.advertise<PCLPointCloud> (out, 1);
    pubvel = nx.advertise<geometry_msgs::Twist> (out_vel, 1);

    // Set to Spin
    ros::spin();

    return (0);
}