navigation_master.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "visualization_msgs/MarkerArray.h"
#include "visualization_msgs/Marker.h"
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include "bipedRobin_msgs/InitialStepsService.h"
#include "bipedRobin_msgs/SetModeService.h"
#include "bipedRobin_msgs/StepTarget3DService.h"
#include "std_srvs/Empty.h"
#include "std_msgs/UInt8.h"
#include <string.h>
 /*
                Dieser Knoten übernimmt die Koordinierung zwischen Roboter und FootstepPlanner
                Services die Angeboten werden:
                Statische Planung
                Dynamsiche Planung
                Replan: Neuplanung der Schritte
                CreateMap: Neu Karte erstellen
                Localize: Lokalisieren des Roboters im Raum
                Walk: Durchführen der geplanten Schritte
 */
#define DEFAULT_VARIABLE_NUMBER 150
#define DEFAULT_WORDLENGTH 20   /* multible of 4 and compatible with simulink receiving file */

//Funktionen
void staticInitialSteps();
void setInitialSteps(tf::StampedTransform left, tf::StampedTransform right);
void setInitialStep();
void staticPlanning();
void dynamicPlanning();
void localize();
void createMap();
void goalpose();
void replan();
void restart();
void pub_goal();
void sendMarker(visualization_msgs::Marker step, tf::StampedTransform map);
void setdynamicInitialSteps(visualization_msgs::Marker left, visualization_msgs::Marker right, tf::StampedTransform map);

//Clients Subscriber Publisher
ros::ServiceClient initialStep_client;
ros::ServiceClient initialSteps_client;
ros::ServiceClient localize_client;
ros::ServiceClient createMap_client;
ros::ServiceClient footstep_client;
ros::ServiceClient goalpose_client;
ros::ServiceClient restart_client;
ros::Subscriber markerArray;
ros::Subscriber goalpose_sub;
ros::Publisher goal_pub;

//Flags
bool isReal;
bool sendStep_flag = false;
bool first_call_flag = true;
bool newMarkerArray_flag = false;
bool walk_flag = false;
bool plan_flag = false;
bool staticPlanner_flag = false;
bool finished = false;
bool random_flag = true;

int dynamic_left_right = 1;
int stepsLeftInBuffer = 0;
int dynamicBufferSize = 0;
int stepBufferSize = 0;

visualization_msgs::MarkerArray currentFootsteps;
geometry_msgs::PoseStamped currentGoalpose;
tf::StampedTransform old_left;
tf::StampedTransform old_right;

static tf::TransformListener* pListener = NULL;
static tf::TransformBroadcaster* pBroadcaster = NULL;

void localize(){ //Ruft die Lokalisierung auf
        ROS_INFO("localize Service called");
        std_srvs::Empty srv;
        localize_client.call(srv);
}

void createMap(){ //Erstellt eine neue Karte
        ROS_INFO("createMap Service called");
        std_srvs::Empty srv;    
        createMap_client.call(srv);             
}

void goalpose(){ //Published die Koordinaten des Zielmarkers falls vorhanden
        ROS_INFO("Automatic Goalpose Service called");
        std_srvs::Empty srv;
        goalpose_client.call(srv);
}

void restart(){ //Startet den FootstepPlanner neue
        ROS_INFO("Planner restart Service called");
        std_srvs::Empty srv;
        restart_client.call(srv);
}

void replan(){
        ROS_INFO("replan Service called");
        newMarkerArray_flag = false;    //neue Schritte zu lassen
        setInitialSteps(old_left, old_right); //alte Startschritte setzen       
        ros::spinOnce();        
        goalpose();  //einlesen der Zielmarker Position
        ros::spinOnce();                        
        createMap(); //Neu erstellen der Karte
        ros::spinOnce();
        pub_goal(); //Ziel erneut an den FootstepPlanner schicken um neuPlanung zu starten.
}

