00001 /* 00002 * Copyright 2015 Aldebaran 00003 * 00004 * Licensed under the Apache License, Version 2.0 (the "License"); 00005 * you may not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 * 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 * 00016 */ 00017 00018 /* 00019 * LOCAL includes 00020 */ 00021 #include "joint_state.hpp" 00022 00023 namespace naoqi 00024 { 00025 namespace publisher 00026 { 00027 00028 JointStatePublisher::JointStatePublisher( const std::string& topic ): 00029 topic_( topic ), 00030 is_initialized_( false ) 00031 {} 00032 00033 void JointStatePublisher::publish( const sensor_msgs::JointState& js_msg, 00034 const std::vector<geometry_msgs::TransformStamped>& tf_transforms ) 00035 { 00036 pub_joint_states_.publish( js_msg ); 00037 00041 tf_broadcasterPtr_->sendTransform(tf_transforms); 00042 } 00043 00044 00045 void JointStatePublisher::reset( ros::NodeHandle& nh ) 00046 { 00047 pub_joint_states_ = nh.advertise<sensor_msgs::JointState>( topic_, 10 ); 00048 00049 tf_broadcasterPtr_ = boost::make_shared<tf2_ros::TransformBroadcaster>(); 00050 00051 is_initialized_ = true; 00052 } 00053 00054 bool JointStatePublisher::isSubscribed() const 00055 { 00056 // assume JS and TF as essential, so publish always 00057 return true; 00058 } 00059 00060 } //publisher 00061 } // naoqi