$search
00001 /********************************************************************* 00002 * 00003 * Copyright (c) 2009, 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 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 }