void pub_goal(){ //Published das Ziel um eine neuplanung zu erwirken
        ROS_INFO("Goal republished");
        geometry_msgs::PoseStamped goalpose = currentGoalpose;  
        if(random_flag){ //Selbes Ziel führt zu keiner Neuplanung deswegen zufalls zahlen hinzu addieren
                goalpose.pose.position.x += (rand()%5) * 0.01;
                goalpose.pose.position.y += (rand()%5) * 0.01;
        }
        goalpose.header.stamp = ros::Time::now();
        goal_pub.publish(goalpose);     
}

//Sender einen als Schritt Koordinaten an den Roboter
void sendMarker(visualization_msgs::Marker step, tf::StampedTransform map){     
        tf::StampedTransform marker;
        tf::Vector3 marker_Vector;
        tf::Quaternion marker_Rotation; 
        marker_Vector.setX(step.pose.position.x);
        marker_Vector.setY(step.pose.position.y);
        marker_Vector.setZ(step.pose.position.z);
        marker_Rotation.setX(step.pose.orientation.x);
        marker_Rotation.setY(step.pose.orientation.y);
        marker_Rotation.setZ(step.pose.orientation.z);
        marker_Rotation.setW(step.pose.orientation.w);
        marker.setOrigin(marker_Vector);
        marker.setRotation(marker_Rotation);            
        marker.mult(map, marker);
        bipedRobin_msgs::StepTarget3DService srv;
        //unterscheiden zwischen linken und rechten fuß
        if(step.color.r > 0) { //links
                srv.request.step.leg = 1;
        } else { //rechts
                srv.request.step.leg = 0;
        }
        srv.request.step.pose.position.x = marker.getOrigin().x();
        srv.request.step.pose.position.y = marker.getOrigin().y();
        srv.request.step.pose.position.z = 0;
        srv.request.step.pose.orientation.x = marker.getRotation().x();
        srv.request.step.pose.orientation.y = marker.getRotation().y();
        srv.request.step.pose.orientation.z = marker.getRotation().z();
        srv.request.step.pose.orientation.w = marker.getRotation().w();                                                                 
        footstep_client.call(srv);
}

