Ask Your Question
0

Trying to mimic TurtleBot Follower

asked 2012-06-11 00:44:19 -0600

McMurdo gravatar image

updated 2016-10-24 08:59:47 -0600

ngrennan gravatar image

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.

edit retag flag offensive close merge delete

Comments

@McMurdo Please don't shout.

tfoote gravatar imagetfoote ( 2012-06-11 07:45:01 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted
1

answered 2012-06-11 07:43:59 -0600

tfoote gravatar image

First off sensor_msgs/PointCloud and sensor_msgs/PointCloud2 are not compatible. If you want to use the two of them you need to subscribe and republish.

For the speed of the transport the subscription in the follower is chosen to use the pcl::PointCloudXYZ specificially because that is the datatype which the OpenNI nodelet is publishing. And by subscribing with the same datatype as is being published in the same process (using nodelets) the follower gets the data with zero copies(just a pointer pass). If you change the data type you will incur the costs of serializing, copying and deserializing, which is the performance difference you are seeing.

edit flag offensive delete link more

Comments

Two years hence, I understand the point.

McMurdo gravatar imageMcMurdo ( 2014-03-12 04:52:04 -0600 )edit
0

answered 2012-06-11 01:14:56 -0600

McMurdo gravatar image
satyaki@localghost:~/ros_workspace/turtlebot_follower$ rostopic hz /camera/depth/points
subscribed to [/camera/depth/points]
average rate: 0.268
    min: 3.727s max: 3.727s std dev: 0.00000s window: 2
no new messages
no new messages
no new messages
average rate: 0.265
    min: 3.727s max: 3.807s std dev: 0.04000s window: 3
no new messages
no new messages
no new messages
average rate: 0.264
    min: 3.727s max: 3.826s std dev: 0.04293s window: 4
no new messages
no new messages
average rate: 0.265
    min: 3.727s max: 3.826s std dev: 0.04095s window: 5
no new messages
no new messages
no new messages
average rate: 0.265
    min: 3.727s max: 3.826s std dev: 0.03741s window: 6
no new messages
no new messages
no new messages
average rate: 0.263
    min: 3.727s max: 3.878s std dev: 0.04999s window: 7
no new messages
no new messages
no new messages
average rate: 0.264
    min: 3.713s max: 3.878s std dev: 0.05470s window: 8
no new messages
no new messages
average rate: 0.265
    min: 3.713s max: 3.878s std dev: 0.05464s window: 9
no new messages
no new messages
no new messages
average rate: 0.265
    min: 3.713s max: 3.878s std dev: 0.05163s window: 10
no new messages
no new messages
no new messages
average rate: 0.265
    min: 3.713s max: 3.878s std dev: 0.04994s window: 11
no new messages
no new messages
no new messages
average rate: 0.265
    min: 3.713s max: 3.878s std dev: 0.04764s window: 12
no new messages
no new messages
average rate: 0.265
    min: 3.713s max: 3.878s std dev: 0.04809s window: 13
no new messages
no new messages
no new messages
average rate: 0.265
    min: 3.713s max: 3.878s std dev: 0.04702s window: 14
no new messages
no new messages
no new messages
average rate: 0.265
    min: 3.713s max: 3.878s std dev: 0.04567s window: 15
no new messages
no new messages
no new messages
average rate: 0.265
    min: 3.713s max: 3.878s std dev: 0.04496s window: 16
no new messages
no new messages
no new messages
average rate: 0.265
    min: 3.713s max: 3.878s std dev: 0.04371s window: 17
no new messages
no new messages
average rate: 0.265
    min: 3.713s max: 3.910s std dev: 0.05370s window: 18
no new messages
no new messages
no new messages
no new messages
no new messages
no new messages
average rate: 0.255
    min: 3.713s max: 6.413s std dev: 0.60565s window: 19
no new messages
no new messages
no new messages
average rate: 0.255
    min: 3.713s max: 6.413s std dev: 0.59135s window: 20
no new messages
no new messages
average rate: 0.256
    min: 3.713s max: 6.413s std dev: 0.57756s window: 21
no ...
(more)
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2012-06-11 00:44:19 -0600

Seen: 1,114 times

Last updated: Jun 11 '12