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 <sensor_msgs/Image.h>
00035 #include <visualization_msgs/Marker.h>
00036 #include <turtlebot_msgs/SetFollowState.h>
00037 
00038 #include "dynamic_reconfigure/server.h"
00039 #include "turtlebot_follower/FollowerConfig.h"
00040 
00041 #include <depth_image_proc/depth_traits.h>
00042 
00043 
00044 namespace turtlebot_follower
00045 {
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<sensor_msgs::Image>("/camera/depth/points", 1, &TurtlebotFollower::imagecb, 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 imagecb(const sensor_msgs::ImageConstPtr& depth_msg)
00143   {
00144 
00145     // Precompute the sin function for each row and column
00146     uint32_t image_width = depth_msg->width;
00147     float x_radians_per_pixel = 60.0/57.0/image_width;
00148     float sin_pixel_x[image_width];
00149     for (int x = 0; x < image_width; ++x) {
00150       sin_pixel_x[x] = sin((x - image_width/ 2.0)  * x_radians_per_pixel);
00151     }
00152 
00153     uint32_t image_height = depth_msg->height;
00154     float y_radians_per_pixel = 45.0/57.0/image_width;
00155     float sin_pixel_y[image_height];
00156     for (int y = 0; y < image_height; ++y) {
00157       // Sign opposite x for y up values
00158       sin_pixel_y[y] = sin((image_height/ 2.0 - y)  * y_radians_per_pixel);
00159     }
00160 
00161     //X,Y,Z of the centroid
00162     float x = 0.0;
00163     float y = 0.0;
00164     float z = 1e6;
00165     //Number of points observed
00166     unsigned int n = 0;
00167 
00168     //Iterate through all the points in the region and find the average of the position
00169     const float* depth_row = reinterpret_cast<const float*>(&depth_msg->data[0]);
00170     int row_step = depth_msg->step / sizeof(float);
00171     for (int v = 0; v < (int)depth_msg->height; ++v, depth_row += row_step)
00172     {
00173      for (int u = 0; u < (int)depth_msg->width; ++u)
00174      {
00175        float depth = depth_image_proc::DepthTraits<float>::toMeters(depth_row[u]);
00176        if (!depth_image_proc::DepthTraits<float>::valid(depth) || depth > max_z_) continue;
00177        float y_val = sin_pixel_y[v] * depth;
00178        float x_val = sin_pixel_x[u] * depth;
00179        if ( y_val > min_y_ && y_val < max_y_ &&
00180             x_val > min_x_ && x_val < max_x_)
00181        {
00182          x += x_val;
00183          y += y_val;
00184          z = std::min(z, depth); //approximate depth as forward.
00185          n++;
00186        }
00187      }
00188     }
00189 
00190     //If there are points, find the centroid and calculate the command goal.
00191     //If there are no points, simply publish a stop goal.
00192     if (n>4000)
00193     {
00194       x /= n;
00195       y /= n;
00196       if(z > max_z_){
00197         ROS_INFO_THROTTLE(1, "Centroid too far away %f, stopping the robot\n%s", z);
00198         if (enabled_)
00199         {
00200           cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
00201         }
00202         return;
00203       }
00204 
00205       ROS_INFO_THROTTLE(1, "Centroid at %f %f %f with %d points", x, y, z, n);
00206       publishMarker(x, y, z);
00207 
00208       if (enabled_)
00209       {
00210         geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
00211         cmd->linear.x = (z - goal_z_) * z_scale_;
00212         cmd->angular.z = -x * x_scale_;
00213         cmdpub_.publish(cmd);
00214       }
00215     }
00216     else
00217     {
00218       ROS_INFO_THROTTLE(1, "Not enough points(%d) detected, stopping the robot", n);
00219       publishMarker(x, y, z);
00220 
00221       if (enabled_)
00222       {
00223         cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
00224       }
00225     }
00226 
00227     publishBbox();
00228   }
00229 
00230   bool changeModeSrvCb(turtlebot_msgs::SetFollowState::Request& request,
00231                        turtlebot_msgs::SetFollowState::Response& response)
00232   {
00233     if ((enabled_ == true) && (request.state == request.STOPPED))
00234     {
00235       ROS_INFO("Change mode service request: following stopped");
00236       cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
00237       enabled_ = false;
00238     }
00239     else if ((enabled_ == false) && (request.state == request.FOLLOW))
00240     {
00241       ROS_INFO("Change mode service request: following (re)started");
00242       enabled_ = true;
00243     }
00244 
00245     response.result = response.OK;
00246     return true;
00247   }
00248 
00249   void publishMarker(double x,double y,double z)
00250   {
00251     visualization_msgs::Marker marker;
00252     marker.header.frame_id = "/camera_rgb_optical_frame";
00253     marker.header.stamp = ros::Time();
00254     marker.ns = "my_namespace";
00255     marker.id = 0;
00256     marker.type = visualization_msgs::Marker::SPHERE;
00257     marker.action = visualization_msgs::Marker::ADD;
00258     marker.pose.position.x = x;
00259     marker.pose.position.y = y;
00260     marker.pose.position.z = z;
00261     marker.pose.orientation.x = 0.0;
00262     marker.pose.orientation.y = 0.0;
00263     marker.pose.orientation.z = 0.0;
00264     marker.pose.orientation.w = 1.0;
00265     marker.scale.x = 0.2;
00266     marker.scale.y = 0.2;
00267     marker.scale.z = 0.2;
00268     marker.color.a = 1.0;
00269     marker.color.r = 1.0;
00270     marker.color.g = 0.0;
00271     marker.color.b = 0.0;
00272     //only if using a MESH_RESOURCE marker type:
00273     markerpub_.publish( marker );
00274   }
00275 
00276   void publishBbox()
00277   {
00278     double x = (min_x_ + max_x_)/2;
00279     double y = (min_y_ + max_y_)/2;
00280     double z = (0 + max_z_)/2;
00281 
00282     double scale_x = (max_x_ - x)*2;
00283     double scale_y = (max_y_ - y)*2;
00284     double scale_z = (max_z_ - z)*2;
00285 
00286     visualization_msgs::Marker marker;
00287     marker.header.frame_id = "/camera_rgb_optical_frame";
00288     marker.header.stamp = ros::Time();
00289     marker.ns = "my_namespace";
00290     marker.id = 1;
00291     marker.type = visualization_msgs::Marker::CUBE;
00292     marker.action = visualization_msgs::Marker::ADD;
00293     marker.pose.position.x = x;
00294     marker.pose.position.y = -y;
00295     marker.pose.position.z = z;
00296     marker.pose.orientation.x = 0.0;
00297     marker.pose.orientation.y = 0.0;
00298     marker.pose.orientation.z = 0.0;
00299     marker.pose.orientation.w = 1.0;
00300     marker.scale.x = scale_x;
00301     marker.scale.y = scale_y;
00302     marker.scale.z = scale_z;
00303     marker.color.a = 0.5;
00304     marker.color.r = 0.0;
00305     marker.color.g = 1.0;
00306     marker.color.b = 0.0;
00307     //only if using a MESH_RESOURCE marker type:
00308     bboxpub_.publish( marker );
00309   }
00310 
00311   ros::Subscriber sub_;
00312   ros::Publisher cmdpub_;
00313   ros::Publisher markerpub_;
00314   ros::Publisher bboxpub_;
00315 };
00316 
00317 PLUGINLIB_DECLARE_CLASS(turtlebot_follower, TurtlebotFollower, turtlebot_follower::TurtlebotFollower, nodelet::Nodelet);
00318 
00319 }


gapter
Author(s):
autogenerated on Thu Jun 6 2019 22:05:13