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.h 00050 * @brief observes the footprint of care-o-bot 00051 * 00052 * Generates rectangular footprint of care-o-bot depending on setup 00053 * of tray and arm. 00054 * 00055 ****************************************************************/ 00056 #ifndef COB_FOOTPRINT_OBSERVER_H 00057 #define COB_FOOTPRINT_OBSERVER_H 00058 00059 //################## 00060 //#### includes #### 00061 00062 // ROS includes 00063 #include <ros/ros.h> 00064 #include <XmlRpc.h> 00065 00066 // message includes 00067 #include <geometry_msgs/Point.h> 00068 #include <geometry_msgs/PolygonStamped.h> 00069 #include <geometry_msgs/Vector3.h> 00070 00071 #include <tf/transform_listener.h> 00072 00073 #include <boost/tokenizer.hpp> 00074 #include <boost/foreach.hpp> 00075 #include <boost/algorithm/string.hpp> 00076 00077 #include <cob_footprint_observer/GetFootprint.h> 00083 class FootprintObserver 00084 { 00085 public: 00089 FootprintObserver(); 00093 ~FootprintObserver(); 00094 00098 void checkFootprint(); 00099 00106 bool getFootprintCB(cob_footprint_observer::GetFootprint::Request &req, cob_footprint_observer::GetFootprint::Response &resp); 00107 00108 // public members 00109 ros::NodeHandle nh_; 00110 ros::Publisher topic_pub_footprint_; 00111 ros::ServiceServer srv_get_footprint_; 00112 00113 private: 00119 std::vector<geometry_msgs::Point> loadRobotFootprint(ros::NodeHandle node); 00120 00124 void publishFootprint(); 00125 00131 double sign(double x); 00132 00133 // private members 00134 std::vector<geometry_msgs::Point> robot_footprint_; 00135 double epsilon_; 00136 double footprint_front_initial_, footprint_rear_initial_, footprint_left_initial_, footprint_right_initial_; 00137 double footprint_front_, footprint_rear_, footprint_left_, footprint_right_; 00138 tf::TransformListener tf_listener_; 00139 std::string frames_to_check_; 00140 std::string robot_base_frame_; 00141 00142 pthread_mutex_t m_mutex; 00143 00144 ros::Time last_tf_missing_; 00145 unsigned int times_warned_; 00146 }; 00147 00148 #endif