Trying to mimic TurtleBot Follower

edit

I was trying to mimic the work of turtlebot_follower by copy-pasting the code from the follower.cpp in the same package. The code is below:

#include "ros/ros.h"
#include "pluginlib/class_list_macros.h" 
#include "nodelet/nodelet.h" 
#include <geometry_msgs/Twist.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include "dynamic_reconfigure/server.h"
#include "turtlebot_follower/FollowerConfig.h"

#define min_x -0.2
#define min_y 0.1
#define max_x 0.2
#define max_y 0.5
#define max_z 1.0
#define goal_z 0.6
#define x_scale 7.0
#define z_scale 2.0


typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
ros::Subscriber sub_;
ros::Publisher cmdpub_;
geometry_msgs::Twist cmd;


int main(int argsc, char **ararr)
{
 ros::init(argsc, ararr, "foll");
 ros::NodeHandle nh;


 void cloudcb(const sensor_msgs::PointCloud2ConstPtr&);
 cmdpub_ = nh.advertise<geometry_msgs::Twist> ("cmd_vel", 1);
 sub_= nh.subscribe<sensor_msgs::PointCloud2>("/camera/depth/points", 1, cloudcb);
 while(ros::ok())
 {
    ROS_INFO("GOING GOING BOING!!!");
    ros::spinOnce();
 }

 int t = 5;
 ros::Rate rate(1);     

}

void cloudcb(const sensor_msgs::PointCloud2ConstPtr& input)
{
    PointCloud cloud;
    pcl::fromROSMsg (*input, cloud);
    //X,Y,Z of the centroid
    double x = 0.0;
    double y = 0.0;
    double z = 0.0;
    //Number of points observed
    unsigned int n = 0;
    //Iterate through all the points in the region and find the average of the position
    BOOST_FOREACH (const pcl::PointXYZ& pt, cloud.points)
    {
      //First, ensure that the point's position is valid. This must be done in a seperate
      //if because we do not want to perform comparison on a nan value.
      if (!std::isnan(x) && !std::isnan(y) && !std::isnan(z))
      {
        //Test to ensure the point is within the aceptable box.
        if (-pt.y > min_y && -pt.y < max_y && pt.x < max_x && pt.x > min_x && pt.z < max_z)
        {
          //Add the point to the totals
          x += pt.x;
          y += pt.y;
          z += pt.z;
          n++;
        }
      }
    }
    if (n)
    { 
      x /= n; 
      y /= n; 
      z /= n;  

      ROS_DEBUG("Centriod at %f %f %f with %d points", x, y, z, n);


      cmd.linear.x = (z - goal_z) * z_scale;
      cmd.angular.z = -x * x_scale;
      cmdpub_.publish(cmd);
      ROS_ERROR("GOT INPUT!");
    }
    else
    {
      ROS_DEBUG("No points detected, stopping the robot");
      cmdpub_.publish(geometry_msgs::Twist());
    }
}

I have editted the callback in this one to get a sensor_msgs::PointCloud2 data instead of the pcl::PointCloud<pcl::pointxyz> data that is there in the original follower.cpp file.

I have tried with both the cases. I have found that the callbacks occur two slowly (once every 3-5 seconds), when I used the pcl::PointCloud<pcl::pointxyz> datatype for the callbacks and the callbacks never occurred when I used the sensor_msgs::PointCloud datatype.

Is there a problem with the code? The turtlebot_follower launch file (follower.launch) works great!!! But this one, apparently having the same logic as the other, works too slow. The robot tries to reorient itself once in every 3-5 seconds. Please help.