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 #include "../tools/from_any_value.hpp"
00023 
00024 /*
00025 * BOOST includes
00026 */
00027 #include <boost/foreach.hpp>
00028 #define for_each BOOST_FOREACH
00029 
00030 namespace naoqi
00031 {
00032 namespace converter
00033 {
00034 
00035 SonarConverter::SonarConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session )
00036   : BaseConverter( name, frequency, session ),
00037     p_memory_( session->service("ALMemory") ),
00038     p_sonar_( session->service("ALSonar") ),
00039     is_subscribed_(false)
00040 {
00041   std::vector<std::string> keys;
00042   if (robot_ == robot::PEPPER) {
00043     keys.push_back("Device/SubDeviceList/Platform/Front/Sonar/Sensor/Value");
00044     keys.push_back("Device/SubDeviceList/Platform/Back/Sonar/Sensor/Value");
00045     frames_.push_back("SonarFront_frame");
00046     frames_.push_back("SonarBack_frame");
00047     //topics_.push_back(topic + "/Front_sensor");
00048     //topics_.push_back(topic + "/Back_sensor");
00049   } else if (robot_ == robot::NAO) {
00050     keys.push_back("Device/SubDeviceList/US/Left/Sensor/Value");
00051     keys.push_back("Device/SubDeviceList/US/Right/Sensor/Value");
00052     frames_.push_back("LSonar_frame");
00053     frames_.push_back("RSonar_frame");
00054     //topics_.push_back(topic + "/Left_sensor");
00055     //topics_.push_back(topic + "/Right_sensor");
00056   }
00057 
00058   // Prepare the messages
00059   msgs_.resize(frames_.size());
00060   for(size_t i = 0; i < msgs_.size(); ++i)
00061     {
00062       msgs_[i].header.frame_id = frames_[i];
00063       msgs_[i].min_range = 0.25;
00064       msgs_[i].max_range = 2.55;
00065       msgs_[i].field_of_view = 0.523598776;
00066       msgs_[i].radiation_type = sensor_msgs::Range::ULTRASOUND;
00067     }
00068 
00069   keys_.resize(keys.size());
00070   size_t i = 0;
00071   for(std::vector<std::string>::const_iterator it = keys.begin(); it != keys.end(); ++it, ++i)
00072     keys_[i] = *it;
00073 }
00074 
00075 SonarConverter::~SonarConverter()
00076 {
00077   if (is_subscribed_)
00078   {
00079     p_sonar_.call<void>("unsubscribe", "ROS");
00080     is_subscribed_ = false;
00081   }
00082 }
00083 
00084 void SonarConverter::registerCallback( message_actions::MessageAction action, Callback_t cb )
00085 {
00086   callbacks_[action] = cb;
00087 }
00088 
00089 void SonarConverter::callAll( const std::vector<message_actions::MessageAction>& actions )
00090 {
00091   if (!is_subscribed_)
00092   {
00093     p_sonar_.call<void>("subscribe", "ROS");
00094     is_subscribed_ = true;
00095   }
00096 
00097   std::vector<float> values;
00098   try {
00099       qi::AnyValue anyvalues = p_memory_.call<qi::AnyValue>("getListData", keys_);
00100       tools::fromAnyValueToFloatVector(anyvalues, values);
00101   } catch (const std::exception& e) {
00102     std::cerr << "Exception caught in SonarConverter: " << e.what() << std::endl;
00103     return;
00104   }
00105   ros::Time now = ros::Time::now();
00106   for(size_t i = 0; i < msgs_.size(); ++i)
00107   {
00108     msgs_[i].header.stamp = now;
00109     msgs_[i].range = float(values[i]);
00110   }
00111 
00112   for_each( message_actions::MessageAction action, actions )
00113   {
00114     callbacks_[action]( msgs_ );
00115   }
00116 }
00117 
00118 void SonarConverter::reset( )
00119 {
00120   if (is_subscribed_)
00121   {
00122     p_sonar_.call<void>("unsubscribe", "ROS");
00123     is_subscribed_ = false;
00124   }
00125 }
00126 
00127 } // publisher
00128 } //naoqi


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