Go to the documentation of this file.00001 #ifndef __SLAM_FMWK_ROS_UTILS_H
00002 #define __SLAM_FMWK_ROS_UTILS_H
00003
00004 #include <string>
00005 #include <tf/transform_broadcaster.h>
00006 #include <ros/ros.h>
00007
00014 inline void publish_2D_transform(const std::string &target_frame,
00015 const std::string &base_frame,
00016 double x, double y, double th) {
00017
00018 tf::Vector3 translation(x, y, 0);
00019 tf::Quaternion rotation;
00020 rotation.setRPY(0, 0, th);
00021
00022 tf::Transform tr(rotation, translation);
00023 tf::StampedTransform st_trans(tr, ros::Time::now(), base_frame, target_frame);
00024
00025 static tf::TransformBroadcaster br;
00026 br.sendTransform(st_trans);
00027 }
00028
00029 #endif