$search
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 { 00061 nh_ = ros::NodeHandle("~"); 00062 00063 m_mutex = PTHREAD_MUTEX_INITIALIZER; 00064 00065 // publish footprint 00066 topic_pub_footprint_ = nh_.advertise<geometry_msgs::PolygonStamped>("adjusted_footprint",1); 00067 00068 // advertise service 00069 srv_get_footprint_ = nh_.advertiseService("/get_footprint", &FootprintObserver::getFootprintCB, this); 00070 00071 // read footprint_source parameter 00072 std::string footprint_source; 00073 if(!nh_.hasParam("footprint_source")) ROS_WARN("Checking default location (/local_costmap_node/costmap) for initial footprint parameter."); 00074 nh_.param("footprint_source", footprint_source, std::string("/local_costmap_node/costmap")); 00075 00076 // node handle to get footprint from parameter server 00077 ros::NodeHandle footprint_source_nh_(footprint_source); 00078 00079 // load the robot footprint from the parameter server if its available in the local costmap namespace 00080 robot_footprint_ = loadRobotFootprint(footprint_source_nh_); 00081 if(robot_footprint_.size() > 4) 00082 ROS_WARN("You have set more than 4 points as robot_footprint, cob_footprint_observer can deal only with rectangular footprints so far!"); 00083 00084 // get the frames for which to check the footprint 00085 if(!nh_.hasParam("frames_to_check")) ROS_WARN("No frames to check for footprint observer. Only using initial footprint!"); 00086 nh_.param("frames_to_check", frames_to_check_, std::string("")); 00087 00088 if(!nh_.hasParam("robot_base_frame")) ROS_WARN("No parameter robot_base_frame on parameter server. Using default [/base_link]."); 00089 nh_.param("robot_base_frame", robot_base_frame_, std::string("/base_link")); 00090 00091 last_tf_missing_ = ros::Time::now(); 00092 } 00093 00094 // Destructor 00095 FootprintObserver::~FootprintObserver() 00096 {} 00097 00098 // GetFootprint Service callback 00099 bool FootprintObserver::getFootprintCB(cob_footprint_observer::GetFootprint::Request &req, cob_footprint_observer::GetFootprint::Response &resp) 00100 { 00101 // create Polygon message for service call 00102 geometry_msgs::PolygonStamped footprint_poly; 00103 footprint_poly.header.frame_id = robot_base_frame_; 00104 footprint_poly.header.stamp = ros::Time::now(); 00105 00106 footprint_poly.polygon.points.resize(robot_footprint_.size()); 00107 for(unsigned int i=0; i<robot_footprint_.size(); ++i) { 00108 footprint_poly.polygon.points[i].x = robot_footprint_[i].x; 00109 footprint_poly.polygon.points[i].y = robot_footprint_[i].y; 00110 footprint_poly.polygon.points[i].z = robot_footprint_[i].z; 00111 } 00112 00113 resp.footprint = footprint_poly; 00114 resp.success.data = true; 00115 00116 return true; 00117 } 00118 00119 // load robot footprint from costmap_2d_ros to keep same footprint format 00120 std::vector<geometry_msgs::Point> FootprintObserver::loadRobotFootprint(ros::NodeHandle node){ 00121 std::vector<geometry_msgs::Point> footprint; 00122 geometry_msgs::Point pt; 00123 double padding; 00124 00125 std::string padding_param, footprint_param; 00126 if(!node.searchParam("footprint_padding", padding_param)) 00127 padding = 0.01; 00128 else 00129 node.param(padding_param, padding, 0.01); 00130 00131 //grab the footprint from the parameter server if possible 00132 XmlRpc::XmlRpcValue footprint_list; 00133 std::string footprint_string; 00134 std::vector<std::string> footstring_list; 00135 if(node.searchParam("footprint", footprint_param)){ 00136 node.getParam(footprint_param, footprint_list); 00137 if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) { 00138 footprint_string = std::string(footprint_list); 00139 00140 //if there's just an empty footprint up there, return 00141 if(footprint_string == "[]" || footprint_string == "") 00142 return footprint; 00143 00144 boost::erase_all(footprint_string, " "); 00145 00146 boost::char_separator<char> sep("[]"); 00147 boost::tokenizer<boost::char_separator<char> > tokens(footprint_string, sep); 00148 footstring_list = std::vector<std::string>(tokens.begin(), tokens.end()); 00149 } 00150 //make sure we have a list of lists 00151 if(!(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_list.size() > 2) && !(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString && footstring_list.size() > 5)){ 00152 ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s", footprint_param.c_str(), std::string(footprint_list).c_str()); 00153 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]]"); 00154 } 00155 00156 if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray) { 00157 for(int i = 0; i < footprint_list.size(); ++i){ 00158 //make sure we have a list of lists of size 2 00159 XmlRpc::XmlRpcValue point = footprint_list[i]; 00160 if(!(point.getType() == XmlRpc::XmlRpcValue::TypeArray && point.size() == 2)){ 00161 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"); 00162 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"); 00163 } 00164 00165 //make sure that the value we're looking at is either a double or an int 00166 if(!(point[0].getType() == XmlRpc::XmlRpcValue::TypeInt || point[0].getType() == XmlRpc::XmlRpcValue::TypeDouble)){ 00167 ROS_FATAL("Values in the footprint specification must be numbers"); 00168 throw std::runtime_error("Values in the footprint specification must be numbers"); 00169 } 00170 pt.x = point[0].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[0]) : (double)(point[0]); 00171 pt.x += sign(pt.x) * padding; 00172 00173 //make sure that the value we're looking at is either a double or an int 00174 if(!(point[1].getType() == XmlRpc::XmlRpcValue::TypeInt || point[1].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.y = point[1].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[1]) : (double)(point[1]); 00179 pt.y += sign(pt.y) * padding; 00180 00181 footprint.push_back(pt); 00182 00183 node.deleteParam(footprint_param); 00184 std::ostringstream oss; 00185 bool first = true; 00186 BOOST_FOREACH(geometry_msgs::Point p, footprint) { 00187 if(first) { 00188 oss << "[[" << p.x << "," << p.y << "]"; 00189 first = false; 00190 } 00191 else { 00192 oss << ",[" << p.x << "," << p.y << "]"; 00193 } 00194 } 00195 oss << "]"; 00196 node.setParam(footprint_param, oss.str().c_str()); 00197 node.setParam("footprint", oss.str().c_str()); 00198 } 00199 } 00200 00201 else if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) { 00202 std::vector<geometry_msgs::Point> footprint_spec; 00203 bool valid_foot = true; 00204 BOOST_FOREACH(std::string t, footstring_list) { 00205 if( t != "," ) { 00206 boost::erase_all(t, " "); 00207 boost::char_separator<char> pt_sep(","); 00208 boost::tokenizer<boost::char_separator<char> > pt_tokens(t, pt_sep); 00209 std::vector<std::string> point(pt_tokens.begin(), pt_tokens.end()); 00210 00211 if(point.size() != 2) { 00212 ROS_WARN("Each point must have exactly 2 coordinates"); 00213 valid_foot = false; 00214 break; 00215 } 00216 00217 std::vector<double>tmp_pt; 00218 BOOST_FOREACH(std::string p, point) { 00219 std::istringstream iss(p); 00220 double temp; 00221 if(iss >> temp) { 00222 tmp_pt.push_back(temp); 00223 } 00224 else { 00225 ROS_WARN("Each coordinate must convert to a double."); 00226 valid_foot = false; 00227 break; 00228 } 00229 } 00230 if(!valid_foot) 00231 break; 00232 00233 geometry_msgs::Point pt; 00234 pt.x = tmp_pt[0]; 00235 pt.y = tmp_pt[1]; 00236 00237 footprint_spec.push_back(pt); 00238 } 00239 } 00240 if (valid_foot) { 00241 footprint = footprint_spec; 00242 node.setParam("footprint", footprint_string); 00243 } 00244 else { 00245 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()); 00246 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]]"); 00247 } 00248 } 00249 } 00250 00251 footprint_right_ = 0.0f; footprint_left_ = 0.0f; footprint_front_ = 0.0f; footprint_rear_ = 0.0f; 00252 //extract rectangular borders for simplifying: 00253 for(unsigned int i=0; i<footprint.size(); i++) { 00254 if(footprint[i].x > footprint_front_) footprint_front_ = footprint[i].x; 00255 if(footprint[i].x < footprint_rear_) footprint_rear_ = footprint[i].x; 00256 if(footprint[i].y > footprint_left_) footprint_left_ = footprint[i].y; 00257 if(footprint[i].y < footprint_right_) footprint_right_ = footprint[i].y; 00258 } 00259 ROS_DEBUG("Extracted rectangular footprint for cob_footprint_observer: Front: %f, Rear %f, Left: %f, Right %f", footprint_front_, footprint_rear_, footprint_left_, footprint_right_); 00260 00261 if ( fabs(footprint_front_ - footprint_rear_) == 0.0 || fabs(footprint_right_ - footprint_left_) == 0.0){ 00262 ROS_WARN("Footprint has no physical dimension!"); 00263 } 00264 00265 // save initial footprint 00266 footprint_front_initial_ = footprint_front_; 00267 footprint_rear_initial_ = footprint_rear_; 00268 footprint_left_initial_ = footprint_left_; 00269 footprint_right_initial_ = footprint_right_; 00270 00271 return footprint; 00272 } 00273 00274 // checks if footprint has to be adjusted and does so if necessary 00275 void FootprintObserver::checkFootprint(){ 00276 // check each frame 00277 std::string frame; 00278 std::stringstream ss; 00279 ss << frames_to_check_; 00280 00281 double x_rear, x_front, y_left, y_right; 00282 x_front = footprint_front_initial_; 00283 x_rear = footprint_rear_initial_; 00284 y_left = footprint_left_initial_; 00285 y_right = footprint_right_initial_; 00286 00287 bool missing_frame_exists = false; 00288 while(ss >> frame){ 00289 // get transform between robot base frame and frame 00290 if(tf_listener_.canTransform(robot_base_frame_, frame, ros::Time(0))) { 00291 tf::StampedTransform transform; 00292 tf_listener_.lookupTransform(robot_base_frame_, frame, ros::Time(0), transform); 00293 00294 tf::Vector3 frame_position = transform.getOrigin(); 00295 00296 // check if frame position is outside of current footprint 00297 if(frame_position.x() > x_front) x_front = frame_position.x(); 00298 if(frame_position.x() < x_rear) x_rear = frame_position.x(); 00299 if(frame_position.y() > y_left) y_left = frame_position.y(); 00300 if(frame_position.y() < y_right) y_right = frame_position.y(); 00301 } else if ( (ros::Time::now() - last_tf_missing_).toSec() > 5.0) { 00302 missing_frame_exists = true; 00303 ROS_WARN("Footprint Observer: Transformation for %s not available! Frame %s not considered in adjusted footprint!", 00304 frame.c_str(), frame.c_str()); 00305 } 00306 } 00307 if (missing_frame_exists) 00308 last_tf_missing_ = ros::Time::now(); 00309 00310 pthread_mutex_lock(&m_mutex); 00311 // adjust footprint 00312 footprint_front_ = x_front; 00313 footprint_rear_ = x_rear; 00314 footprint_left_ = y_left; 00315 footprint_right_ = y_right; 00316 pthread_mutex_unlock(&m_mutex); 00317 00318 // create new footprint vector 00319 geometry_msgs::Point point; 00320 std::vector<geometry_msgs::Point> points; 00321 00322 point.x = footprint_front_; 00323 point.y = footprint_left_; 00324 point.z = 0; 00325 points.push_back(point); 00326 point.y = footprint_right_; 00327 points.push_back(point); 00328 point.x = footprint_rear_; 00329 points.push_back(point); 00330 point.y = footprint_left_; 00331 points.push_back(point); 00332 00333 pthread_mutex_lock(&m_mutex); 00334 robot_footprint_ = points; 00335 pthread_mutex_unlock(&m_mutex); 00336 00337 // publish the adjusted footprint 00338 publishFootprint(); 00339 } 00340 00341 // publishes the adjusted footprint 00342 void FootprintObserver::publishFootprint(){ 00343 00344 // create Polygon message 00345 geometry_msgs::PolygonStamped footprint_poly; 00346 footprint_poly.header.frame_id = robot_base_frame_; 00347 footprint_poly.header.stamp = ros::Time::now(); 00348 00349 footprint_poly.polygon.points.resize(robot_footprint_.size()); 00350 for(unsigned int i=0; i<robot_footprint_.size(); ++i) { 00351 footprint_poly.polygon.points[i].x = robot_footprint_[i].x; 00352 footprint_poly.polygon.points[i].y = robot_footprint_[i].y; 00353 footprint_poly.polygon.points[i].z = robot_footprint_[i].z; 00354 } 00355 00356 // publish adjusted footprint 00357 topic_pub_footprint_.publish(footprint_poly); 00358 00359 } 00360 00361 // compute the sign of x 00362 double FootprintObserver::sign(double x){ 00363 if(x >= 0.0f) return 1.0f; 00364 else return -1.0f; 00365 } 00366 00367 //####################### 00368 //#### main programm #### 00369 int main(int argc, char** argv) 00370 { 00371 // initialize ROS, specify name of node 00372 ros::init(argc,argv,"footprint_observer"); 00373 00374 // create observer 00375 FootprintObserver footprintObserver; 00376 00377 // run footprint observer periodically until node has been shut down 00378 ros::Rate loop_rate(30); // Hz 00379 while(footprintObserver.nh_.ok()){ 00380 footprintObserver.checkFootprint(); 00381 ros::spinOnce(); 00382 loop_rate.sleep(); 00383 } 00384 00385 return 0; 00386 };