localizer.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "tf/transform_listener.h"
#include "tf/transform_broadcaster.h"
#include "std_srvs/Empty.h"

/* Dieser Knoten sucht nach einem localizer Marker um die Wahre Position des Roboters im Raum festzulegen
   Wird kein localizer gefunden wird eine standard Transformation gepublished
         Sollte der Marker verloren gehen wird die letzte Transformation für immer gepublished
*/

static tf::TransformListener* pListener = NULL;
tf::StampedTransform odom_localizer;
bool callfailed = false;
bool firstcallfailed = true;


bool localizeCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res) {
        try { 
                if(!callfailed){                                
                        pListener->lookupTransform("/odom", "/localizer_1", ros::Time(0), odom_localizer);
                        firstcallfailed = false;
                }
        } catch (tf::TransformException ex) {
                callfailed = true;                                                              
        }       
        return true;
}

int main(int argc, char **argv)
{
  /* init ROS */  
  
  ros::init(argc, argv, "biped_localizer");
        ros::NodeHandle n;

        static tf::TransformBroadcaster br;
        pListener = new(tf::TransformListener);
        ros::Rate loop_rate(30);

        ros::Time now = ros::Time::now();
        

        ros::ServiceServer localize_srv = n.advertiseService("localize_srv", localizeCallback);
        
        while(ros::ok){ 

                now = ros::Time::now(); 

                tf::Quaternion rotation_clean;
                tf::Transform localizer;
                tf::Transform localizer2;
                tf::Transform map_localizer;
                tf::Transform map_odom;

                if(!firstcallfailed){                   
                        localizer2.setOrigin(btVector3(0, 0, 0));  
                        localizer2.setBasis(btMatrix3x3(0,0,1,0,-1,0,1,0,0));
                        localizer.mult(odom_localizer, localizer2);
                        rotation_clean.setRotation(btVector3(0,0,1), localizer.getRotation().getAngle());
                        localizer.setRotation(rotation_clean);          
                        map_localizer.setOrigin(btVector3(0, 2, 0.18));  
                        map_localizer.setBasis(btMatrix3x3(1,0,0,0,1,0,0,0,1));                 
                        map_odom.mult(map_localizer, localizer.inverse());
                        br.sendTransform(tf::StampedTransform(map_odom, now, "/map", "/odom"));                         
                } else {
                        map_odom.setOrigin(btVector3(2, 4, 0)); 
                        map_odom.setBasis(btMatrix3x3(1,0,0,0,1,0,0,0,1));
                        br.sendTransform(tf::StampedTransform(map_odom, now, "/map", "/odom"));
                }
                loop_rate.sleep();      
                ros::spinOnce();
        }
  return 0; 
}