Go to the documentation of this file.00001 #include "ros/ros.h"
00002 #include "geometry_msgs/PoseWithCovarianceStamped.h"
00003 #include "tf/transform_listener.h"
00004 #include "std_srvs/Empty.h"
00005
00006
00007
00008 static tf::TransformListener* pListener = NULL;
00009 ros::Publisher pub;
00010
00011 bool goalposeCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res) {
00012 tf::StampedTransform transform;
00013 try {
00014 pListener->lookupTransform("/map", "/nav_goal_desired", ros::Time(0), transform);
00015 } catch (tf::TransformException ex) {
00016 ROS_INFO("GoalPose not found, set goal manually");
00017 return false;
00018 }
00019
00020 tf::Transform rotate;
00021 rotate.setOrigin(btVector3(0, 0, 0));
00022 rotate.setBasis(btMatrix3x3(0,-1,0,1,0,0,0,0,1));
00023 transform.mult(transform, rotate);
00024 geometry_msgs::PoseStamped pose;
00025 pose.pose.position.x = transform.getOrigin().x();
00026 pose.pose.position.y = transform.getOrigin().y();
00027 pose.pose.position.z = 0;
00028 pose.pose.orientation.x = transform.getRotation().x();
00029 pose.pose.orientation.y = transform.getRotation().y();
00030 pose.pose.orientation.z = transform.getRotation().z();
00031 pose.pose.orientation.w = transform.getRotation().w();
00032 pose.header.stamp = ros::Time::now();
00033 pose.header.frame_id = "/map";
00034 pub.publish(pose);
00035 return true;
00036 }
00037
00038 int main(int argc, char **argv)
00039 {
00040
00041
00042 ros::init(argc, argv, "biped_goalpose");
00043 ros::NodeHandle n;
00044
00045 pub = n.advertise<geometry_msgs::PoseStamped>("goal2", 1000);
00046
00047 pListener = new(tf::TransformListener);
00048
00049 ros::ServiceServer localize_srv = n.advertiseService("goalpose_srv", goalposeCallback);
00050
00051 ros::spin();
00052
00053
00054 return 0;
00055 }
00056