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/ChangeState.h"
00038 #include "turtlebot_follower/FollowerConfig.h"
00039 
00040 #include <visualization_msgs/Marker.h>
00041 
00042 
00043 namespace turtlebot_follower
00044 {
00045 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00046 
00047 //* The turtlebot follower nodelet.
00053 class TurtlebotFollower : public nodelet::Nodelet
00054 {
00055 public:
00060   TurtlebotFollower() : min_y_(0.1), max_y_(0.5),
00061                         min_x_(-0.2), max_x_(0.2),
00062                         max_z_(0.8), goal_z_(0.6),
00063                         z_scale_(1.0), x_scale_(5.0)
00064   {
00065 
00066   }
00067 
00068   ~TurtlebotFollower()
00069   {
00070     delete config_srv_;
00071   }
00072 
00073 private:
00074   double min_y_; 
00075   double max_y_; 
00076   double min_x_; 
00077   double max_x_; 
00078   double max_z_; 
00079   double goal_z_; 
00080   double z_scale_; 
00081   double x_scale_; 
00082   bool   enabled_; 
00084   // Service for start/stop following
00085   ros::ServiceServer switch_srv_;
00086 
00087   // Dynamic reconfigure server
00088   dynamic_reconfigure::Server<turtlebot_follower::FollowerConfig>* config_srv_;
00089 
00095   virtual void onInit()
00096   {
00097     ros::NodeHandle& nh = getNodeHandle();
00098     ros::NodeHandle& private_nh = getPrivateNodeHandle();
00099 
00100     private_nh.getParam("min_y", min_y_);
00101     private_nh.getParam("max_y", max_y_);
00102     private_nh.getParam("min_x", min_x_);
00103     private_nh.getParam("max_x", max_x_);
00104     private_nh.getParam("max_z", max_z_);
00105     private_nh.getParam("goal_z", goal_z_);
00106     private_nh.getParam("z_scale", z_scale_);
00107     private_nh.getParam("x_scale", x_scale_);
00108     private_nh.getParam("enabled", enabled_);
00109 
00110     cmdpub_ = private_nh.advertise<geometry_msgs::Twist> ("cmd_vel", 1);
00111     markerpub_ = private_nh.advertise<visualization_msgs::Marker>("marker",1);
00112     bboxpub_ = private_nh.advertise<visualization_msgs::Marker>("bbox",1);
00113     sub_= nh.subscribe<PointCloud>("depth/points", 1, &TurtlebotFollower::cloudcb, this);
00114 
00115     switch_srv_ = private_nh.advertiseService("change_state", &TurtlebotFollower::changeModeSrvCb, this);
00116 
00117     config_srv_ = new dynamic_reconfigure::Server<turtlebot_follower::FollowerConfig>(private_nh);
00118     dynamic_reconfigure::Server<turtlebot_follower::FollowerConfig>::CallbackType f =
00119         boost::bind(&TurtlebotFollower::reconfigure, this, _1, _2);
00120     config_srv_->setCallback(f);
00121   }
00122 
00123   void reconfigure(turtlebot_follower::FollowerConfig &config, uint32_t level)
00124   {
00125     min_y_ = config.min_y;
00126     max_y_ = config.max_y;
00127     min_x_ = config.min_x;
00128     max_x_ = config.max_x;
00129     max_z_ = config.max_z;
00130     goal_z_ = config.goal_z;
00131     z_scale_ = config.z_scale;
00132     x_scale_ = config.x_scale;
00133   }
00134 
00142   void cloudcb(const PointCloud::ConstPtr&  cloud)
00143   {
00144     //X,Y,Z of the centroid
00145     float x = 0.0;
00146     float y = 0.0;
00147     float z = 1e6;
00148     //Number of points observed
00149     unsigned int n = 0;
00150     //Iterate through all the points in the region and find the average of the position
00151     BOOST_FOREACH (const pcl::PointXYZ& pt, cloud->points)
00152     {
00153       //First, ensure that the point's position is valid. This must be done in a seperate
00154       //if because we do not want to perform comparison on a nan value.
00155       if (!std::isnan(x) && !std::isnan(y) && !std::isnan(z))
00156       {
00157         //Test to ensure the point is within the aceptable box.
00158         if (-pt.y > min_y_ && -pt.y < max_y_ && pt.x < max_x_ && pt.x > min_x_ && pt.z < max_z_)
00159         {
00160           //Add the point to the totals
00161           x += pt.x;
00162           y += pt.y;
00163           z = std::min(z, pt.z);
00164           n++;
00165         }
00166       }
00167     }
00168 
00169     //If there are points, find the centroid and calculate the command goal.
00170     //If there are no points, simply publish a stop goal.
00171     if (n>4000)
00172     {
00173       x /= n;
00174       y /= n;
00175       if(z > max_z_){
00176         ROS_DEBUG("No valid points detected, stopping the robot");
00177         if (enabled_)
00178         {
00179           cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
00180         }
00181         return;
00182       }
00183 
00184       ROS_DEBUG("Centroid at %f %f %f with %d points", x, y, z, n);
00185       publishMarker(x, y, z);
00186 
00187       if (enabled_)
00188       {
00189         geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
00190         cmd->linear.x = (z - goal_z_) * z_scale_;
00191         cmd->angular.z = -x * x_scale_;
00192         cmdpub_.publish(cmd);
00193       }
00194     }
00195     else
00196     {
00197       ROS_DEBUG("No points detected, stopping the robot");
00198       publishMarker(x, y, z);
00199 
00200       if (enabled_)
00201       {
00202         cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
00203       }
00204     }
00205 
00206     publishBbox();
00207   }
00208 
00209   bool changeModeSrvCb(ChangeState::Request& request, ChangeState::Response& response)
00210   {
00211     if ((enabled_ == true) && (request.state == request.STOPPED))
00212     {
00213       ROS_INFO("Change mode service request: following stopped");
00214       cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
00215       enabled_ = false;
00216     }
00217     else if ((enabled_ == false) && (request.state == request.FOLLOW))
00218     {
00219       ROS_INFO("Change mode service request: following (re)started");
00220       enabled_ = true;
00221     }
00222 
00223     response.result = response.OK;
00224     return true;
00225   }
00226 
00227   void publishMarker(double x,double y,double z)
00228   {
00229     visualization_msgs::Marker marker;
00230     marker.header.frame_id = "/camera_rgb_optical_frame";
00231     marker.header.stamp = ros::Time();
00232     marker.ns = "my_namespace";
00233     marker.id = 0;
00234     marker.type = visualization_msgs::Marker::SPHERE;
00235     marker.action = visualization_msgs::Marker::ADD;
00236     marker.pose.position.x = x;
00237     marker.pose.position.y = y;
00238     marker.pose.position.z = z;
00239     marker.pose.orientation.x = 0.0;
00240     marker.pose.orientation.y = 0.0;
00241     marker.pose.orientation.z = 0.0;
00242     marker.pose.orientation.w = 1.0;
00243     marker.scale.x = 0.2;
00244     marker.scale.y = 0.2;
00245     marker.scale.z = 0.2;
00246     marker.color.a = 1.0;
00247     marker.color.r = 1.0;
00248     marker.color.g = 0.0;
00249     marker.color.b = 0.0;
00250     //only if using a MESH_RESOURCE marker type:
00251     markerpub_.publish( marker );
00252   }
00253 
00254   void publishBbox()
00255   {
00256     double x = (min_x_ + max_x_)/2;
00257     double y = (min_y_ + max_y_)/2;
00258     double z = (0 + max_z_)/2;
00259 
00260     double scale_x = (max_x_ - x)*2;
00261     double scale_y = (max_y_ - y)*2;
00262     double scale_z = (max_z_ - z)*2;
00263 
00264     visualization_msgs::Marker marker;
00265     marker.header.frame_id = "/camera_rgb_optical_frame";
00266     marker.header.stamp = ros::Time();
00267     marker.ns = "my_namespace";
00268     marker.id = 1;
00269     marker.type = visualization_msgs::Marker::CUBE;
00270     marker.action = visualization_msgs::Marker::ADD;
00271     marker.pose.position.x = x;
00272     marker.pose.position.y = -y;
00273     marker.pose.position.z = z;
00274     marker.pose.orientation.x = 0.0;
00275     marker.pose.orientation.y = 0.0;
00276     marker.pose.orientation.z = 0.0;
00277     marker.pose.orientation.w = 1.0;
00278     marker.scale.x = scale_x;
00279     marker.scale.y = scale_y;
00280     marker.scale.z = scale_z;
00281     marker.color.a = 0.5;
00282     marker.color.r = 0.0;
00283     marker.color.g = 1.0;
00284     marker.color.b = 0.0;
00285     //only if using a MESH_RESOURCE marker type:
00286     bboxpub_.publish( marker );
00287   }
00288 
00289   ros::Subscriber sub_;
00290   ros::Publisher cmdpub_;
00291   ros::Publisher markerpub_;
00292   ros::Publisher bboxpub_;
00293 };
00294 
00295 PLUGINLIB_DECLARE_CLASS(turtlebot_follower, TurtlebotFollower, turtlebot_follower::TurtlebotFollower, nodelet::Nodelet);
00296 
00297 }


turtlebot_follower
Author(s): Tony Pratkanis
autogenerated on Mon Oct 6 2014 08:05:42