goalpose.cpp

Go to the documentation of this file.

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

//Dieser Knoten sucht nach einem bestimmten ZeilMarker und published dessen Position als goal_pose für den FootstepPlanner

static tf::TransformListener* pListener = NULL;
ros::Publisher pub;

bool goalposeCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res) {
        tf::StampedTransform transform;
        try {
                pListener->lookupTransform("/map", "/nav_goal_desired", ros::Time(0), transform);
        } catch (tf::TransformException ex) {
                ROS_INFO("GoalPose not found, set goal manually");
                return false;
        }

        tf::Transform rotate;
        rotate.setOrigin(btVector3(0, 0, 0));
        rotate.setBasis(btMatrix3x3(0,-1,0,1,0,0,0,0,1));
        transform.mult(transform, rotate);
        geometry_msgs::PoseStamped pose;
        pose.pose.position.x = transform.getOrigin().x();
        pose.pose.position.y = transform.getOrigin().y();
        pose.pose.position.z = 0;
        pose.pose.orientation.x = transform.getRotation().x();
        pose.pose.orientation.y = transform.getRotation().y();
        pose.pose.orientation.z = transform.getRotation().z();
        pose.pose.orientation.w = transform.getRotation().w();
        pose.header.stamp = ros::Time::now();
        pose.header.frame_id = "/map";
        pub.publish(pose);
        return true;
}

int main(int argc, char **argv)
{
  /* init ROS */

        ros::init(argc, argv, "biped_goalpose");
        ros::NodeHandle n;

        pub = n.advertise<geometry_msgs::PoseStamped>("goal2", 1000);

        pListener = new(tf::TransformListener);

        ros::ServiceServer localize_srv = n.advertiseService("goalpose_srv", goalposeCallback);

        ros::spin();


  return 0;
}