human_tracker.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: human_tracker.cpp 30719 2010-07-09 20:28:41Z pangercic $
00035  *
00036  */
00037 
00047 // ROS core
00048 #include <ros/ros.h>
00049 #include <pcl/common/angles.h>
00050 
00051 #include <tf/transform_listener.h>
00052 #include <kinect_cleanup/GrabObject.h>
00053 #include <kinect_cleanup/ReleaseObject.h>
00054 
00055 using namespace std;
00056 
00057 class HumanTracker
00058 {
00059 protected:
00060   ros::NodeHandle nh_;
00061   
00062 public:
00063   tf::TransformListener tf_;
00064   std::string gesture_arm_, pointing_arm_;
00065   std::string neck_frame_, world_;
00066   double tf_buffer_time_;
00067   bool start_, stop_;
00068   ros::ServiceClient client_grab, client_release;
00070   HumanTracker  (ros::NodeHandle &n) : nh_(n)
00071   {
00072     nh_.param("world", world_, std::string("openni_rgb_optical_frame"));
00073     nh_.param("neck_frame", neck_frame_, std::string("neck"));
00074     nh_.param("gesture_arm", gesture_arm_, std::string("left"));
00075     nh_.param("pointing_arm", pointing_arm_, std::string("right"));
00076 
00077     nh_.param("tf_buffer_time", tf_buffer_time_, 1.0);
00078     start_ = stop_ = false;
00079     client_grab = nh_.serviceClient<kinect_cleanup::GrabObject>("/grab_object");
00080     client_release = nh_.serviceClient<kinect_cleanup::ReleaseObject>("/release_object");
00081   }
00082 
00084   // spin (!)
00085   void spin ()
00086   {
00087     ros::Rate loop_rate(20);
00088     while (ros::ok())
00089     {
00090       //find if we have start or stop gesture
00091       ros::Time time = ros::Time::now();
00092       bool found_transform1 = tf_.waitForTransform(neck_frame_, gesture_arm_ + "_elbow",
00093                                                    time, ros::Duration(tf_buffer_time_));
00094       bool found_transform2 = tf_.waitForTransform(neck_frame_, gesture_arm_ + "_hand",
00095                                                    time, ros::Duration(tf_buffer_time_));
00096       if (found_transform1 && found_transform2)
00097       {
00098         tf::StampedTransform transform1, transform2;
00099         tf_.lookupTransform(neck_frame_, gesture_arm_ + "_elbow", time, transform1);
00100         tf_.lookupTransform(neck_frame_, gesture_arm_ + "_hand", time, transform2);                
00101         
00102         //stop gesture
00103         //std::cerr << fabs(transform2.getOrigin().x()) <<  " " << fabs(transform1.getOrigin().x()) << std::endl;
00104         //if (fabs(transform2.getOrigin().x()) > 1.5 * fabs(transform1.getOrigin().x()))
00105         //std::cerr << transform1.getOrigin().z() <<  " " << transform2.getOrigin().z() << std::endl;
00106         if (fabs(transform2.getOrigin().x()) > 0.6)
00107         {
00108           ROS_INFO("Stop");
00109           stop_ = true;
00110           kinect_cleanup::ReleaseObject srv_release;
00111           srv_release.request.release = stop_;
00112           if (client_release.call (srv_release))
00113           {
00114             ROS_INFO ("Service call /release_object successful");
00115           }
00116           else
00117           {
00118             ROS_ERROR ("Failed to call service /release_object");
00119           }
00120         }
00121 
00122         //start gesture (triggers computation of hand end-effector point and pointing direction)
00123         // else if (0.9 * fabs(transform1.getOrigin().x()) < fabs(transform2.getOrigin().x()) && 
00124         //          fabs(transform2.getOrigin().x()) < 1.1 * fabs(transform1.getOrigin().x()) &&
00125         //          transform1.getOrigin().y() < transform2.getOrigin().y())
00126         else if (-0.3 > transform2.getOrigin().z())
00127         {
00128           ROS_INFO("Start");
00129           start_ = true;
00130         }
00131         else
00132         {
00133           //          ROS_INFO("No gesture");
00134         }
00135       }
00136       
00137       //computation of hand end-effector point and pointing direction
00138       if (start_)
00139       {
00140         //sleep(17);
00141         time = ros::Time::now();
00142         bool found_transform3 = tf_.waitForTransform(world_, pointing_arm_ + "_elbow",
00143                                                      time, ros::Duration(tf_buffer_time_));
00144 
00145         bool found_transform4 = tf_.waitForTransform(world_, pointing_arm_ + "_hand",
00146                                                      time, ros::Duration(tf_buffer_time_));
00147 
00148         if (found_transform3 && found_transform4)
00149         {
00150           tf::StampedTransform transform3, transform4;
00151           tf_.lookupTransform(world_, pointing_arm_ + "_elbow", time, transform3);
00152           tf_.lookupTransform(world_, pointing_arm_ + "_hand", time, transform4);
00153           btVector3 direction = transform4.getOrigin() - transform3.getOrigin();
00154           std::vector <float> point_axis (6, 0.0);
00155           point_axis[0] = transform4.getOrigin().x();
00156           point_axis[1] = transform4.getOrigin().y();
00157           point_axis[2] = transform4.getOrigin().z();
00158           point_axis[3] = direction.normalize().x();
00159           point_axis[4] = direction.normalize().y();
00160           point_axis[5] = direction.normalize().z();
00161           
00162           //service call
00163           kinect_cleanup::GrabObject srv_grab;
00164           //ROS_INFO("point and direction:");
00165           for (unsigned long i=0; i < point_axis.size(); i++)
00166           {
00167             srv_grab.request.point_line[i] = point_axis[i];
00168             //ROS_INFO_STREAM(" " << point_axis[i]);
00169           }
00170           
00171           if (client_grab.call (srv_grab))
00172           {
00173             ROS_INFO ("Service call /grab_object successful");
00174           }
00175           else
00176           {
00177             ROS_ERROR ("Failed to call service /grab_object");
00178           }
00179           start_ = false;
00180         } 
00181       }
00182       ros::spinOnce();
00183       loop_rate.sleep();
00184     }
00185   }
00186 };
00187 
00188 int main (int argc, char** argv)
00189 {
00190   ros::init (argc, argv, "human_tracker_node");
00191   ros::NodeHandle n("~");
00192   HumanTracker ht(n);
00193   //ros::spin ();
00194   ht.spin();
00195   return (0);
00196 }
00197 
00198 /* ]--- */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


kinect_cleanup
Author(s): Zoltan-Csaba Marton, Dejan Pangercic
autogenerated on Sun Oct 6 2013 11:57:20