naoqi_driver: laser.cpp Source File

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 "laser.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 static const char* laserMemoryKeys[] = {
00036   // RIGHT LASER
00037   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg01/X/Sensor/Value",
00038   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg01/Y/Sensor/Value",
00039   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg02/X/Sensor/Value",
00040   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg02/Y/Sensor/Value",
00041   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg03/X/Sensor/Value",
00042   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg03/Y/Sensor/Value",
00043   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg04/X/Sensor/Value",
00044   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg04/Y/Sensor/Value",
00045   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg05/X/Sensor/Value",
00046   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg05/Y/Sensor/Value",
00047   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg06/X/Sensor/Value",
00048   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg06/Y/Sensor/Value",
00049   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg07/X/Sensor/Value",
00050   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg07/Y/Sensor/Value",
00051   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg08/X/Sensor/Value",
00052   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg08/Y/Sensor/Value",
00053   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg09/X/Sensor/Value",
00054   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg09/Y/Sensor/Value",
00055   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg10/X/Sensor/Value",
00056   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg10/Y/Sensor/Value",
00057   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg11/X/Sensor/Value",
00058   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg11/Y/Sensor/Value",
00059   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg12/X/Sensor/Value",
00060   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg12/Y/Sensor/Value",
00061   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg13/X/Sensor/Value",
00062   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg13/Y/Sensor/Value",
00063   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg14/X/Sensor/Value",
00064   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg14/Y/Sensor/Value",
00065   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg15/X/Sensor/Value",
00066   "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg15/Y/Sensor/Value",
00067   // FRONT LASER
00068   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg01/X/Sensor/Value",
00069   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg01/Y/Sensor/Value",
00070   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg02/X/Sensor/Value",
00071   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg02/Y/Sensor/Value",
00072   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg03/X/Sensor/Value",
00073   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg03/Y/Sensor/Value",
00074   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg04/X/Sensor/Value",
00075   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg04/Y/Sensor/Value",
00076   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg05/X/Sensor/Value",
00077   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg05/Y/Sensor/Value",
00078   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg06/X/Sensor/Value",
00079   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg06/Y/Sensor/Value",
00080   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg07/X/Sensor/Value",
00081   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg07/Y/Sensor/Value",
00082   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg08/X/Sensor/Value",
00083   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg08/Y/Sensor/Value",
00084   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg09/X/Sensor/Value",
00085   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg09/Y/Sensor/Value",
00086   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg10/X/Sensor/Value",
00087   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg10/Y/Sensor/Value",
00088   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg11/X/Sensor/Value",
00089   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg11/Y/Sensor/Value",
00090   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg12/X/Sensor/Value",
00091   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg12/Y/Sensor/Value",
00092   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg13/X/Sensor/Value",
00093   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg13/Y/Sensor/Value",
00094   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg14/X/Sensor/Value",
00095   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg14/Y/Sensor/Value",
00096   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg15/X/Sensor/Value",
00097   "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg15/Y/Sensor/Value",
00098   // LEFT LASER
00099   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg01/X/Sensor/Value",
00100   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg01/Y/Sensor/Value",
00101   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg02/X/Sensor/Value",
00102   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg02/Y/Sensor/Value",
00103   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg03/X/Sensor/Value",
00104   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg03/Y/Sensor/Value",
00105   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg04/X/Sensor/Value",
00106   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg04/Y/Sensor/Value",
00107   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg05/X/Sensor/Value",
00108   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg05/Y/Sensor/Value",
00109   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg06/X/Sensor/Value",
00110   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg06/Y/Sensor/Value",
00111   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg07/X/Sensor/Value",
00112   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg07/Y/Sensor/Value",
00113   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg08/X/Sensor/Value",
00114   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg08/Y/Sensor/Value",
00115   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg09/X/Sensor/Value",
00116   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg09/Y/Sensor/Value",
00117   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg10/X/Sensor/Value",
00118   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg10/Y/Sensor/Value",
00119   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg11/X/Sensor/Value",
00120   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg11/Y/Sensor/Value",
00121   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg12/X/Sensor/Value",
00122   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg12/Y/Sensor/Value",
00123   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg13/X/Sensor/Value",
00124   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg13/Y/Sensor/Value",
00125   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg14/X/Sensor/Value",
00126   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg14/Y/Sensor/Value",
00127   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg15/X/Sensor/Value",
00128   "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg15/Y/Sensor/Value",
00129 };
00130 
00131 LaserConverter::LaserConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session ):
00132   BaseConverter( name, frequency, session ),
00133   p_memory_(session->service("ALMemory"))
00134 {
00135 }
00136 
00137 void LaserConverter::registerCallback( message_actions::MessageAction action, Callback_t cb )
00138 {
00139   callbacks_[action] = cb;
00140 }
00141 
00142 void LaserConverter::callAll( const std::vector<message_actions::MessageAction>& actions )
00143 {
00144   static const std::vector<std::string> laser_keys_value(laserMemoryKeys, laserMemoryKeys+90);
00145 
00146   std::vector<float> result_value;
00147   try {
00148       qi::AnyValue anyvalues = p_memory_.call<qi::AnyValue>("getListData", laser_keys_value);
00149       tools::fromAnyValueToFloatVector(anyvalues, result_value);
00150   } catch (const std::exception& e) {
00151     std::cerr << "Exception caught in LaserConverter: " << e.what() << std::endl;
00152     return;
00153   }
00154   msg_.header.stamp = ros::Time::now();
00155   //  prepare the right sensor frame
00156   size_t pos = 0;
00157 
00165   // ranges between 0-29
00166   for( size_t i=0; i<30; i=i+2, ++pos)
00167   {
00168     const float lx = result_value[28-i]; // segments are internally flipped
00169     const float ly = result_value[28-i+1]; // segments are internally flipped
00170     float bx = lx*std::cos(-1.757) - ly*std::sin(-1.757) - 0.018;
00171     float by = lx*std::sin(-1.757) + ly*std::cos(-1.757) - 0.090;
00172     float dist = std::sqrt( std::pow(bx,2) + std::pow(by,2) );
00173     //float dist = std::sqrt( std::pow(lx,2) + std::pow(ly,2) );
00174     //std::cout << "got a distance at "<< pos << " with "  << dist << std::endl;
00175     msg_.ranges[pos] = dist;
00176   }
00177 
00178   pos = pos+8; // leave out 8 blanks ==> pos = 15+8
00179 
00180   // ranges between 30-59
00181   for( size_t i=0; i<30; i=i+2, ++pos)
00182   {
00183     const float lx = result_value[58-i];
00184     const float ly = result_value[58-i+1];
00185     float bx = lx + 0.056 ;
00186     float by = ly;
00187     float dist = std::sqrt( std::pow(bx,2) + std::pow(by,2) );
00188     //float dist = std::sqrt( std::pow(lx,2) + std::pow(ly,2) );
00189     //std::cout << "got a distance at "<< pos << " with "  << dist << std::endl;
00190     msg_.ranges[pos] = dist;
00191   }
00192 
00193   pos = pos+8; // leave out again 8 blanks ==> pos = 15+8+15+8
00194 
00195   for( size_t i=0; i<30; i=i+2, ++pos)
00196   {
00197     const float lx = result_value[88-i];
00198     const float ly = result_value[88-i+1];
00199     float bx = lx*std::cos(1.757) - ly*std::sin(1.757) - 0.018;
00200     float by = lx*std::sin(1.757) + ly*std::cos(1.757) + 0.090;
00201     float dist = std::sqrt( std::pow(bx,2) + std::pow(by,2) );
00202     //float dist = std::sqrt( std::pow(lx,2) + std::pow(ly,2) );
00203     //std::cout << "got a distance at "<< pos << " with "  << dist << std::endl;
00204     msg_.ranges[pos] = dist;
00205   }
00206 
00207   for_each( message_actions::MessageAction action, actions )
00208   {
00209     callbacks_[action](msg_);
00210   }
00211 }
00212 
00213 void LaserConverter::reset( )
00214 {
00215   msg_.header.frame_id = "base_footprint";
00216   msg_.angle_min = -2.0944;   // -120
00217   msg_.angle_max = 2.0944;    // +120
00218   msg_.angle_increment = (2*2.0944) / (15+15+15+8+8); // 240 deg FoV / 61 points (blind zones inc)
00219   msg_.range_min = 0.1; // in m
00220   msg_.range_max = 1.5; // in m
00221   msg_.ranges = std::vector<float>(61, -1.0f);
00222 }
00223 
00224 } //converter
00225 } // naoqi