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 #include <ros/ros.h>
00035
00036 #include "static_transform_broadcaster/SetStaticTransform.h"
00037 #include "static_transform_broadcaster/RemoveStaticTransforms.h"
00038
00039 #include "static_transform_broadcaster/static_transform_broadcaster.h"
00040
00041 namespace static_transform_broadcaster {
00042
00043 static std::string set_transform_service_name = "set_static_transform";
00044 static std::string remove_transform_service_name = "remove_static_transform";
00045
00046 class StaticTransformBroadcasterNode
00047 {
00048 private:
00049 StaticTransformBroadcaster broadcaster_;
00050
00052 ros::NodeHandle priv_nh_;
00054 ros::NodeHandle root_nh_;
00055
00056 ros::ServiceServer set_transform_srv_;
00057 ros::ServiceServer remove_transforms_srv_;
00058
00059 bool setTransformCB(SetStaticTransform::Request &request, SetStaticTransform::Response &response)
00060 {
00061 broadcaster_.setTransform(request.transform);
00062 return true;
00063 }
00064
00065 bool removeTransformsCB(RemoveStaticTransforms::Request &request, RemoveStaticTransforms::Response &response)
00066 {
00067 if (request.remove_all_transforms)
00068 {
00069 broadcaster_.removeAllTransforms();
00070 return true;
00071 }
00072 for (size_t i=0; i<request.child_frame_ids.size(); i++)
00073 {
00074 broadcaster_.removeTransform(request.child_frame_ids[i]);
00075 }
00076 return true;
00077 }
00078
00079 public:
00080 StaticTransformBroadcasterNode() : priv_nh_("~"), root_nh_("")
00081 {
00082 set_transform_srv_ = priv_nh_.advertiseService(set_transform_service_name,
00083 &StaticTransformBroadcasterNode::setTransformCB, this);
00084 remove_transforms_srv_ = priv_nh_.advertiseService(remove_transform_service_name,
00085 &StaticTransformBroadcasterNode::removeTransformsCB, this);
00086 }
00087
00088 ~StaticTransformBroadcasterNode() {}
00089 };
00090
00091 }
00092
00093 int main(int argc, char **argv)
00094 {
00095 ros::init(argc, argv, "static_transform_broadcaster");
00096 ros::NodeHandle nh;
00097 static_transform_broadcaster::StaticTransformBroadcasterNode node;
00098 ros::spin();
00099 return 0;
00100 }