$search
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 }