Advertisement
ljl16

subscriber

Nov 23rd, 2011
431
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 1.62 KB | None | 0 0
  1. #include <ros/ros.h>
  2. #include <tf/transform_listener.h>
  3. #include <iostream>
  4. #include <nav_msgs/Odometry.h>
  5.  
  6. using namespace std;
  7. /*#include <turtlesim/Velocity.h>
  8. #include <turtlesim/Spawn.h>*/
  9.  
  10. // %Tag(CALLBACK)%
  11.  
  12. void chatterCallback(const nav_msgs::Odometry::ConstPtr& msg)
  13. {
  14. ROS_INFO("I heard: [%f]", msg->pose.pose.position.x);
  15. }
  16.  
  17.  
  18. int main(int argc, char** argv){
  19. ros::init(argc, argv, "my_tf_listener");
  20.  
  21. ros::NodeHandle node;
  22.  
  23. // ros::service::waitForService("odom");
  24. /* ros::ServiceClient add_turtle =
  25. node.serviceClient<turtlesim::Spawn>("spawn");
  26. turtlesim::Spawn srv;
  27. add_turtle.call(srv);
  28.  
  29. ros::Publisher turtle_vel =
  30. node.advertise<turtlesim::Velocity>("turtle2/command_velocity", 10);
  31. */
  32. tf::TransformListener listener;
  33. ros::Subscriber sub = node.subscribe("odom", 1000, chatterCallback);
  34. ros::spin();
  35. ros::Rate rate(1.0);
  36. while (node.ok()){
  37. tf::StampedTransform transform;
  38. try{
  39. listener.lookupTransform("tf", "odom",
  40. ros::Time::now(), transform);
  41. }
  42. catch (tf::TransformException ex){
  43. ROS_ERROR("%s",ex.what());
  44. }
  45. cout << transform.getOrigin().y()<<endl;
  46. cout << transform.getOrigin().x()<<endl;
  47. // turtlesim::Velocity vel_msg;
  48. /* vel_msg.angular = 4 * atan2(transform.getOrigin().y(),
  49. transform.getOrigin().x());
  50. vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
  51. pow(transform.getOrigin().y(), 2));
  52. turtle_vel.publish(vel_msg);*/
  53.  
  54. rate.sleep();
  55. }
  56. return 0;
  57. };
  58.  
  59.  
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement