00001 // Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC. 00002 // Author 00003 // All rights reserved. 00004 // 00005 // This file is part of iri-ros-pkg 00006 // iri-ros-pkg is free software: you can redistribute it and/or modify 00007 // it under the terms of the GNU Lesser General Public License as published by 00008 // the Free Software Foundation, either version 3 of the License, or 00009 // at your option) any later version. 00010 // 00011 // This program is distributed in the hope that it will be useful, 00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 // GNU Lesser General Public License for more details. 00015 // 00016 // You should have received a copy of the GNU Lesser General Public License 00017 // along with this program. If not, see <http://www.gnu.org/licenses/>. 00018 // 00019 // IMPORTANT NOTE: This code has been generated through a script from the 00020 // iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness 00021 // of the scripts. ROS topics can be easly add by using those scripts. Please 00022 // refer to the IRI wiki page for more information: 00023 // http://wikiri.upc.es/index.php/Robotics_Lab 00024 00025 #ifndef _ry_oa_bridge_alg_node_h_ 00026 #define _ry_oa_bridge_alg_node_h_ 00027 00028 #include <iri_base_algorithm/iri_base_algorithm.h> 00029 #include "ry_oa_bridge_alg.h" 00030 00031 //yarp headers 00032 #include <yarp/os/Bottle.h> 00033 #include <yarp/os/BufferedPort.h> 00034 #include <yarp/os/Network.h> 00035 00036 // [publisher subscriber headers] 00037 #include <geometry_msgs/PoseWithCovarianceStamped.h> 00038 #include <iri_ry_oa_bridge/oaStatus.h> 00039 #include <geometry_msgs/Twist.h> 00040 #include <nav_msgs/Odometry.h> 00041 #include <sensor_msgs/LaserScan.h> 00042 00043 // [service client headers] 00044 00045 // [action server client headers] 00046 00047 using namespace yarp::os; 00048 00049 class CvelocityCommandPort : public BufferedPort<Bottle> 00050 { 00051 protected: 00052 double vv,ww; 00053 CMutex velocities_mutex_; 00054 00055 public: 00056 virtual void onRead(Bottle& bot) //callback function 00057 { 00058 unsigned int ii=0; 00059 velocities_mutex_.enter(); 00060 vv = bot.get(ii++).asDouble(); 00061 ww = bot.get(ii++).asDouble(); 00062 velocities_mutex_.exit(); 00063 } 00064 double getV() 00065 { 00066 velocities_mutex_.enter(); 00067 return vv; 00068 velocities_mutex_.exit(); 00069 } 00070 double getW() 00071 { 00072 velocities_mutex_.enter(); 00073 return ww; 00074 velocities_mutex_.exit(); 00075 } 00076 }; 00077 00078 class CoaStatusPort : public BufferedPort<Bottle> 00079 { 00080 protected: 00081 int status, state; 00082 CMutex st_mutex_; 00083 00084 public: 00085 virtual void onRead(Bottle& bot) //callback function 00086 { 00087 unsigned int ii=0; 00088 st_mutex_.enter(); 00089 status = bot.get(ii++).asInt(); 00090 state = bot.get(ii++).asInt(); 00091 st_mutex_.exit(); 00092 } 00093 int getStatus() 00094 { 00095 st_mutex_.enter(); 00096 return status; 00097 st_mutex_.exit(); 00098 } 00099 int getState() 00100 { 00101 st_mutex_.enter(); 00102 return state; 00103 st_mutex_.exit(); 00104 } 00105 }; 00106 00111 class RyOaBridgeAlgNode : public algorithm_base::IriBaseAlgorithm<RyOaBridgeAlgorithm> 00112 { 00113 private: 00114 // [publisher attributes] 00115 ros::Publisher status_publisher_; 00116 iri_ry_oa_bridge::oaStatus oaStatus_msg_; 00117 ros::Publisher command_publisher_; 00118 geometry_msgs::Twist Twist_msg_; 00119 00120 // [subscriber attributes] 00121 ros::Subscriber localization_subscriber_; 00122 void localization_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg); 00123 CMutex localization_mutex_; 00124 ros::Subscriber odometry_subscriber_; 00125 void odometry_callback(const nav_msgs::Odometry::ConstPtr& msg); 00126 CMutex odometry_mutex_; 00127 ros::Subscriber vertical_laser_subscriber_; 00128 void vertical_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg); 00129 CMutex vertical_laser_mutex_; 00130 ros::Subscriber front_laser_subscriber_; 00131 void front_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg); 00132 CMutex front_laser_mutex_; 00133 00134 // [service attributes] 00135 00136 // [client attributes] 00137 00138 // [action server attributes] 00139 00140 // [action client attributes] 00141 00142 //yarp comm's. In/Out tag from the point of view of this node 00143 BufferedPort<Bottle> yarpOdometryOut; 00144 BufferedPort<Bottle> yarpVerticalLaserOut; 00145 BufferedPort<Bottle> yarpFrontLaserOut; 00146 BufferedPort<Bottle> yarpLocalizationOut; 00147 CvelocityCommandPort yarpVelocitiesIn; 00148 CoaStatusPort yarpOaStatusIn; 00149 00150 double lastOdoTs; //last odometry time stamp; 00151 00152 public: 00159 RyOaBridgeAlgNode(void); 00160 00167 ~RyOaBridgeAlgNode(void); 00168 00169 protected: 00182 void mainNodeThread(void); 00183 00196 void node_config_update(Config &config, uint32_t level); 00197 00204 void addNodeDiagnostics(void); 00205 00206 // [diagnostic functions] 00207 00208 // [test functions] 00209 }; 00210 00211 #endif