sonar.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 "sonar.hpp"
00022 
00023 namespace naoqi
00024 {
00025 namespace publisher
00026 {
00027 
00028 SonarPublisher::SonarPublisher( const std::vector<std::string>& topics )
00029   : is_initialized_(false),
00030   topics_(topics)
00031 {
00032 }
00033 
00034 void SonarPublisher::publish( const std::vector<sensor_msgs::Range>& sonar_msgs )
00035 {
00036   if ( pubs_.size() != sonar_msgs.size() )
00037   {
00038     std::cerr << "Incorrect number of sonar range messages in sonar publisher. " << sonar_msgs.size() << "/" << pubs_.size() << std::endl;
00039     return;
00040   }
00041 
00042   for( size_t i=0; i<sonar_msgs.size(); ++i)
00043   {
00044     pubs_[i].publish( sonar_msgs[i] );
00045   }
00046 }
00047 
00048 void SonarPublisher::reset( ros::NodeHandle& nh )
00049 {
00050   pubs_ = std::vector<ros::Publisher>();
00051   for( size_t i=0; i<topics_.size(); ++i)
00052   {
00053     pubs_.push_back( nh.advertise<sensor_msgs::Range>(topics_[i], 1) );
00054   }
00055 
00056   is_initialized_ = true;
00057 }
00058 
00059 } // publisher
00060 } //naoqi


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