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 
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 
50 {
51 
53  pub_ = it.advertiseCamera( topic_, 1 );
54 
55  // Unregister compressedDepth topics for non depth cameras
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