src
publishers
publishers/camera.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 "
camera.hpp
"
22
23
/*
24
* ALDEBARAN includes
25
*/
26
#include "../tools/alvisiondefinitions.h"
// for kTop...
27
28
namespace
naoqi
29
{
30
namespace
publisher
31
{
32
33
CameraPublisher::CameraPublisher
(
const
std::string& topic,
int
camera_source ):
34
topic_( topic ),
35
is_initialized_(false),
36
camera_source_( camera_source )
37
{
38
}
39
40
CameraPublisher::~CameraPublisher
()
41
{
42
}
43
44
void
CameraPublisher::publish
(
const
sensor_msgs::ImagePtr& img,
const
sensor_msgs::CameraInfo& camera_info )
45
{
46
pub_
.
publish
( *img, camera_info );
47
}
48
49
void
CameraPublisher::reset
(
ros::NodeHandle
& nh )
50
{
51
52
image_transport::ImageTransport
it( nh );
53
pub_
= it.
advertiseCamera
(
topic_
, 1 );
54
55
// Unregister compressedDepth topics for non depth cameras
56
if
(
camera_source_
!=
AL::kDepthCamera
)
57
{
58
// Get our URI as a caller
59
std::string node_name =
ros::this_node::getName
();
60
XmlRpc::XmlRpcValue
args
, result, payload;
61
args
[0] = node_name;
62
args
[1] = node_name;
63
ros::master::execute
(
"lookupNode"
,
args
, result, payload,
false
);
64
args
[2] = result[2];
65
66
// List the topics to remove
67
std::vector<std::string> topic_list;
68
topic_list.push_back(std::string(
"/"
) + node_name +
"/"
+
topic_
+ std::string(
"/compressedDepth"
));
69
topic_list.push_back(std::string(
"/"
) + node_name +
"/"
+
topic_
+ std::string(
"/compressedDepth/parameter_updates"
));
70
topic_list.push_back(std::string(
"/"
) + node_name +
"/"
+
topic_
+ std::string(
"/compressedDepth/parameter_descriptions"
));
71
72
// Remove undesirable topics
73
for
(std::vector<std::string>::const_iterator
topic
= topic_list.begin();
topic
!= topic_list.end(); ++
topic
)
74
{
75
args
[1] = *
topic
;
76
ros::master::execute
(
"unregisterPublisher"
,
args
, result, payload,
false
);
77
}
78
}
79
80
is_initialized_
=
true
;
81
}
82
83
}
// publisher
84
}
//naoqi
image_transport::ImageTransport
camera.hpp
image_transport::CameraPublisher::publish
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
naoqi::publisher::CameraPublisher::~CameraPublisher
~CameraPublisher()
Definition:
publishers/camera.cpp:40
image_transport::ImageTransport::advertiseCamera
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
naoqi
Definition:
converter.hpp:29
naoqi::publisher::CameraPublisher::publish
void publish(const sensor_msgs::ImagePtr &img, const sensor_msgs::CameraInfo &camera_info)
Definition:
publishers/camera.cpp:44
naoqi::publisher::CameraPublisher::is_initialized_
bool is_initialized_
Definition:
publishers/camera.hpp:62
naoqi::publisher::CameraPublisher::topic
std::string topic() const
Definition:
publishers/camera.hpp:39
ros::master::execute
ROSCPP_DECL bool execute(const std::string &method, const XmlRpc::XmlRpcValue &request, XmlRpc::XmlRpcValue &response, XmlRpc::XmlRpcValue &payload, bool wait_for_master)
ros::this_node::getName
const ROSCPP_DECL std::string & getName()
naoqi::publisher::CameraPublisher::camera_source_
int camera_source_
Definition:
publishers/camera.hpp:67
naoqi::publisher::CameraPublisher::topic_
std::string topic_
Definition:
publishers/camera.hpp:60
AL::kDepthCamera
const int kDepthCamera
Definition:
alvisiondefinitions.h:38
args
naoqi::publisher::CameraPublisher::pub_
image_transport::CameraPublisher pub_
Definition:
publishers/camera.hpp:65
naoqi::publisher::CameraPublisher::CameraPublisher
CameraPublisher(const std::string &topic, int camera_source)
Definition:
publishers/camera.cpp:33
naoqi::publisher::CameraPublisher::reset
void reset(ros::NodeHandle &nh)
Definition:
publishers/camera.cpp:49
XmlRpc::XmlRpcValue
ros::NodeHandle
naoqi_driver
Author(s): Karsten Knese
autogenerated on Sat Feb 3 2024 03:50:06