turtle_tf2_broadcaster.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
4 #include <geometry_msgs/TransformStamped.h>
5 #include <turtlesim/Pose.h>
6 
7 std::string turtle_name;
8 
9 
10 
11 void poseCallback(const turtlesim::PoseConstPtr& msg){
13  geometry_msgs::TransformStamped transformStamped;
14 
15  transformStamped.header.stamp = ros::Time::now();
16  transformStamped.header.frame_id = "world";
17  transformStamped.child_frame_id = turtle_name;
18  transformStamped.transform.translation.x = msg->x;
19  transformStamped.transform.translation.y = msg->y;
20  transformStamped.transform.translation.z = 0.0;
22  q.setRPY(0, 0, msg->theta);
23  transformStamped.transform.rotation.x = q.x();
24  transformStamped.transform.rotation.y = q.y();
25  transformStamped.transform.rotation.z = q.z();
26  transformStamped.transform.rotation.w = q.w();
27 
28  br.sendTransform(transformStamped);
29 }
30 
31 int main(int argc, char** argv){
32  ros::init(argc, argv, "my_tf2_broadcaster");
33 
34  ros::NodeHandle private_node("~");
35  if (! private_node.hasParam("turtle"))
36  {
37  if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
38  turtle_name = argv[1];
39  }
40  else
41  {
42  private_node.getParam("turtle", turtle_name);
43  }
44 
45  ros::NodeHandle node;
46  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
47 
48  ros::spin();
49  return 0;
50 };
51 
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
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)
int main(int argc, char **argv)
ROSCPP_DECL void spin(Spinner &spinner)
std::string turtle_name
void sendTransform(const geometry_msgs::TransformStamped &transform)
bool getParam(const std::string &key, std::string &s) const
static Time now()
bool hasParam(const std::string &key) const
#define ROS_ERROR(...)
void poseCallback(const turtlesim::PoseConstPtr &msg)


turtle_tf2
Author(s): Denis Štogl
autogenerated on Wed Jun 5 2019 20:50:43