follower.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "ros/ros.h"
00031 #include "pluginlib/class_list_macros.h" 
00032 #include "nodelet/nodelet.h" 
00033 #include <geometry_msgs/Twist.h>
00034 #include <pcl_ros/point_cloud.h>
00035 #include <pcl/point_types.h>
00036 #include "dynamic_reconfigure/server.h"
00037 #include "turtlebot_follower/FollowerConfig.h"
00038 
00039 
00040 namespace turtlebot_follower
00041 {
00042 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00043 
00044 //* The turtlebot follower nodelet.
00050 class TurtlebotFollower : public nodelet::Nodelet 
00051 {
00052 public:
00057   TurtlebotFollower() : min_y_(0.1), max_y_(0.5),
00058                         min_x_(-0.2), max_x_(0.2),
00059                         max_z_(0.8), goal_z_(0.6),
00060                         z_scale_(1.0), x_scale_(5.0) 
00061   {
00062     
00063   }
00064 
00065   ~TurtlebotFollower()
00066   {
00067     delete srv_;
00068   }
00069   
00070 private:
00071   double min_y_; 
00072   double max_y_; 
00073   double min_x_; 
00074   double max_x_; 
00075   double max_z_; 
00076   double goal_z_; 
00077   double z_scale_; 
00078   double x_scale_; 
00081   // Dynamic reconfigure server
00082   dynamic_reconfigure::Server<turtlebot_follower::FollowerConfig>* srv_;
00083   
00089   virtual void onInit() 
00090   {
00091     ros::NodeHandle& nh = getNodeHandle();
00092     ros::NodeHandle& private_nh = getPrivateNodeHandle();
00093     
00094     private_nh.getParam("min_y", min_y_);
00095     private_nh.getParam("max_y", max_y_);
00096     private_nh.getParam("min_x", min_x_);
00097     private_nh.getParam("max_z", max_z_);
00098     private_nh.getParam("goal_z", goal_z_);
00099     private_nh.getParam("z_scale", z_scale_);
00100     private_nh.getParam("x_scale", x_scale_);
00101     
00102     cmdpub_ = nh.advertise<geometry_msgs::Twist> ("cmd_vel", 1);
00103     sub_= nh.subscribe<PointCloud>("/camera/depth/points", 1, &TurtlebotFollower::cloudcb, this);\
00104 
00105     srv_ = new dynamic_reconfigure::Server<turtlebot_follower::FollowerConfig>(private_nh);
00106     dynamic_reconfigure::Server<turtlebot_follower::FollowerConfig>::CallbackType f = boost::bind(&TurtlebotFollower::reconfigure, this, _1, _2);
00107     srv_->setCallback(f);
00108 
00109   }
00110 
00111   void reconfigure(turtlebot_follower::FollowerConfig &config, uint32_t level)
00112   {
00113     min_y_ = config.min_y;
00114     max_y_ = config.max_y;
00115     min_x_ = config.min_x;
00116     max_x_ = config.max_x;
00117     max_z_ = config.max_z;
00118     goal_z_ = config.goal_z;
00119     z_scale_ = config.z_scale;
00120     x_scale_ = config.x_scale;
00121   }
00122   
00130   void cloudcb(const PointCloud::ConstPtr&  cloud)
00131   {
00132     //X,Y,Z of the centroid
00133     double x = 0.0;
00134     double y = 0.0;
00135     double z = 0.0;
00136     //Number of points observed
00137     unsigned int n = 0;
00138     //Iterate through all the points in the region and find the average of the position
00139     BOOST_FOREACH (const pcl::PointXYZ& pt, cloud->points)
00140     {
00141       //First, ensure that the point's position is valid. This must be done in a seperate
00142       //if because we do not want to perform comparison on a nan value.
00143       if (!std::isnan(x) && !std::isnan(y) && !std::isnan(z))
00144       {
00145         //Test to ensure the point is within the aceptable box.
00146         if (-pt.y > min_y_ && -pt.y < max_y_ && pt.x < max_x_ && pt.x > min_x_ && pt.z < max_z_)
00147         {
00148           //Add the point to the totals
00149           x += pt.x;
00150           y += pt.y;
00151           z += pt.z;
00152           n++;
00153         }
00154       }
00155     }
00156     
00157     //If there are points, find the centroid and calculate the command goal.
00158     //If there are no points, simply publish a stop goal.
00159     if (n)
00160     { 
00161       x /= n; 
00162       y /= n; 
00163       z /= n;  
00164       
00165       ROS_DEBUG("Centriod at %f %f %f with %d points", x, y, z, n);
00166       
00167       geometry_msgs::Twist cmd;
00168       cmd.linear.x = (z - goal_z_) * z_scale_;
00169       cmd.angular.z = -x * x_scale_;
00170       cmdpub_.publish(cmd);
00171     }
00172     else
00173     {
00174       ROS_DEBUG("No points detected, stopping the robot");
00175       cmdpub_.publish(geometry_msgs::Twist());
00176     }
00177   }
00178   
00179   
00180   ros::Subscriber sub_;
00181   ros::Publisher cmdpub_;
00182 };
00183 
00184 PLUGINLIB_DECLARE_CLASS(turtlebot_follower, TurtlebotFollower, turtlebot_follower::TurtlebotFollower, nodelet::Nodelet);
00185 
00186 }


turtlebot_follower
Author(s): Tony Pratkanis
autogenerated on Fri Dec 6 2013 20:51:08