publishers/joint_state.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2015 Aldebaran
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 /*
19 * LOCAL includes
20 */
21 #include "joint_state.hpp"
22 
23 namespace naoqi
24 {
25 namespace publisher
26 {
27 
28 JointStatePublisher::JointStatePublisher( const std::string& topic ):
29  topic_( topic ),
30  is_initialized_( false )
31 {}
32 
33 void JointStatePublisher::publish( const sensor_msgs::JointState& js_msg,
34  const std::vector<geometry_msgs::TransformStamped>& tf_transforms )
35 {
36  pub_joint_states_.publish( js_msg );
37 
41  tf_broadcasterPtr_->sendTransform(tf_transforms);
42 }
43 
44 
46 {
47  pub_joint_states_ = nh.advertise<sensor_msgs::JointState>( topic_, 10 );
48 
49  tf_broadcasterPtr_ = boost::make_shared<tf2_ros::TransformBroadcaster>();
50 
51  is_initialized_ = true;
52 }
53 
55 {
56  // assume JS and TF as essential, so publish always
57  return true;
58 }
59 
60 } //publisher
61 } // naoqi
naoqi::publisher::JointStatePublisher::is_initialized_
bool is_initialized_
Definition: publishers/joint_state.hpp:65
naoqi::publisher::JointStatePublisher::tf_broadcasterPtr_
boost::shared_ptr< tf2_ros::TransformBroadcaster > tf_broadcasterPtr_
Definition: publishers/joint_state.hpp:58
naoqi::publisher::JointStatePublisher::JointStatePublisher
JointStatePublisher(const std::string &topic="/joint_states")
Definition: publishers/joint_state.cpp:28
joint_state.hpp
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
naoqi
Definition: converter.hpp:29
naoqi::publisher::JointStatePublisher::isSubscribed
virtual bool isSubscribed() const
Definition: publishers/joint_state.cpp:54
naoqi::publisher::JointStatePublisher::publish
virtual void publish(const sensor_msgs::JointState &js_msg, const std::vector< geometry_msgs::TransformStamped > &tf_transforms)
Definition: publishers/joint_state.cpp:33
naoqi::publisher::JointStatePublisher::reset
virtual void reset(ros::NodeHandle &nh)
Definition: publishers/joint_state.cpp:45
naoqi::publisher::JointStatePublisher::topic_
std::string topic_
Definition: publishers/joint_state.hpp:63
naoqi::publisher::JointStatePublisher::pub_joint_states_
ros::Publisher pub_joint_states_
Definition: publishers/joint_state.hpp:61
ros::NodeHandle


naoqi_driver
Author(s): Karsten Knese
autogenerated on Sat Feb 3 2024 03:50:06