00001 /****************************************************************************** 00002 STDR Simulator - Simple Two DImensional Robot Simulator 00003 Copyright (C) 2013 STDR Simulator 00004 This program is free software; you can redistribute it and/or modify 00005 it under the terms of the GNU General Public License as published by 00006 the Free Software Foundation; either version 3 of the License, or 00007 (at your option) any later version. 00008 This program is distributed in the hope that it will be useful, 00009 but WITHOUT ANY WARRANTY; without even the implied warranty of 00010 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00011 GNU General Public License for more details. 00012 You should have received a copy of the GNU General Public License 00013 along with this program; if not, write to the Free Software Foundation, 00014 Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 00015 00016 Authors : 00017 * Manos Tsardoulias, etsardou@gmail.com 00018 * Aris Thallas, aris.thallas@gmail.com 00019 * Chris Zalidis, zalidis@gmail.com 00020 ******************************************************************************/ 00021 00022 #ifndef ROBOT_H 00023 #define ROBOT_H 00024 00025 #include <ros/ros.h> 00026 #include <nodelet/nodelet.h> 00027 #include <tf/transform_broadcaster.h> 00028 #include <stdr_msgs/RobotMsg.h> 00029 #include <stdr_msgs/MoveRobot.h> 00030 #include <stdr_robot/sensors/sensor_base.h> 00031 #include <stdr_robot/sensors/laser.h> 00032 #include <stdr_robot/sensors/sonar.h> 00033 #include <stdr_robot/sensors/rfid_reader.h> 00034 #include <stdr_robot/sensors/co2.h> 00035 #include <stdr_robot/sensors/microphone.h> 00036 #include <stdr_robot/sensors/thermal.h> 00037 #include <stdr_robot/motion/motion_controller_base.h> 00038 #include <stdr_robot/motion/ideal_motion_controller.h> 00039 #include <nav_msgs/OccupancyGrid.h> 00040 #include <nav_msgs/Odometry.h> 00041 #include <actionlib/client/simple_action_client.h> 00042 #include <stdr_msgs/RegisterRobotAction.h> 00043 00048 namespace stdr_robot { 00049 00050 typedef actionlib::SimpleActionClient<stdr_msgs::RegisterRobotAction> 00051 RegisterRobotClient; 00052 typedef boost::shared_ptr<RegisterRobotClient> RegisterRobotClientPtr; 00053 00058 class Robot : public nodelet::Nodelet { 00059 00060 public: 00061 00066 Robot(void); 00067 00072 void onInit(void); 00073 00080 void initializeRobot(const actionlib::SimpleClientGoalState& state, 00081 const stdr_msgs::RegisterRobotResultConstPtr result); 00082 00088 void mapCallback(const nav_msgs::OccupancyGridConstPtr& msg); 00089 00096 bool moveRobotCallback(stdr_msgs::MoveRobot::Request& req, 00097 stdr_msgs::MoveRobot::Response& res); 00098 00103 ~Robot(void); 00104 00105 private: 00106 00111 bool collisionExists( 00112 const geometry_msgs::Pose2D& newPose, 00113 const geometry_msgs::Pose2D& collisionPoint); 00114 00115 bool collisionExistsNoPath( 00116 const geometry_msgs::Pose2D& newPose); 00117 00123 bool checkUnknownOccupancy(const geometry_msgs::Pose2D& newPose); 00124 00129 void publishTransforms(const ros::TimerEvent&); 00130 00131 00132 private: 00133 00135 ros::Subscriber _mapSubscriber; 00136 00138 ros::Timer _tfTimer; 00139 00141 ros::ServiceServer _moveRobotService; 00142 00144 SensorPtrVector _sensors; 00145 00147 nav_msgs::OccupancyGrid _map; 00148 00150 tf::TransformBroadcaster _tfBroadcaster; 00151 00153 ros::Publisher _odomPublisher; 00154 00156 geometry_msgs::Pose2D _currentPose; 00157 00159 geometry_msgs::Pose2D _previousPose; 00160 00162 MotionControllerPtr _motionControllerPtr; 00163 00165 RegisterRobotClientPtr _registerClientPtr; 00166 00168 std::vector<std::pair<float,float> > _footprint; 00169 00170 std::vector<std::pair<int,int> > getPointsBetween( 00171 int x1, int y1, int x2, int y2) ; 00172 00174 bool _previousMovementXAxis; 00175 00177 bool _previousMovementYAxis; 00178 }; 00179 00180 } // namespace stdr_robot 00181 00182 00183 #endif