src
publishers
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
45
void
JointStatePublisher::reset
(
ros::NodeHandle
& nh )
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
54
bool
JointStatePublisher::isSubscribed
()
const
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