motivational_behavior.h
Go to the documentation of this file.
00001 /*************************************************************************************************************************
00002  * Author:         Minglong Li
00003  * Affiliation:    State Key Laboratory of High Performance Computing (HPCL), College of Computer, National University of Defense Technology
00004  * Email:            minglong_l@163.com
00005  * Created on:  Sep. 5th, 2016
00006  * Description:  In ALLIANCE, the ability for robots to respond to unexpected events, robot failures, and so forth, is 
00007  *                        provided through the use of motivations. These motivations are designed to allow robot team members
00008  *                        to perform tasks only as long as they demonstrate their ability to have the desired effect on the world.
00009  *                        This differs from the commonly used technique for task allocation that begins with breaking down the 
00010  *                        mission (or part of the mission) into subtasks, and then computing the "optimal" robot-to-task mapping 
00011  *                        based upon agent skill levels, with little recourse for robot failures after the allocation has occurred.                
00012  **************************************************************************************************************************/
00013 #ifndef MOTIVATIONAL_BEHAVIOR_H_
00014 #define MOTIVATIONAL_BEHAVIOR_H_
00015 
00016 #include <iostream>
00017 #include <ros/ros.h>
00018 #include <nodelet/nodelet.h>
00019 #include <pluginlib/class_list_macros.h>
00020 #include <std_msgs/Bool.h>
00021 #include <micros_mars_task_alloc/Heartbeat.h>
00022 #include <vector>
00023 
00024 namespace micros_mars_task_alloc{
00025     using namespace std;
00026     
00027     template<typename VS_MsgType>//the sensory_feedback message type of the 'Virtual Sensory' messages.
00028     class MotivationalBehavior : public nodelet::Nodelet
00029     {
00030     public:
00031         MotivationalBehavior(): motivation_(0), sensory_feedback_(1), sensory_feedback_exist_(false), maximum_time_step_(100000), active_time_duration_(0),  one_cycle_(0.1),
00032                                              delta_fast_(10), delta_slow_(0.01), threshold_(1000), impatience_reset_(1), activity_suppression_(1){}
00033         virtual ~MotivationalBehavior(){}
00034         virtual void onInit()
00035         {
00036             nh_ = getMTPrivateNodeHandle();
00037             if (!(nh_.getParam("robot_number", robot_number_)))
00038             {
00039                 std::cout << "Fail to get the parameter robot_number." << std::endl;
00040                 return;
00041             }           
00042             if (!(nh_.getParam("behavior_set_number", behavior_set_number_)))
00043             {
00044                 std::cout << "Fail to get the parameter behavior_set_number." << std::endl;
00045                 return;
00046             }     
00047             if(!(nh_.getParam("sensory_feedback_topic", sensory_feedback_topic_)))
00048             {
00049                 std::cout << "Fail to get the parameter sensory_feedback_topic." << std::endl;
00050                 return;
00051             }
00052             if(!(nh_.getParam("intra_robot_comm_topic", intra_robot_comm_topic_)))
00053             {
00054                 std::cout << "Fail to get the parameter intra_robot_comm_topic." << std::endl;
00055                 return;
00056             }
00057             if(!(nh_.getParam("inter_robot_comm_topic", inter_robot_comm_topic_)))
00058             {
00059                 std::cout << "Fail to get the parameter inter_robot_comm_topic." << std::endl;
00060                 return;
00061             }
00062             if(!(nh_.getParam("forward_topic", forward_topic_)))
00063             {
00064                 std::cout << "Fail to get the parameter forward_topic." << std::endl;
00065                 return;
00066             }
00067             //initialise the vector heartbeat_, the row number is robot_number_ and the column number is maximum_time_step_.
00068             //heartbeat_ is a 2-dimision vector.
00069             heartbeat_.resize(robot_number_);
00070             for (int i = 0; i < heartbeat_.size(); i++)
00071             {
00072                 heartbeat_.resize(maximum_time_step_);
00073             }
00074             //Set all the elements in heartbeat_ to zero.
00075             for (int i = 0; i < heartbeat_.size(); i++)
00076             {
00077                 for (int j = 0; j < heartbeat_[0].size(); j++)
00078                 {
00079                     heartbeat_[i][j] = false;
00080                 }
00081             }
00082             //initialise the heartbeat_count,  heartbeat_count _  is  a floating point-to-reference, which is used for the ring vector.
00083             heartbeat_count_.resize(robot_number_);
00084             for (int i = 0; i < heartbeat_count_.size(); i++)
00085             {
00086                 heartbeat_count_[i] = 0;
00087             }
00088             /*
00089             //initialise the vector comm_received and comm_received_timestamp_, the default value of the elements in this vector is set to 0.
00090             for (int i = 0; i < robot_number_; i++)
00091             {
00092                 comm_received_.push_back(0);
00093                 comm_received_timestamp_.push_back(0);
00094             }*/
00095             pub_intra_heartbeat_ = nh_.advertise<std_msgs::Bool>(intra_robot_comm_topic_, 10);
00096             pub_inter_heartbeat_ = nh_.advertise<std_msgs::Bool>(inter_robot_comm_topic_, 10);
00097             pub_forward_ = nh_.advertise<std_msgs::Bool>(forward_topic_, 10);
00098             sub_0_ = nh_.subscribe(sensory_feedback_topic_, 10, &MotivationalBehavior::sensory_feedback_callback, this);//sub_0_ is used to handle the sensory_feedback_ messages.
00099             sub_1_ = nh_.subscribe(inter_robot_comm_topic_, 10, &MotivationalBehavior::inter_robot_comm_callback, this);//sub_1_ is used to handle the inter-robot communicating messages.
00100             sub_2_ = nh_.subscribe(intra_robot_comm_topic_, 10, &MotivationalBehavior::intra_robot_comm_callback, this);//sub_2_ is used to hangle the intra-robot communicating messages.
00101             main_periodic_timer_ = nh_.createTimer(ros::Duration(one_cycle_), &MotivationalBehavior::main_logic_callback, this);//control the main logic.
00102         }
00103 
00104         void sensory_feedback_callback(const boost::shared_ptr<const VS_MsgType> & msg)
00105         {
00106             //Assumption: the sensory_feedback message flow is stable, and the publishing rate is larger than 1 Hz.
00107             std::cout << "sensory_feedback_callback start!" << std::endl;
00108             sensory_feedback_exist_ = true;
00109             sensory_feedback_timestamp_ = ros::Time::now().toSec();//if the sensory_feedback is received, even only once, this flag paramter will be set to true.
00110             sensory_feedback_ = 1;
00111         }
00112         void inter_robot_comm_callback(const micros_mars_task_alloc::HeartbeatConstPtr & msg)
00113         {
00114             std::cout << "inter_robot_comm_callback start!" << std::endl;
00115             if( (msg->robot_ID != this_robot_ID_)&&(msg->behavior_set_ID == this_behavior_set_ID_) )
00116             {
00117                 heartbeat_[msg->robot_ID][ heartbeat_count_[msg->robot_ID] ] = msg->heartbeat;
00118                 heartbeat_count_[msg->robot_ID]+=1;
00119                 if(heartbeat_count_[msg->robot_ID] == maximum_time_step_)
00120                     heartbeat_count_[msg->robot_ID] = 0;
00121             }
00122         }
00123         void intra_robot_comm_callback(const micros_mars_task_alloc::HeartbeatConstPtr & msg)
00124         {
00125             std::cout << "intra_robot_comm_callback start!" << std::endl;
00126             //if any other behavior set in this robot is activated, and the robot_ID in the messages must be "this_robot_ID_"
00127             if((msg->heartbeat==true) && (msg->robot_ID == this_robot_ID_) && (msg->behavior_set_ID != this_behavior_set_ID_))
00128                 activity_suppression_ = 0;
00129             else//none of other behavior set in this robot is activated.
00130                 activity_suppression_ = 1;
00131         }
00132         void impatience_reset_calc()//This function should be put into the main logic function.
00133         {
00134             //impatience reset: when this robot hears about another robot performing this behavior set.
00135             bool flag = false;
00136             bool flag1, flag2;
00137             for (int i = 0; i < robot_number_; i ++)
00138             {
00139                 if (i == this_robot_ID_) continue;
00140                 int current  = heartbeat_count_[i];
00141                 flag1 = true;
00142                 for (int j = 1; j < current; j++)
00143                 {
00144                     if (heartbeat_[i][j] == true)
00145                         flag1 = false;
00146                 }
00147                 flag2 = false;
00148                 if (heartbeat_[i][current] == true)
00149                     flag2 = true;
00150                 if (flag1 && flag2)
00151                 {
00152                     flag = true;
00153                     break;
00154                 }
00155             }
00156             if (flag == true)
00157                 impatience_reset_ = 1;
00158             else
00159                 impatience_reset_ = 0;
00160         }
00161         void impatience_calc()//This function should be put into the main logic function.
00162         {
00163             bool delta_fast_flag = true;
00164             for (int i = 0; i < robot_number_; i++)//i is robot_ID
00165             {
00166                 if(i == this_robot_ID_) break;
00167                 int current = heartbeat_count_[i];
00168                 if(heartbeat_[i][current] == true)
00169                 {
00170                     delta_fast_flag = false;
00171                     break;
00172                 }
00173             }
00174             if(delta_fast_flag)
00175                 impatience_ = delta_fast_;
00176             else
00177                 impatience_ = delta_slow_;                        
00178         }
00179         void acquiescence_calc()
00180         {
00181             cout << "acquiescence_calc start" << endl;     
00182             double phi = 200.0;
00183             double lamda = 300.0; //behavior set aij of robot ri has been active for more than lamda time.
00184             if (active_ == 0)
00185                 acquiescence_ = 1;// It means that this beahvior set in this robot is not active.
00186             else
00187             {
00188                  //double current_time = ros::Time::now().toSec();//current_time is a time_stamp.
00189                  if ( active_time_duration_ > lamda)
00190                     acquiescence_ = 0;
00191                 else
00192                 {
00193                     bool flag = false;
00194                     bool flag1 = false;
00195                     if (active_time_duration_ > phi)
00196                         flag1 ==true;
00197                     bool flag2 = false;
00198                     for(int i = 0; i < robot_number_; i++)
00199                     {
00200                         if (i == this_robot_ID_) continue;
00201                         int current = heartbeat_count_[i];
00202                         if(heartbeat_[i][current] == true)
00203                         {
00204                             flag2 = true;
00205                             break;
00206                         }
00207                     }
00208                     flag = flag1 && flag2;
00209                     if(flag) 
00210                         acquiescence_ = 0;
00211                     else
00212                         acquiescence_ = 1;
00213                 }
00214             }
00215         }
00216         void send_inter_heartbeat(bool heartbeat)
00217         {
00218             micros_mars_task_alloc::HeartbeatPtr heartbeat_ptr(new micros_mars_task_alloc::Heartbeat);
00219             heartbeat_ptr -> robot_ID = this_robot_ID_;
00220             heartbeat_ptr -> behavior_set_ID = this_behavior_set_ID_;
00221             heartbeat_ptr -> heartbeat = heartbeat;
00222             pub_inter_heartbeat_.publish(heartbeat_ptr);
00223         }
00224         void send_intra_heartbeat(bool heartbeat)
00225         {
00226             micros_mars_task_alloc::HeartbeatPtr heartbeat_ptr(new micros_mars_task_alloc::Heartbeat);
00227             heartbeat_ptr -> robot_ID = this_robot_ID_;
00228             heartbeat_ptr -> behavior_set_ID = this_behavior_set_ID_;
00229             heartbeat_ptr -> heartbeat = heartbeat;
00230             pub_intra_heartbeat_.publish(heartbeat_ptr);
00231         }
00232         void main_logic_callback(const ros::TimerEvent&)
00233         {
00234             double update_time;
00235             update_time = ros::Time::now().toSec();
00236             std_msgs::BoolPtr bool_ptr(new std_msgs::Bool);
00237             if ((sensory_feedback_exist_) && (update_time - sensory_feedback_timestamp_ > 1.0))//The time duration for the sensory feedback is 1.0 sensond, the '1.0' can control the real-time efficiency.
00238             {
00239                 sensory_feedback_ = 0;
00240             }
00241             impatience_calc();
00242             impatience_reset_calc();
00243             acquiescence_calc();
00244             motivation_ = (motivation_ + impatience_) * sensory_feedback_ * activity_suppression_ * impatience_reset_ * acquiescence_;
00245             if(motivation_ > threshold_) 
00246                 motivation_ = threshold_;
00247             if(motivation_ == threshold_)
00248              {
00249                 active_ = true;
00250                 active_time_duration_ += one_cycle_;
00251                 bool_ptr -> data = true;
00252                 send_intra_heartbeat(true);
00253                 send_inter_heartbeat(true);
00254                 pub_forward_.publish(bool_ptr);
00255              } 
00256              else
00257              {
00258                 active_ = false;
00259                 active_time_duration_ = 0;
00260                 bool_ptr -> data  = false;
00261                 send_inter_heartbeat(false);
00262                 send_inter_heartbeat(false);
00263                 pub_forward_.publish(bool_ptr);
00264              }
00265         }
00266     private:    
00267         ros::NodeHandle nh_;
00268         ros::Timer main_periodic_timer_;
00269         ros::Subscriber sub_0_;
00270         ros::Subscriber sub_1_;
00271         ros::Subscriber sub_2_;
00272         ros::Publisher pub_intra_heartbeat_;//publish heartbeat messages in this robot, which is used for the control of activity_suppression.
00273         ros::Publisher pub_inter_heartbeat_;//publish heartbeat messages among team robots, which is used for the control of comm_received.
00274         ros::Publisher pub_forward_;
00275         
00276         int this_robot_ID_;//the ID of this robot, to be set by the paramters in the launch file.
00277         int this_behavior_set_ID_;//the ID of this behavior set in this robot, to be set by the parameters in the launch file.
00278         int behavior_set_number_;//the total number of the behavior sets, which is set by the local parameters
00279         int robot_number_;//the total number of the robots, which is set by the local parameters 
00280         int maximum_time_step_;
00281 
00282         std::string sensory_feedback_topic_;
00283         std::string inter_robot_comm_topic_;
00284         std::string intra_robot_comm_topic_;
00285         std::string forward_topic_;
00286     
00287         //Some parameters used to calculate the motivation, TODO: these may be used as local parameters.
00288         double motivation_;
00289         double impatience_;
00290         double acquiescence_;
00291         double sensory_feedback_;//TODO: the default value of sensory feedback is 1.
00292         double sensory_feedback_timestamp_;
00293         
00294         bool sensory_feedback_exist_;//Not all motivational behavior exist the sensory feedback item, the default value of this parameter is false, which means it does not exist.
00295         double activity_suppression_;//if any other behavior set in this robot is activated, the motivation of this behavior set is set to 0. 
00296         double impatience_reset_;
00297         
00298         bool active_;
00299         double active_time_duration_;
00300         double one_cycle_;
00301         
00302         double delta_slow_;//when another robot is doing this work, the increasing velocity of impatience in this motivational behavior is delta_slow_
00303         double delta_fast_;//when none of the robots is doing this work, the increasing velocity of impatience in this motivational behavior is delta_fast_
00304         double threshold_;
00305         
00306         vector<int> heartbeat_count_;
00307         vector< vector<bool> > heartbeat_;
00308 
00309         //comm_received was omitted in this version.
00310         //vector<double> comm_received_;//The size of this vector is robot_number, we don't need to know
00311         //vector<double> comm_received_timestamp_;//the vector to log the timestamp.
00312     }; 
00313     //typedef micros_mars_task_alloc::MotivationalBehavior<std_msgs::Bool> MotivationalBehaviorTest;
00314 }//namespace micros_mars_task_alloc
00315 //PLUGINLIB_EXPORT_CLASS(micros_mars_task_alloc::MotivationalBehaviorTest, nodelet::Nodelet)
00316 #endif


micros_mars_task_alloc
Author(s): Minglong Li , Xiaodong Yi , Yanzhen Wang , Zhongxuan Cai
autogenerated on Mon Jul 1 2019 19:55:03