pr2demo.cpp

Go to the documentation of this file.

#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <stdio.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

using namespace std;
using namespace ros;
using namespace nav_msgs;

bool atGoal = false;
geometry_msgs::TransformStamped geo_pose;

void sendGoal(double x, double y, double theta, Publisher pub){
  //tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(tf::createQuaternionFromYaw(theta), tf::Point(x, y, 0.0)), ros::Time::now(), panel_->getGlobalFrame());
  geometry_msgs::PoseStamped goal;
  //tf::poseStampedTFToMsg(p, goal);
  //ROS_DEBUG("setting goal: Position(%.3f, %.3f, %.3f), Orientation(%.3f, %.3f, %.3f, %.3f) = Angle: %.3f\n", goal.pose.position.x, goal.pose.position.y, goal.pose.position. z, goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w, angle);

  goal.pose.position.x = x;
  goal.pose.position.y = y;
  goal.pose.position.z = 0;
  btQuaternion temp;
  temp.setEulerZYX(theta,0,0);
  goal.pose.orientation.x = temp.getX();
  goal.pose.orientation.y = temp.getY();
  goal.pose.orientation.z = temp.getZ();
  goal.pose.orientation.w = temp.getW();
  goal.header.stamp = ros::Time::now();
  goal.header.frame_id = "/map";//panel_->getGlobalFrame();
  pub.publish(goal);
}

void getPose(tf::TransformListener* listener, double* x, double* y, double* th){
  tf::StampedTransform transform;
  try{
    listener->lookupTransform("/map", "/base_footprint", ros::Time(0), transform);
  }
  catch (tf::TransformException ex){
  ROS_ERROR("%s",ex.what());
  }

  transformStampedTFToMsg(transform, geo_pose);

  *x = geo_pose.transform.translation.x;
  *y = geo_pose.transform.translation.y;
  *th = tf::getYaw(geo_pose.transform.rotation);
}

void sendPose(double x, double y, double theta, Publisher pub){
  //tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(tf::createQuaternionFromYaw(theta), tf::Point(x, y, 0.0)), ros::Time::now(), panel_->getGlobalFrame());
  geometry_msgs::PoseWithCovarianceStamped goal;
  //tf::poseStampedTFToMsg(p, goal);
  //ROS_DEBUG("setting goal: Position(%.3f, %.3f, %.3f), Orientation(%.3f, %.3f, %.3f, %.3f) = Angle: %.3f\n", goal.pose.position.x, goal.pose.position.y, goal.pose.position. z, goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w, angle);

  goal.pose.pose.position.x = x;
  goal.pose.pose.position.y = y;
  goal.pose.pose.position.z = 0;
  btQuaternion temp;
  temp.setEulerZYX(theta,0,0);
  goal.pose.pose.orientation.x = temp.getX();
  goal.pose.pose.orientation.y = temp.getY();
  goal.pose.pose.orientation.z = temp.getZ();
  goal.pose.pose.orientation.w = temp.getW();
  goal.header.stamp = ros::Time::now();
  goal.header.frame_id = "/map";//panel_->getGlobalFrame();
  pub.publish(goal);
}

int main(int argc, char** argv){
  ros::init(argc, argv, "demo_sbpl_dynamic_env");


  Publisher robotGoal = ros::NodeHandle().advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1);
  Publisher robotStart = ros::NodeHandle().advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1);
  tf::TransformListener listener;

  sleep(10.0);

  //initial pose 13.5 3.7

  //sendGoal(6.3, 4.5, M_PI, robotGoal);
  //sendGoal(3.67, 0.69, M_PI, robotGoal);
  //sendGoal(2.0, 5.0, M_PI, robotGoal);


  //double goals[2][3] = {{2.648, 5.015, 0}, {12.927, 5.197, -3.129}};
  double goals[2][3] = {{1.404, 7.161, -0.012}, {15.948, 7.304, 3.141}};

  sendPose(goals[0][0], goals[0][1], goals[0][2], robotStart);
  

  ros::NodeHandle n;
  double t = ros::Time::now().toSec();
  double x,y,th;
  while(n.ok()){
    t = ros::Time::now().toSec();
    atGoal = false;
    sendGoal(goals[0][0], goals[0][1], goals[0][2], robotGoal);
    while(n.ok() && ros::Time::now().toSec()-t < 180.0 && !atGoal){
      getPose(&listener,&x,&y,&th);
      double dx = goals[0][0]-x;
      double dy = goals[0][1]-y;
      double dist = sqrt(dx*dx+dy*dy);
      atGoal = dist < 1.0;
      ros::Duration(2.0).sleep();
    }

    t = ros::Time::now().toSec();
    atGoal = false;
    sendGoal(goals[1][0], goals[1][1], goals[1][2], robotGoal);
    while(n.ok() && ros::Time::now().toSec()-t < 180.0 && !atGoal){
      getPose(&listener,&x,&y,&th);
      double dx = goals[1][0]-x;
      double dy = goals[1][1]-y;
      double dist = sqrt(dx*dx+dy*dy);
      atGoal = dist < 1.0;
      ros::Duration(2.0).sleep();
    }
  }

  ros::spin();
}

---

demo_sbpl_dynamic_env
Author(s): Michael Phillips
autogenerated on Fri Jan 11 09:37:33 2013