test_subscription.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  *  Copyright (c) 2014, Willow Garage, Inc.
00004  *  All rights reserved.
00005  *
00006  *  Redistribution and use in source and binary forms, with or without
00007  *  modification, are permitted provided that the following conditions
00008  *  are met:
00009  *
00010  *   * Redistributions of source code must retain the above copyright
00011  *     notice, this list of conditions and the following disclaimer.
00012  *   * Redistributions in binary form must reproduce the above
00013  *     copyright notice, this list of conditions and the following
00014  *     disclaimer in the documentation and/or other materials provided
00015  *     with the distribution.
00016  *   * Neither the name of the Willow Garage nor the names of its
00017  *     contributors may be used to endorse or promote products derived
00018  *     from this software without specific prior written permission.
00019  *
00020  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  *  POSSIBILITY OF SUCH DAMAGE.
00032 
00033  *  Author: Julius Kammerl (jkammerl@willowgarage.com)
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     // wait for the action server to start
00064     ac_.waitForServer(); //will wait for infinite time
00065 
00066     ROS_INFO("Action server started, sending goal.");
00067     // send a goal to the action
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 = 5;
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     //wait for the action to return
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   // Called once when the goal completes
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     //ROS_INFO("Answer: %i", result->sequence.back());
00098     ros::shutdown();
00099   }
00100 
00101   // Called once when the goal becomes active
00102   void activeCb()
00103   {
00104     ROS_INFO("Goal just went active");
00105   }
00106 
00107   // Called every time feedback is received for the goal
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_republisher");
00140 
00141 TestSubscription tf2_test_republisher(ros::this_node::getName());
00142 ros::spin();
00143 
00144 return 0;
00145 }


tf2_web_republisher
Author(s): Julius Kammer
autogenerated on Mon Oct 6 2014 07:54:39