camera.cpp
Go to the documentation of this file.
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 "camera.hpp"
00022 
00023 /*
00024 * ALDEBARAN includes
00025 */
00026 #include "../tools/alvisiondefinitions.h" // for kTop...
00027 
00028 namespace naoqi
00029 {
00030 namespace publisher
00031 {
00032 
00033 CameraPublisher::CameraPublisher( const std::string& topic, int camera_source ):
00034   topic_( topic ),
00035   is_initialized_(false),
00036   camera_source_( camera_source )
00037 {
00038 }
00039 
00040 CameraPublisher::~CameraPublisher()
00041 {
00042 }
00043 
00044 void CameraPublisher::publish( const sensor_msgs::ImagePtr& img, const sensor_msgs::CameraInfo& camera_info )
00045 {
00046   pub_.publish( *img, camera_info );
00047 }
00048 
00049 void CameraPublisher::reset( ros::NodeHandle& nh )
00050 {
00051 
00052   image_transport::ImageTransport it( nh );
00053   pub_ = it.advertiseCamera( topic_, 1 );
00054 
00055   // Unregister compressedDepth topics for non depth cameras
00056   if (camera_source_!=AL::kDepthCamera)
00057   {
00058     // Get our URI as a caller
00059     std::string node_name = ros::this_node::getName();
00060     XmlRpc::XmlRpcValue args, result, payload;
00061     args[0] = node_name;
00062     args[1] = node_name;
00063     ros::master::execute("lookupNode", args, result, payload, false);
00064     args[2] = result[2];
00065 
00066     // List the topics to remove
00067     std::vector<std::string> topic_list;
00068     topic_list.push_back(std::string("/") + node_name + "/" + topic_ + std::string("/compressedDepth"));
00069     topic_list.push_back(std::string("/") + node_name + "/" + topic_ + std::string("/compressedDepth/parameter_updates"));
00070     topic_list.push_back(std::string("/") + node_name + "/" + topic_ + std::string("/compressedDepth/parameter_descriptions"));
00071 
00072     // Remove undesirable topics
00073     for(std::vector<std::string>::const_iterator topic = topic_list.begin(); topic != topic_list.end(); ++topic)
00074     {
00075       args[1] = *topic;
00076       ros::master::execute("unregisterPublisher", args, result, payload, false);
00077     }
00078   }
00079 
00080   is_initialized_ = true;
00081 }
00082 
00083 } // publisher
00084 } //naoqi


naoqi_driver
Author(s): Karsten Knese
autogenerated on Sun Sep 17 2017 02:57:14