publishers/sonar.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 "sonar.hpp"
22 
23 namespace naoqi
24 {
25 namespace publisher
26 {
27 
28 SonarPublisher::SonarPublisher( const std::vector<std::string>& topics )
29  : is_initialized_(false),
30  topics_(topics)
31 {
32 }
33 
34 void SonarPublisher::publish( const std::vector<sensor_msgs::Range>& sonar_msgs )
35 {
36  if ( pubs_.size() != sonar_msgs.size() )
37  {
38  std::cerr << "Incorrect number of sonar range messages in sonar publisher. " << sonar_msgs.size() << "/" << pubs_.size() << std::endl;
39  return;
40  }
41 
42  for( size_t i=0; i<sonar_msgs.size(); ++i)
43  {
44  pubs_[i].publish( sonar_msgs[i] );
45  }
46 }
47 
49 {
50  pubs_ = std::vector<ros::Publisher>();
51  for( size_t i=0; i<topics_.size(); ++i)
52  {
53  pubs_.push_back( nh.advertise<sensor_msgs::Range>(topics_[i], 1) );
54  }
55 
56  is_initialized_ = true;
57 }
58 
59 } // publisher
60 } //naoqi
std::vector< ros::Publisher > pubs_
void reset(ros::NodeHandle &nh)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
SonarPublisher(const std::vector< std::string > &topics)
void publish(const std::vector< sensor_msgs::Range > &sonar_msgs)
std::vector< std::string > topics_


naoqi_driver
Author(s): Karsten Knese
autogenerated on Sat Feb 15 2020 03:24:26