Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #ifndef TF_KEYBOARD_CAL_DEMO_TF_LISTENER_H
00040 #define TF_KEYBOARD_CAL_DEMO_TF_LISTENER_H
00041
00042
00043 #include <ros/ros.h>
00044 #include <tf/transform_listener.h>
00045
00046
00047 #include <tf_conversions/tf_eigen.h>
00048 #include <eigen_conversions/eigen_msg.h>
00049
00050
00051 #include <rviz_visual_tools/rviz_visual_tools.h>
00052
00053 namespace tf_keyboard_cal
00054 {
00055 class DemoTFListener
00056 {
00057 public:
00061 DemoTFListener() : name_("demo_tf_listener"), nh_("~")
00062 {
00063 visual_tools_.reset(new rviz_visual_tools::RvizVisualTools("world", "/demo_tf_listener/markers"));
00064 visual_tools_->deleteAllMarkers();
00065
00066 ROS_INFO_STREAM_NAMED(name_, "DemoTFListener Ready.");
00067 }
00068
00069 void runLoop()
00070 {
00071
00072 ros::Duration(1.0).sleep();
00073
00074 const bool verbose = false;
00075
00076 ros::Rate rate(10.0);
00077 while (ros::ok())
00078 {
00079 Eigen::Affine3d pose;
00080
00081 getPose("/world", "/thing", pose);
00082
00083
00084 if (verbose)
00085 std::cout << "Position: \n" << pose.translation() << std::endl;
00086
00087
00088 visual_tools_->publishXArrow(pose);
00089
00090 rate.sleep();
00091 }
00092 }
00093
00094 bool getPose(const std::string& from_frame, const std::string& to_frame, Eigen::Affine3d &pose)
00095 {
00096 tf::StampedTransform tf_transform;
00097 try
00098 {
00099 tf_.lookupTransform(from_frame, to_frame, ros::Time(0), tf_transform);
00100 }
00101 catch (tf::TransformException ex)
00102 {
00103 ROS_ERROR("%s", ex.what());
00104 return false;
00105 }
00106
00107
00108 tf::transformTFToEigen(tf_transform, pose);
00109 return true;
00110 }
00111
00112 private:
00113
00114
00115 std::string name_;
00116
00117
00118 ros::NodeHandle nh_;
00119
00120
00121 rviz_visual_tools::RvizVisualToolsPtr visual_tools_;
00122
00123 tf::TransformListener tf_;
00124
00125 };
00126
00127
00128 typedef boost::shared_ptr<DemoTFListener> DemoTFListenerPtr;
00129 typedef boost::shared_ptr<const DemoTFListener> DemoTFListenerConstPtr;
00130
00131 }
00132 #endif // TF_KEYBOARD_CAL_DEMO_TF_LISTENER_H
00133
00134 int main(int argc, char** argv)
00135 {
00136
00137 ros::init(argc, argv, "demo_tf_listener");
00138 ROS_INFO_STREAM_NAMED("main", "Starting DemoTFListener...");
00139
00140
00141 ros::AsyncSpinner spinner(2);
00142 spinner.start();
00143
00144
00145 tf_keyboard_cal::DemoTFListener server;
00146 server.runLoop();
00147
00148
00149 ROS_INFO_STREAM_NAMED("main", "Shutting down.");
00150 ros::shutdown();
00151
00152 return 0;
00153 }