PatrolAgent.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2014, ISR University of Coimbra.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the ISR University of Coimbra nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Luca Iocchi (2014-2016)
00036 *********************************************************************/
00037 
00038 #include <sstream>
00039 #include <string>
00040 #include <vector>
00041 #include <ros/ros.h>
00042 #include <move_base_msgs/MoveBaseAction.h>
00043 #include <actionlib/client/simple_action_client.h>
00044 #include <tf/transform_broadcaster.h>
00045 #include <tf/transform_listener.h>
00046 #include <nav_msgs/Odometry.h>
00047 #include <std_msgs/Int16MultiArray.h>
00048 
00049 
00050 #include "getgraph.h"
00051 
00052 #define NUM_MAX_ROBOTS 32
00053 #define INTERFERENCE_DISTANCE 2
00054 
00055 #include "message_types.h"
00056 
00057 typedef unsigned int uint;
00058 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
00059 
00060 class PatrolAgent {
00061 
00062 protected:
00063     
00064     int TEAMSIZE;
00065     int ID_ROBOT;
00066 
00067     double xPos[NUM_MAX_ROBOTS]; //tabelas de posições (atençao ao index pro caso de 1 so robot)
00068     double yPos[NUM_MAX_ROBOTS]; //tabelas de posições (atençao ao index pro caso de 1 so robot)
00069 
00070     tf::TransformListener *listener;
00071 
00072     std::string graph_file, mapname;
00073     uint dimension; // Graph Dimension
00074     uint current_vertex; // current vertex
00075     bool ResendGoal; // Send the same goal again (if goal failed...)
00076     bool interference;
00077     double last_interference;
00078     bool goal_complete;
00079     bool initialize;
00080     bool end_simulation;
00081     int next_vertex;
00082     // uint backUpCounter;
00083     vertex *vertex_web;
00084     double *instantaneous_idleness;  // local idleness
00085     double *last_visit;
00086     std::vector<int> vresults; // results exchanged among robots
00087     bool goal_canceled_by_user;
00088     double goal_reached_wait, communication_delay, last_communication_delay_time, lost_message_rate;
00089     std::string initial_positions;
00090     int aborted_count, resend_goal_count;
00091     
00092     MoveBaseClient *ac; // action client for reaching target goals
00093     
00094     ros::Subscriber odom_sub, positions_sub;
00095     ros::Publisher positions_pub;
00096     ros::Subscriber results_sub;
00097     ros::Publisher results_pub;
00098     ros::Publisher cmd_vel_pub;
00099 
00100     
00101 public:
00102     
00103     PatrolAgent() { 
00104         listener=NULL;
00105         next_vertex = -1;
00106         initialize = true;
00107         end_simulation = false;
00108         ac = NULL;
00109     }
00110     
00111     virtual void init(int argc, char** argv);
00112     void ready();
00113     void initialize_node();
00114     void readParams(); // read ROS parameters
00115     void update_idleness();  // local idleness
00116     
00117     virtual void run();
00118     
00119     void getRobotPose(int robotid, float &x, float &y, float &theta);
00120     void odomCB(const nav_msgs::Odometry::ConstPtr& msg);
00121     
00122     void sendGoal(int next_vertex);
00123     void cancelGoal();
00124     
00125     void goalDoneCallback(const actionlib::SimpleClientGoalState &state, const move_base_msgs::MoveBaseResultConstPtr &result);
00126     void goalActiveCallback();
00127     void goalFeedbackCallback(const move_base_msgs::MoveBaseFeedbackConstPtr &feedback);
00128 
00129     
00130     void send_goal_reached();
00131     bool check_interference (int ID_ROBOT);
00132     void do_interference_behavior();
00133     void backup();
00134     
00135     void onGoalNotComplete(); // what to do when a goal has NOT been reached (aborted)
00136     
00137     // Events
00138     virtual void onGoalComplete(); // what to do when a goal has been reached
00139     virtual void processEvents();  // processes algorithm-specific events
00140     
00141     // Robot-Robot Communication
00142     void send_positions();
00143     void receive_positions();
00144     virtual void send_results();  // when goal is completed
00145     virtual void receive_results();  // asynchronous call
00146     void do_send_message(std_msgs::Int16MultiArray &msg);
00147     void send_interference();
00148     void positionsCB(const nav_msgs::Odometry::ConstPtr& msg);
00149     void resultsCB(const std_msgs::Int16MultiArray::ConstPtr& msg);
00150     
00151     // Must be implemented by sub-classes
00152     virtual int compute_next_vertex() = 0;
00153 
00154 };
00155 
00156 


patrolling_sim
Author(s):
autogenerated on Thu Jun 6 2019 20:27:09