//Funktion für die eigentliche Schrittplanung
void dynamicPlanning(){
        restart();      //neustarten des Planners
        ros::Duration(3).sleep();
        ros::spinOnce();
        if(first_call_flag) {   //nur beim aller ersten Aufruf durchführen
                setInitialStep(); 
                localize();     
                ros::Duration(2).sleep();                               
        } else {
                sendStep_flag = true;
        }               
        createMap();
        staticInitialSteps();   //Wirkliche Fuß Posiotenen als StartSchritte setzen.
        int currentStep = 0;  //Aktueller Schritt der gesendet werden soll
        tf::StampedTransform map;  //Transformation zu map Koordinaten
        visualization_msgs::Marker temp[stepBufferSize];
        visualization_msgs::Marker Buffer[stepBufferSize];  //dynamischer Buffer
        visualization_msgs::Marker left;
        visualization_msgs::Marker right;
        int stepsInDynamicBuffer = 0; //Schritte die noch im dynamischen Buffer liegen
        finished = false;  //Variable ob Zielposition erreicht 
        bool replan_flag = true;
        first_call_flag = true;
        walk_flag = false;
        ros::Rate loop_rate(100);
        try {                           
                pListener->lookupTransform("/odom", "/map", ros::Time(0), map);
        } catch (tf::TransformException ex) {                                                   
        }
        while(ros::ok){
                if(replan_flag){                                
                        ROS_INFO("Planning started");                   
                        goalpose();                     
                        createMap();
                        ros::spinOnce();
                        pub_goal();
                        newMarkerArray_flag = false;
                        ROS_INFO("Waiting for walk approval");
                        if(walk_flag){
                                ROS_INFO("Waiting for new Footsteps from Planner");             
                                while(!newMarkerArray_flag) ros::spinOnce(); loop_rate.sleep(); 
                                newMarkerArray_flag = false;
                                int count = 0;
                                while(!newMarkerArray_flag && (count < 30)){
                                        ros::spinOnce(); 
                                        loop_rate.sleep();      
                                        count++; 
                                }
                ROS_INFO("Footsteps received. executing");      
                        }
                        while(!walk_flag) {                     
                                ros::spinOnce(); 
                                loop_rate.sleep();      
                        }                               
                        first_call_flag = false;                
                        replan_flag = false;                    
                        
                        visualization_msgs::MarkerArray footsteps = currentFootsteps;
                        int startStep = 2;

                        if(fabs(footsteps.markers[2].pose.position.x - footsteps.markers[0].pose.position.x) < 0.05 && fabs(footsteps.markers[2].pose.position.y - footsteps.markers[0].pose.position.y) < 0.05){
                                if(footsteps.markers.size() > 3){                               
                                        startStep = 3;  
                                }               
                        }       
                        if(((footsteps.markers[startStep].color.r > 0) && (dynamic_left_right == 1)) || ((footsteps.markers[startStep].color.g > 0) && (dynamic_left_right == 0))){
                                startStep--;
                        }                               
                        int i = startStep;
                        int k = 0;                      
                        while(i < startStep+stepBufferSize && i < footsteps.markers.size()){
                                temp[k] = footsteps.markers[i];                         
                                i++; k++; stepsInDynamicBuffer++;               
                        }
                        if(i == footsteps.markers.size()){
                                finished = true;        
                                if(temp[k-1].color.r > 0) {
                                        left = temp[k-1];
                                        dynamic_left_right = 1; 
                                } else {
                                        right = temp[k-1];
                                        dynamic_left_right = 0;
                                }
                        }       else {
                                finished = false;                       
                                if(temp[k-1].color.r > 0) {
                                        left = temp[k-1];
                                        right = temp[k-2];
                                        dynamic_left_right = 1;
                                } else {
                                        right = temp[k-1];
                                        left = temp[k-2];
                                        dynamic_left_right = 0;
                                }
                        }
                                                        
                }                       
                
                if(currentStep == 0){
                        for(int p = 0; p < stepBufferSize; p++){
                                Buffer[p] = temp[p];                            
                        }                       
                        setdynamicInitialSteps(left, right, map);               
                }       
                
                if(currentStep == stepBufferSize) {
                        currentStep = 0;
                        replan_flag = true;
                }       

                if(finished && stepsInDynamicBuffer == 0){
                        ROS_INFO("Reached Target destination");
                        walk_flag = false;
                        break;
                }
                
                if(sendStep_flag && !replan_flag){      
                        sendStep_flag = false;
                        sendMarker(Buffer[currentStep], map);
                        currentStep++;
                        stepsInDynamicBuffer--;
                }
                loop_rate.sleep();      
                ros::spinOnce();                                
        }
        ros::spinOnce();
}

//Funktion zum setzen der Startschritte des FootstepPlanners auf bestimmte Positionen
void setdynamicInitialSteps(visualization_msgs::Marker leftMarker, visualization_msgs::Marker rightMarker,      tf::StampedTransform map){
        ROS_INFO("setdynamicInitialSteps Service called");
        tf::StampedTransform left;
        tf::StampedTransform right;
        tf::Vector3 marker_Vector;
        tf::Quaternion marker_Rotation; 
        marker_Vector.setX(leftMarker.pose.position.x);
        marker_Vector.setY(leftMarker.pose.position.y);
        marker_Vector.setZ(leftMarker.pose.position.z);
        marker_Rotation.setX(leftMarker.pose.orientation.x);
        marker_Rotation.setY(leftMarker.pose.orientation.y);
        marker_Rotation.setZ(leftMarker.pose.orientation.z);
        marker_Rotation.setW(leftMarker.pose.orientation.w);
        left.setOrigin(marker_Vector);
        left.setRotation(marker_Rotation);
        bipedRobin_msgs::InitialStepsService srv;

        marker_Vector.setX(rightMarker.pose.position.x);
        marker_Vector.setY(rightMarker.pose.position.y);
        marker_Vector.setZ(rightMarker.pose.position.z);
        marker_Rotation.setX(rightMarker.pose.orientation.x);
        marker_Rotation.setY(rightMarker.pose.orientation.y);
        marker_Rotation.setZ(rightMarker.pose.orientation.z);
        marker_Rotation.setW(rightMarker.pose.orientation.w);
        right.setOrigin(marker_Vector);
        right.setRotation(marker_Rotation);     

        setInitialSteps(left, right);   
}

