cob_footprint_observer.cpp
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2012
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00006  *
00007  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00008  *
00009  * Project name: care-o-bot
00010  * ROS stack name: cob_navigation
00011  * ROS package name: cob_footprint_observer
00012  *                
00013  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00014  *      
00015  * Author: Matthias Gruhler, email: Matthias.Gruhler@ipa.fraunhofer.de
00016  *
00017  * Date of creation: 27.02.2012
00018  *
00019  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00020  *
00021  * Redistribution and use in source and binary forms, with or without
00022  * modification, are permitted provided that the following conditions are met:
00023  *
00024  *   * Redistributions of source code must retain the above copyright
00025  *     notice, this list of conditions and the following disclaimer.
00026  *   * Redistributions in binary form must reproduce the above copyright
00027  *     notice, this list of conditions and the following disclaimer in the
00028  *     documentation and/or other materials provided with the distribution.
00029  *   * Neither the name of the Fraunhofer Institute for Manufacturing 
00030  *     Engineering and Automation (IPA) nor the names of its
00031  *     contributors may be used to endorse or promote products derived from
00032  *     this software without specific prior written permission.
00033  *
00034  * This program is free software: you can redistribute it and/or modify
00035  * it under the terms of the GNU Lesser General Public License LGPL as 
00036  * published by the Free Software Foundation, either version 3 of the 
00037  * License, or (at your option) any later version.
00038  * 
00039  * This program is distributed in the hope that it will be useful,
00040  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00041  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00042  * GNU Lesser General Public License LGPL for more details.
00043  * 
00044  * You should have received a copy of the GNU Lesser General Public 
00045  * License LGPL along with this program. 
00046  * If not, see <http://www.gnu.org/licenses/>.
00047  *
00048  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00049  * @file  cob_footprint_observer.cpp
00050  * @brief  observes the footprint of care-o-bot
00051  *
00052  * Generates rectangular footprint of care-o-bot depending on setup of
00053  * arm and tray
00054  *
00055  ****************************************************************/
00056 #include <cob_footprint_observer.h>
00057 
00058 // Constructor
00059 FootprintObserver::FootprintObserver() :
00060   times_warned_(0)
00061 {
00062   nh_ = ros::NodeHandle("~");
00063 
00064   m_mutex = PTHREAD_MUTEX_INITIALIZER;
00065 
00066   // publish footprint
00067   topic_pub_footprint_ = nh_.advertise<geometry_msgs::PolygonStamped>("adjusted_footprint",1);
00068   
00069   // advertise service
00070   srv_get_footprint_ = nh_.advertiseService("/get_footprint", &FootprintObserver::getFootprintCB, this);
00071 
00072   // read footprint_source parameter
00073   std::string footprint_source; 
00074   if(!nh_.hasParam("footprint_source")) ROS_WARN("Checking default location (/local_costmap_node/costmap) for initial footprint parameter.");
00075   nh_.param("footprint_source", footprint_source, std::string("/local_costmap_node/costmap"));
00076  
00077   // node handle to get footprint from parameter server
00078   ros::NodeHandle footprint_source_nh_(footprint_source);       
00079   
00080   // load the robot footprint from the parameter server if its available in the local costmap namespace
00081   robot_footprint_ = loadRobotFootprint(footprint_source_nh_);
00082   if(robot_footprint_.size() > 4) 
00083     ROS_WARN("You have set more than 4 points as robot_footprint, cob_footprint_observer can deal only with rectangular footprints so far!");
00084 
00085   // get parameter sepcifying minimal changes of footprint that are accepted. (smaller changes are neglected)
00086   if(!nh_.hasParam("epsilon"))
00087     ROS_WARN("No epsilon value specified. Changes in footprint smaller than epsilon are neglected. Using default [0.01m]!");
00088   nh_.param("epsilon", epsilon_, 0.01);
00089     
00090   // get the frames for which to check the footprint
00091   if(!nh_.hasParam("frames_to_check")) ROS_WARN("No frames to check for footprint observer. Only using initial footprint!");
00092   nh_.param("frames_to_check", frames_to_check_, std::string(""));
00093   
00094   if(!nh_.hasParam("robot_base_frame")) ROS_WARN("No parameter robot_base_frame on parameter server. Using default [/base_link].");
00095   nh_.param("robot_base_frame", robot_base_frame_, std::string("/base_link"));
00096 
00097   last_tf_missing_ = ros::Time::now();
00098 }
00099 
00100 // Destructor
00101 FootprintObserver::~FootprintObserver()
00102 {}
00103 
00104 // GetFootprint Service callback
00105 bool FootprintObserver::getFootprintCB(cob_footprint_observer::GetFootprint::Request &req, cob_footprint_observer::GetFootprint::Response &resp)
00106 {
00107   // create Polygon message for service call
00108   geometry_msgs::PolygonStamped footprint_poly;
00109   footprint_poly.header.frame_id = robot_base_frame_;
00110   footprint_poly.header.stamp = ros::Time::now();
00111   
00112   footprint_poly.polygon.points.resize(robot_footprint_.size());
00113   for(unsigned int i=0; i<robot_footprint_.size(); ++i) {
00114     footprint_poly.polygon.points[i].x = robot_footprint_[i].x;
00115     footprint_poly.polygon.points[i].y = robot_footprint_[i].y;
00116     footprint_poly.polygon.points[i].z = robot_footprint_[i].z;   
00117   }
00118 
00119   resp.footprint = footprint_poly;
00120   resp.success.data = true;
00121 
00122   return true;
00123 }
00124 
00125 // load robot footprint from costmap_2d_ros to keep same footprint format
00126 std::vector<geometry_msgs::Point> FootprintObserver::loadRobotFootprint(ros::NodeHandle node){
00127   std::vector<geometry_msgs::Point> footprint;
00128   geometry_msgs::Point pt;
00129   double padding;
00130 
00131   std::string padding_param, footprint_param;
00132   if(!node.searchParam("footprint_padding", padding_param))
00133     padding = 0.01;
00134   else
00135     node.param(padding_param, padding, 0.01);
00136 
00137   //grab the footprint from the parameter server if possible
00138   XmlRpc::XmlRpcValue footprint_list;
00139   std::string footprint_string;
00140   std::vector<std::string> footstring_list;
00141   if(node.searchParam("footprint", footprint_param)){
00142     node.getParam(footprint_param, footprint_list);
00143     if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) {
00144       footprint_string = std::string(footprint_list);
00145 
00146       //if there's just an empty footprint up there, return
00147       if(footprint_string == "[]" || footprint_string == "")
00148         return footprint;
00149 
00150       boost::erase_all(footprint_string, " ");
00151 
00152       boost::char_separator<char> sep("[]");
00153       boost::tokenizer<boost::char_separator<char> > tokens(footprint_string, sep);
00154       footstring_list = std::vector<std::string>(tokens.begin(), tokens.end());
00155     }
00156     //make sure we have a list of lists
00157     if(!(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_list.size() > 2)
00158        && !(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString && footstring_list.size() > 5)){
00159       ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s",
00160                 footprint_param.c_str(), std::string(footprint_list).c_str());
00161       throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least 3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
00162     }
00163 
00164     if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray) {
00165       for(int i = 0; i < footprint_list.size(); ++i){
00166         //make sure we have a list of lists of size 2
00167         XmlRpc::XmlRpcValue point = footprint_list[i];
00168         if(!(point.getType() == XmlRpc::XmlRpcValue::TypeArray && point.size() == 2)){
00169           ROS_FATAL("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
00170           throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
00171         }
00172 
00173         //make sure that the value we're looking at is either a double or an int
00174         if(!(point[0].getType() == XmlRpc::XmlRpcValue::TypeInt || point[0].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
00175           ROS_FATAL("Values in the footprint specification must be numbers");
00176           throw std::runtime_error("Values in the footprint specification must be numbers");
00177         }
00178         pt.x = point[0].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[0]) : (double)(point[0]);
00179         pt.x += sign(pt.x) * padding;
00180 
00181         //make sure that the value we're looking at is either a double or an int
00182         if(!(point[1].getType() == XmlRpc::XmlRpcValue::TypeInt || point[1].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
00183           ROS_FATAL("Values in the footprint specification must be numbers");
00184           throw std::runtime_error("Values in the footprint specification must be numbers");
00185         }
00186         pt.y = point[1].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[1]) : (double)(point[1]);
00187         pt.y += sign(pt.y) * padding;
00188 
00189         footprint.push_back(pt);
00190 
00191         node.deleteParam(footprint_param);
00192         std::ostringstream oss;
00193         bool first = true;
00194         BOOST_FOREACH(geometry_msgs::Point p, footprint) {
00195           if(first) {
00196             oss << "[[" << p.x << "," << p.y << "]";
00197             first = false;
00198           }
00199           else {
00200             oss << ",[" << p.x << "," << p.y << "]";
00201           }
00202         }
00203         oss << "]";
00204         node.setParam(footprint_param, oss.str().c_str());
00205         node.setParam("footprint", oss.str().c_str());
00206       }
00207     }
00208 
00209     else if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) {
00210       std::vector<geometry_msgs::Point> footprint_spec;
00211       bool valid_foot = true;
00212       BOOST_FOREACH(std::string t, footstring_list) {
00213         if( t != "," ) {
00214           boost::erase_all(t, " ");
00215           boost::char_separator<char> pt_sep(",");
00216           boost::tokenizer<boost::char_separator<char> > pt_tokens(t, pt_sep);
00217           std::vector<std::string> point(pt_tokens.begin(), pt_tokens.end());
00218 
00219           if(point.size() != 2) {
00220             ROS_WARN("Each point must have exactly 2 coordinates");
00221             valid_foot = false;
00222             break;
00223           }
00224 
00225           std::vector<double>tmp_pt;
00226           BOOST_FOREACH(std::string p, point) {
00227             std::istringstream iss(p);
00228             double temp;
00229             if(iss >> temp) {
00230               tmp_pt.push_back(temp);
00231             }
00232             else {
00233               ROS_WARN("Each coordinate must convert to a double.");
00234               valid_foot = false;
00235               break;
00236             }
00237           }
00238           if(!valid_foot)
00239             break;
00240 
00241           geometry_msgs::Point pt;
00242           pt.x = tmp_pt[0];
00243           pt.y = tmp_pt[1];
00244 
00245           footprint_spec.push_back(pt);
00246         }
00247       }
00248       if (valid_foot) {
00249         footprint = footprint_spec;
00250         node.setParam("footprint", footprint_string);
00251       }
00252       else {
00253         ROS_FATAL("This footprint is not vaid it must be specified as a list of lists with at least 3 points, you specified %s", footprint_string.c_str());
00254         throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least 3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
00255       }
00256     }
00257   }
00258 
00259   footprint_right_ = 0.0f; footprint_left_ = 0.0f; footprint_front_ = 0.0f; footprint_rear_ = 0.0f;
00260   //extract rectangular borders for simplifying:
00261   for(unsigned int i=0; i<footprint.size(); i++) {
00262     if(footprint[i].x > footprint_front_) footprint_front_ = footprint[i].x;
00263     if(footprint[i].x < footprint_rear_) footprint_rear_ = footprint[i].x;
00264     if(footprint[i].y > footprint_left_) footprint_left_ = footprint[i].y;
00265     if(footprint[i].y < footprint_right_) footprint_right_ = footprint[i].y;
00266   }
00267   ROS_DEBUG("Extracted rectangular footprint for cob_footprint_observer: Front: %f, Rear %f, Left: %f, Right %f",
00268             footprint_front_, footprint_rear_, footprint_left_, footprint_right_);
00269 
00270   if ( fabs(footprint_front_ - footprint_rear_) == 0.0 || fabs(footprint_right_ - footprint_left_) == 0.0){
00271     ROS_WARN("Footprint has no physical dimension!");
00272   }
00273 
00274   // save initial footprint
00275   footprint_front_initial_ = footprint_front_;
00276   footprint_rear_initial_ = footprint_rear_;
00277   footprint_left_initial_ = footprint_left_;
00278   footprint_right_initial_ = footprint_right_;
00279 
00280   return footprint;
00281 }
00282 
00283 // checks if footprint has to be adjusted and does so if necessary
00284 void FootprintObserver::checkFootprint(){
00285   // check each frame
00286   std::string frame;
00287   std::stringstream ss;
00288   ss << frames_to_check_; 
00289 
00290   double x_rear, x_front, y_left, y_right;
00291   x_front = footprint_front_initial_;
00292   x_rear = footprint_rear_initial_;
00293   y_left = footprint_left_initial_;
00294   y_right = footprint_right_initial_;
00295 
00296   bool missing_frame_exists = false;
00297   while(ss >> frame){
00298     // get transform between robot base frame and frame
00299     if(tf_listener_.canTransform(robot_base_frame_, frame, ros::Time(0))) {
00300       tf::StampedTransform transform;
00301       tf_listener_.lookupTransform(robot_base_frame_, frame, ros::Time(0), transform);
00302       
00303       tf::Vector3 frame_position = transform.getOrigin();
00304 
00305       // check if frame position is outside of current footprint
00306       if(frame_position.x() > x_front) x_front = frame_position.x();
00307       if(frame_position.x() < x_rear) x_rear = frame_position.x();
00308       if(frame_position.y() > y_left) y_left = frame_position.y();
00309       if(frame_position.y() < y_right) y_right = frame_position.y();
00310     } else if ( (ros::Time::now() - last_tf_missing_).toSec() > 10.0
00311                 && times_warned_ < 3 ) {
00312       ++times_warned_;
00313       missing_frame_exists = true;
00314       ROS_WARN("Footprint Observer: Transformation for %s not available! Frame %s not considered in adjusted footprint!",
00315                frame.c_str(), frame.c_str());
00316     }
00317   }
00318   if (missing_frame_exists)
00319     last_tf_missing_ = ros::Time::now();
00320 
00321   // check if footprint has changed
00322   if ( fabs( footprint_front_ - x_front ) > epsilon_
00323        || fabs( footprint_rear_ - x_rear ) > epsilon_
00324        || fabs( footprint_left_ - y_left ) > epsilon_
00325        || fabs( footprint_right_ - y_right ) > epsilon_ )
00326   {
00327     pthread_mutex_lock(&m_mutex);
00328     // adjust footprint
00329     footprint_front_ = x_front; 
00330     footprint_rear_ = x_rear;
00331     footprint_left_ = y_left;
00332     footprint_right_ = y_right;
00333     pthread_mutex_unlock(&m_mutex);
00334 
00335     // create new footprint vector
00336     geometry_msgs::Point point;
00337     std::vector<geometry_msgs::Point> points;
00338 
00339     point.x = footprint_front_;
00340     point.y = footprint_left_;
00341     point.z = 0;
00342     points.push_back(point);
00343     point.y = footprint_right_;
00344     points.push_back(point);
00345     point.x = footprint_rear_;
00346     points.push_back(point);
00347     point.y = footprint_left_;
00348     points.push_back(point);
00349 
00350     pthread_mutex_lock(&m_mutex);
00351     robot_footprint_ = points;
00352     pthread_mutex_unlock(&m_mutex);
00353 
00354     // publish the adjusted footprint
00355     publishFootprint();
00356   }
00357   
00358 }
00359 
00360 // publishes the adjusted footprint
00361 void FootprintObserver::publishFootprint(){
00362 
00363   // create Polygon message 
00364   geometry_msgs::PolygonStamped footprint_poly;
00365   footprint_poly.header.frame_id = robot_base_frame_;
00366   footprint_poly.header.stamp = ros::Time::now();
00367   
00368   footprint_poly.polygon.points.resize(robot_footprint_.size());
00369   for(unsigned int i=0; i<robot_footprint_.size(); ++i) {
00370     footprint_poly.polygon.points[i].x = robot_footprint_[i].x;
00371     footprint_poly.polygon.points[i].y = robot_footprint_[i].y;
00372     footprint_poly.polygon.points[i].z = robot_footprint_[i].z;   
00373   }
00374 
00375   // publish adjusted footprint
00376   topic_pub_footprint_.publish(footprint_poly);
00377 
00378 }
00379 
00380 // compute the sign of x
00381 double FootprintObserver::sign(double x){
00382   if(x >= 0.0f) return 1.0f;
00383   else return -1.0f;
00384 }
00385 
00386 //#######################
00387 //#### main programm ####
00388 int main(int argc, char** argv)
00389 {
00390   // initialize ROS, specify name of node
00391   ros::init(argc,argv,"footprint_observer");
00392 
00393   // create observer
00394   FootprintObserver footprintObserver;
00395 
00396   // run footprint observer periodically until node has been shut down
00397   ros::Rate loop_rate(30); // Hz
00398   while(footprintObserver.nh_.ok()){
00399     footprintObserver.checkFootprint();
00400     ros::spinOnce();
00401     loop_rate.sleep();
00402   }
00403 
00404   return 0;
00405 };


cob_footprint_observer
Author(s): Matthias Gruhler
autogenerated on Sun Oct 5 2014 23:03:46