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 #include <ros/ros.h>
00038 #include <tf2_ros/transform_listener.h>
00039
00040 #include <boost/thread/mutex.hpp>
00041 #include "boost/thread.hpp"
00042
00043 #include <tf/tfMessage.h>
00044 #include <tf/transform_datatypes.h>
00045 #include <geometry_msgs/TransformStamped.h>
00046
00047 #include <actionlib/client/simple_action_client.h>
00048 #include <actionlib/client/terminal_state.h>
00049 #include <tf2_web_republisher/TFSubscriptionAction.h>
00050
00051 class TestSubscription
00052 {
00053 public:
00054
00055 TestSubscription(const std::string& name) :
00056 ac_("tf2_web_republisher", true)
00057 {
00058 std::string sub_topic;
00059 nh_.param<std::string>("tf_topic", sub_topic, "/tf_web");
00060 sub_ = nh_.subscribe(sub_topic, 1, &TestSubscription::callbackTF, this);
00061
00062 ROS_INFO("Waiting for action server to start.");
00063
00064 ac_.waitForServer();
00065
00066 ROS_INFO("Action server started, sending goal.");
00067
00068 tf2_web_republisher::TFSubscriptionGoal goal;
00069 goal.source_frames.resize(1);
00070 goal.source_frames[0] = "base";
00071 goal.target_frame = "target";
00072 goal.angular_thres = 0.0f;
00073 goal.trans_thres = 0.0f;
00074 goal.rate = 1;
00075
00076 ac_.sendGoal(goal, boost::bind(&TestSubscription::doneCb, this, _1, _2), boost::bind(&TestSubscription::activeCb, this), boost::bind(&TestSubscription::feedbackCb, this, _1));
00077
00078
00079 bool finished_before_timeout = ac_.waitForResult(ros::Duration(10.0));
00080
00081 if (finished_before_timeout)
00082 {
00083 actionlib::SimpleClientGoalState state = ac_.getState();
00084 ROS_INFO("Action finished: %s", state.toString().c_str());
00085 }
00086 else
00087 ROS_INFO("Action did not finish before the time out.");
00088 }
00089
00090 ~TestSubscription() {}
00091
00092
00093 void doneCb(const actionlib::SimpleClientGoalState& state,
00094 const tf2_web_republisher::TFSubscriptionResultConstPtr& result)
00095 {
00096 ROS_INFO("Finished in state [%s]", state.toString().c_str());
00097
00098 ros::shutdown();
00099 }
00100
00101
00102 void activeCb()
00103 {
00104 ROS_INFO("Goal just went active");
00105 }
00106
00107
00108 void feedbackCb(const tf2_web_republisher::TFSubscriptionFeedbackConstPtr& feedback)
00109 {
00110 ROS_INFO("Got Feedback of length %lu", feedback->transforms.size());
00111 }
00112
00113 void callbackTF(const tf::tfMessageConstPtr &msg)
00114 {
00115 size_t i;
00116 size_t msg_size = msg->transforms.size();
00117 for (i = 0; i < msg_size; i++)
00118 {
00119 const geometry_msgs::TransformStamped& trans_msg = msg->transforms[i];
00120
00121 const std::string& base_key = trans_msg.header.frame_id;
00122 const std::string& target_key = trans_msg.child_frame_id;
00123
00124 ROS_INFO_STREAM("Received transform from "<<base_key<<" to "<<target_key);
00125 }
00126 }
00127
00128
00129 protected:
00130
00131 ros::NodeHandle nh_;
00132 ros::Subscriber sub_;
00133
00134 actionlib::SimpleActionClient<tf2_web_republisher::TFSubscriptionAction> ac_;
00135 };
00136
00137 int main(int argc, char **argv)
00138 {
00139 ros::init(argc, argv, "tf2_test_web_republisher2");
00140
00141 TestSubscription tf2_test_republisher(ros::this_node::getName());
00142 ros::spin();
00143
00144 return 0;
00145 }