specific_transform_subscriber.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <tf/tf.h>
5 #include <iostream>
7 #include <dynamic_tf_publisher/SetDynamicTF.h>
8 
9 void CallSetDynamicTf(std::string parent_frame_id, std::string frame_id, geometry_msgs::Transform transform){
10  dynamic_tf_publisher::SetDynamicTF SetTf;
11  SetTf.request.freq = 10;
12  SetTf.request.cur_tf.header.stamp = ros::Time::now();
13  SetTf.request.cur_tf.header.frame_id = parent_frame_id;
14  SetTf.request.cur_tf.child_frame_id = frame_id;
15  SetTf.request.cur_tf.transform = transform;
17 }
18 
19 
20 void transformCallback(const tf::tfMessage::ConstPtr& msg){
21  for(int i=0; i<msg->transforms.size(); i++){
22  CallSetDynamicTf(msg->transforms[i].header.frame_id, msg->transforms[i].child_frame_id, msg->transforms[i].transform);
23  }
24 }
25 
26 int main(int argc, char** argv)
27 {
28  ros::init(argc, argv, "specific_transform_subscriber");
29 
30  ros::NodeHandle nh_;
31  ros::NodeHandle pnh_("~");
32 
33  ros::Subscriber sub_ = pnh_.subscribe<tf::tfMessage>
34  ("/specific_transform", 100, transformCallback);
35 
36  dynamic_tf_publisher_client = nh_.serviceClient<dynamic_tf_publisher::SetDynamicTF>("set_dynamic_tf");
37  ros::service::waitForService("set_dynamic_tf", -1);
38 
39  ros::spin();
40 }
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
ROSCPP_DECL void spin(Spinner &spinner)
int main(int argc, char **argv)
static Time now()
void transformCallback(const tf::tfMessage::ConstPtr &msg)
void CallSetDynamicTf(std::string parent_frame_id, std::string frame_id, geometry_msgs::Transform transform)
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)
ros::ServiceClient dynamic_tf_publisher_client


jsk_topic_tools
Author(s): Kei Okada , Yusuke Furuta
autogenerated on Tue Feb 6 2018 03:45:19