duck_and_wait.cpp

Go to the documentation of this file.

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

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

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);
}

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

  Publisher robotGoal = ros::NodeHandle().advertise<geometry_msgs::PoseStamped>("/robot_0/move_base_simple/goal", 1);
  Publisher dynObsGoal1 = ros::NodeHandle().advertise<geometry_msgs::PoseStamped>("/robot_1/move_base_simple/goal", 1);
  //Publisher dynObsGoal2 = ros::NodeHandle().advertise<geometry_msgs::PoseStamped>("/robot_2/move_base_simple/goal", 1);
  //dynObs1_sub = n.subscribe("robot_1/base_pose_ground_truth", 1, &FakeTracking::dynamicObstacleCallback1,this);
  //dynObs2_sub = n.subscribe("robot_2/base_pose_ground_truth", 1, &FakeTracking::dynamicObstacleCallback2,this);

  sleep(10);
  ROS_DEBUG("fire one!\n");
  sendGoal(8.913, 6.913, 3.14159, dynObsGoal1);
  sleep(2);
  //sleep(12);
  ROS_DEBUG("fire two!\n");
  sendGoal(20.413, 6.913, 0.0, robotGoal);

  ros::spin();
}

---

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