//Funktion zum Setzen der Startschritte des FootstepPlanners auf die aktuellen Positionen
void staticInitialSteps(){
        ROS_INFO("set staticInitialSteps Service called");
        tf::StampedTransform left;
        tf::StampedTransform right;

        try {
                pListener->lookupTransform("/map", "l_sole", ros::Time(0), left);
        } catch (tf::TransformException ex) {
        }

        try {
                pListener->lookupTransform("/map", "r_sole", ros::Time(0), right);
        } catch (tf::TransformException ex) {
        }
        setInitialSteps(left, right);   
}


//Funktion zum Senden der Startschritte des FootstepPlanners and den Planner
void setInitialSteps(tf::StampedTransform left, tf::StampedTransform right){
        
        old_left = left;  
        old_right = right;

        bipedRobin_msgs::InitialStepsService srv;
        
        srv.request.left.leg=1; 
        srv.request.left.pose.position.x = left.getOrigin().x();
        srv.request.left.pose.position.y = left.getOrigin().y();
        srv.request.left.pose.position.z = left.getOrigin().z();
        srv.request.left.pose.orientation.x = left.getRotation().x();
        srv.request.left.pose.orientation.y = left.getRotation().y();
        srv.request.left.pose.orientation.z = left.getRotation().z();
        srv.request.left.pose.orientation.w = left.getRotation().w();
        
        srv.request.right.leg=0;
        srv.request.right.pose.position.x = right.getOrigin().x();
        srv.request.right.pose.position.y = right.getOrigin().y();
        srv.request.right.pose.position.z = right.getOrigin().z();
        srv.request.right.pose.orientation.x = right.getRotation().x();
        srv.request.right.pose.orientation.y = right.getRotation().y();
        srv.request.right.pose.orientation.z = right.getRotation().z();
        srv.request.right.pose.orientation.w = right.getRotation().w();
        
        initialSteps_client.call(srv); 
}

//Funktion zum Setzen des InitialStep
void setInitialStep(){
        ROS_INFO("setInitialStep Service called");      
        tf::StampedTransform step;
        try {
                pListener->lookupTransform("/odom", "/l_sole", ros::Time(0), step);
        } catch (tf::TransformException ex) {
        }       
        bipedRobin_msgs::StepTarget3DService srv;       
        srv.request.step.leg=1; 
        srv.request.step.pose.position.x = step.getOrigin().x();
        srv.request.step.pose.position.y = step.getOrigin().y();
        srv.request.step.pose.position.z = step.getOrigin().z();
        srv.request.step.pose.orientation.x = step.getRotation().x();
        srv.request.step.pose.orientation.y = step.getRotation().y();
        srv.request.step.pose.orientation.z = step.getRotation().z();
        srv.request.step.pose.orientation.w = step.getRotation().w();
        initialStep_client.call(srv);
}

//Callback um statischen Planner zu starten
bool staticPlanningCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res) {
        if(!plan_flag){ //Verhindern das Planner gestartet wird wenn ein andere läuft  
                ROS_INFO("staticPlanning Service started");
                stepBufferSize = 100; //Anzahl der Schritt bevor eine Neuplanung statt findet -> statisch
                staticPlanner_flag = true;      
                plan_flag = true; //flag um planner aufzurufen
        }
        return true;
}

//Callback um dynamischen Planner zu starten
bool dynamicPlanningCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res) {
        if(!plan_flag){         
                ROS_INFO("dynamicPlanning Service started");
                stepBufferSize = dynamicBufferSize; //Anzahl der Schritte bevor Neuplanung statt findet default 3               
                staticPlanner_flag = false;
                plan_flag = true; //flag um planner aufzurufen
        }
        return true;
}

//Callback um freigabe zum Gehen zu erteilen
bool walkCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res){
        walk_flag = true;
        return true;
}

