00001 #include "ros/ros.h"
00002 #include "geometry_msgs/PoseWithCovarianceStamped.h"
00003 #include "tf/transform_listener.h"
00004 #include "tf/transform_broadcaster.h"
00005 #include "std_srvs/Empty.h"
00006
00007
00008
00009
00010
00011
00012 static tf::TransformListener* pListener = NULL;
00013 tf::StampedTransform odom_localizer;
00014 bool callfailed = false;
00015 bool firstcallfailed = true;
00016
00017
00018 bool localizeCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res) {
00019 try {
00020 if(!callfailed){
00021 pListener->lookupTransform("/odom", "/localizer_1", ros::Time(0), odom_localizer);
00022 firstcallfailed = false;
00023 }
00024 } catch (tf::TransformException ex) {
00025 callfailed = true;
00026 }
00027 return true;
00028 }
00029
00030 int main(int argc, char **argv)
00031 {
00032
00033
00034 ros::init(argc, argv, "biped_localizer");
00035 ros::NodeHandle n;
00036
00037 static tf::TransformBroadcaster br;
00038 pListener = new(tf::TransformListener);
00039 ros::Rate loop_rate(30);
00040
00041 ros::Time now = ros::Time::now();
00042
00043
00044 ros::ServiceServer localize_srv = n.advertiseService("localize_srv", localizeCallback);
00045
00046 while(ros::ok){
00047
00048 now = ros::Time::now();
00049
00050 tf::Quaternion rotation_clean;
00051 tf::Transform localizer;
00052 tf::Transform localizer2;
00053 tf::Transform map_localizer;
00054 tf::Transform map_odom;
00055
00056 if(!firstcallfailed){
00057 localizer2.setOrigin(btVector3(0, 0, 0));
00058 localizer2.setBasis(btMatrix3x3(0,0,1,0,-1,0,1,0,0));
00059 localizer.mult(odom_localizer, localizer2);
00060 rotation_clean.setRotation(btVector3(0,0,1), localizer.getRotation().getAngle());
00061 localizer.setRotation(rotation_clean);
00062 map_localizer.setOrigin(btVector3(0, 2, 0.18));
00063 map_localizer.setBasis(btMatrix3x3(1,0,0,0,1,0,0,0,1));
00064 map_odom.mult(map_localizer, localizer.inverse());
00065 br.sendTransform(tf::StampedTransform(map_odom, now, "/map", "/odom"));
00066 } else {
00067 map_odom.setOrigin(btVector3(2, 4, 0));
00068 map_odom.setBasis(btMatrix3x3(1,0,0,0,1,0,0,0,1));
00069 br.sendTransform(tf::StampedTransform(map_odom, now, "/map", "/odom"));
00070 }
00071 loop_rate.sleep();
00072 ros::spinOnce();
00073 }
00074 return 0;
00075 }
00076