From 7530b853fec63a4607a1b0670bd77f0e5fbd8226 Mon Sep 17 00:00:00 2001 From: ericsenn Date: Fri, 21 May 2021 17:20:35 +0200 Subject: [PATCH] new features in master_wip : LOCATE, 8 GOALS --- src/master_wip.cpp | 206 ++++++++++++++++++++++++++++++++++----------- 1 file changed, 156 insertions(+), 50 deletions(-) diff --git a/src/master_wip.cpp b/src/master_wip.cpp index 4702533..00fe5c3 100644 --- a/src/master_wip.cpp +++ b/src/master_wip.cpp @@ -20,7 +20,7 @@ using namespace std; typedef actionlib::SimpleActionClient MoveBaseClient; // GLOBAL VARIABLES DECLARATION -bool SET, GOTO, ABORT, STOP, BOOLA, BOOLB, BOOLC, BOOLD; +bool SET, GOTO, ABORT, STOP, LOCATE, BOOLA, BOOLB, BOOLC, BOOLD, BOOLE, BOOLF, BOOLG, BOOLH; // global variables needed to expose the position data outside the // recv pose from amcl callback function @@ -99,6 +99,12 @@ void ABORTCallback(const std_msgs::Bool& inbool){ABORT = inbool.data;} void STOPCallback(const std_msgs::Bool& inbool){STOP = inbool.data;} //_____________________________________________________________ +//************************************************************ +//callback for subscribing to the boolean message +//linked to the press button on the ROS Mobile app +void LOCATECallback(const std_msgs::Bool& inbool){LOCATE = inbool.data;} +//_____________________________________________________________ + //************************************************************ //callback for subscribing to the boolean message //linked to the press button on the ROS Mobile app @@ -111,26 +117,47 @@ void ACallback(const std_msgs::Bool& inbool) {BOOLA = inbool.data;} void BCallback(const std_msgs::Bool& inbool) {BOOLB = inbool.data;} //_____________________________________________________________ - //************************************************************ //callback for subscribing to the boolean message //linked to the press button on the ROS Mobile app void CCallback(const std_msgs::Bool& inbool) {BOOLC = inbool.data;} //_____________________________________________________________ - //************************************************************ //callback for subscribing to the boolean message //linked to the press button on the ROS Mobile app void DCallback(const std_msgs::Bool& inbool) {BOOLD = inbool.data;} //_____________________________________________________________ +//************************************************************ +//callback for subscribing to the boolean message +//linked to the press button on the ROS Mobile app +void ECallback(const std_msgs::Bool& inbool) {BOOLE = inbool.data;} +//_____________________________________________________________ + +//************************************************************ +//callback for subscribing to the boolean message +//linked to the press button on the ROS Mobile app +void FCallback(const std_msgs::Bool& inbool) {BOOLF = inbool.data;} +//_____________________________________________________________ + +//************************************************************ +//callback for subscribing to the boolean message +//linked to the press button on the ROS Mobile app +void GCallback(const std_msgs::Bool& inbool) {BOOLG = inbool.data;} +//_____________________________________________________________ + +//************************************************************ +//callback for subscribing to the boolean message +//linked to the press button on the ROS Mobile app +void HCallback(const std_msgs::Bool& inbool) {BOOLH = inbool.data;} +//_____________________________________________________________ //************************************************************* -// function to wait for one button to be pressed +// function to wait for one button to be pressed then released // on the ROS Mobile application. -// returns one int to reflect which button has been pressed +// returns one int to reflect which button has been pressed then released //************************************************************* int getbutton(){ //cout<<"probing buttons"<< endl; @@ -156,6 +183,10 @@ int getbutton(){ keycode = 4; break; } + if (LOCATE == true){ + keycode = 5; + break; + } if (BOOLA == true){ keycode = 10; break; @@ -172,12 +203,28 @@ int getbutton(){ keycode = 13; break; } + if (BOOLE == true){ + keycode = 14; + break; + } + if (BOOLF == true){ + keycode = 15; + break; + } + if (BOOLG == true){ + keycode = 16; + break; + } + if (BOOLH == true){ + keycode = 17; + break; + } } while(ros::ok()){ ros::spinOnce(); loop_rate.sleep(); // wait for all button released - if (!SET || !GOTO || !ABORT || !BOOLA || !BOOLB || !BOOLC || !BOOLD){ + if (!SET || !GOTO || !ABORT || !LOCATE || !BOOLA || !BOOLB || !BOOLC || !BOOLD || !BOOLE || !BOOLF || !BOOLG || !BOOLH){ break; } } @@ -256,24 +303,9 @@ std_msgs::Bool disable; move_base_msgs::MoveBaseGoal goal; move_base_msgs::MoveBaseGoal tableOfGoals[TABLE_SIZE]; -//fill tableOfGoals with blank inputs : header.frame_id set to dummy -for(counter = 0; counter < TABLE_SIZE; counter++){ - //cout<<"counter :"<(&togin), sizeof(togin)); fileLoad.close(); cout << "File tog.bin has been loaded to table of double floats" << endl; + //filling table of goals with values from table of double floats + cout<<"Filling table of goals in memory ..."<("/send_goal/disable",1000); +// Publish a position to amcl /initialpose topic (geometry_msgs/PoseWithCovarianceStamped) +// Mean and covariance with which to (re-)initialize the particle filter. +ros::Publisher pub2 = nh.advertise("/initialpose",1000); + // Starting client MoveBaseClient client("move_base", true); ROS_INFO("Waiting for the action server to start"); @@ -436,6 +496,7 @@ ROS_INFO("Action server started"); ros::Rate loop_rate(10); /* MAIN LOOP ****************************************************** */ +/* **************************************************************** */ while (ros::ok()) { //ros::spinOnce(); @@ -450,9 +511,11 @@ c=getbutton(); //cout << ">> you typed :" << c << endl;//for debug purpose /* SET PRESSED ============================================= */ +/* ========================================================== */ if (c == 1){//SET pressed c=getbutton(); - if ((c == 10) || (c == 11) || (c == 12) || (c == 13)){//A or B or C or D pressed + if ((c == 10) || (c == 11) || (c == 12) || (c == 13) + || (c == 14) || (c == 15) || (c == 16) || (c == 17)){//A or B or C or D or E or F or G or H pressed //read the current pose topic ros::spinOnce(); loop_rate.sleep(); @@ -469,7 +532,7 @@ if (c == 1){//SET pressed //set header goal.target_pose.header.frame_id="map"; //write table of goals - tableOfGoals[c-10] = goal; + tableOfGoals[c-10] = goal;//first index is 0 in C++ cout<<"Goal " << c-9 <<" has been set"<