//Calback für neue Schritt aus FootstepPlanner
void newStepsArrayCallback(const visualization_msgs::MarkerArray markerArray) {
        bool ignore = false;
        //Verhindern das leere Schritte akzeptiert werden:
        for(int i=0; i < markerArray.markers.size(); i++){
                if(markerArray.markers[i].color.r == 0 && markerArray.markers[i].color.g == 0) ignore = true;   
        }
        //Nur wenn mindestens 3 Schritte und neue Schritte gefordert sind bzw. der staticPlanner läuft die Schritte abspeichern
        if(markerArray.markers.size() > 2 && !ignore && (!newMarkerArray_flag || staticPlanner_flag)){
                ROS_INFO("New Footsteps received");
                //Wenn noch mehr als 6 Schritte zu gehen sind (2 sind Startposition) dann das Ziel random Posten.               
                if(markerArray.markers.size() < 7){
                        random_flag = false;
                } else {
                        random_flag = true;
                }
                currentFootsteps = markerArray;
                newMarkerArray_flag = true;
        }       
}

//Callback für Aktuellen Schrittpuffer am Roboter
void stepsLeftCallback(std_msgs::UInt8 stepsLeft) {
        stepsLeftInBuffer = stepsLeft.data;     //Schritte für dynamischen Planner setzen
        if(stepsLeft.data == 0){ //keine Schritte mehr im Buffer
                ros::Duration(0.8).sleep(); //warten bevor neuer Schritt gesendet wird
                sendStep_flag = true; //neuen Schritt senden
        }
}

//Callback für aktuelles Ziel
void goalCallback(geometry_msgs::PoseStamped pose){
        newMarkerArray_flag = false;
        currentGoalpose = pose; //speichert das Ziel ab um es bei Bedarf neu im Planner zu setzen
        //replan();
}

//Callback um neuplanen der Schritte zu erzwingen
bool replanCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res)       {
        replan();
        return true;
}

int main(int argc, char **argv)
{
  /* init ROS */  
  
        ros::init(argc, argv, "navigation_master");
        pListener = new(tf::TransformListener);
        pBroadcaster = new(tf::TransformBroadcaster);   

        ros::NodeHandle n;
        
        n.param("/par_isRealEnvironment", isReal, false); //real oder simulated
        n.param("/par_stepBufferSize", dynamicBufferSize, 5); //Anzahl der Schritt bevor Neuplanung stattfindet

        //Service Server
        ros::ServiceServer staticPlanning_srv = n.advertiseService("staticPlanning_srv", staticPlanningCallback);
        ros::ServiceServer dynamicPlanning_srv = n.advertiseService("dynamicPlanning_srv", dynamicPlanningCallback);
        ros::ServiceServer walk_srv = n.advertiseService("walk_srv", walkCallback);
        ros::ServiceServer replan_srv = n.advertiseService("replan_srv", replanCallback);
        
        //subscriber
        markerArray = n.subscribe<visualization_msgs::MarkerArray>("bipedRobin_footstep_planner/footsteps_array", 1, newStepsArrayCallback);
        ros::Subscriber stepsLeftInBuffer_sub = n.subscribe<std_msgs::UInt8>("stepsLeftInBuffer", 1, stepsLeftCallback);
        goalpose_sub = n.subscribe<geometry_msgs::PoseStamped>("goal2", 0, goalCallback);

        //Service clients
        initialStep_client = n.serviceClient<bipedRobin_msgs::StepTarget3DService>("initialstep_srv");  
        initialSteps_client = n.serviceClient<bipedRobin_msgs::InitialStepsService>("initialSteps_srv");                
        localize_client = n.serviceClient<std_srvs::Empty>("localize_srv");                     
        restart_client = n.serviceClient<std_srvs::Empty>("restart_srv");
        createMap_client = n.serviceClient<std_srvs::Empty>("createMap_srv");
        footstep_client = n.serviceClient<bipedRobin_msgs::StepTarget3DService>("footstep3D_srv");      
        goalpose_client = n.serviceClient<std_srvs::Empty>("goalpose_srv");
        
        //publisher
        goal_pub = n.advertise<geometry_msgs::PoseStamped>("goal", 1000);
        

        //Schleife um Planung zu starten, notwendig damit Service return sendet ohne auf Fertigstellung des Planners zu warten.
        while(ros::ok()){
                if(plan_flag){
                        dynamicPlanning();
                }
                plan_flag = false;
                ros::spinOnce();
        }

        return 0; 
}