touch.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 #include <iostream>
00019 #include <vector>
00020 
00021 #include <boost/make_shared.hpp>
00022 
00023 #include <ros/ros.h>
00024 
00025 #include <qi/anyobject.hpp>
00026 
00027 #include <naoqi_driver/recorder/globalrecorder.hpp>
00028 #include <naoqi_driver/message_actions.h>
00029 
00030 #include "touch.hpp"
00031 
00032 namespace naoqi
00033 {
00034 
00035 template<class T>
00036 TouchEventRegister<T>::TouchEventRegister()
00037 {
00038 }
00039 
00040 template<class T>
00041 TouchEventRegister<T>::TouchEventRegister( const std::string& name, const std::vector<std::string> keys, const float& frequency, const qi::SessionPtr& session )
00042   : serviceId(0),
00043     p_memory_( session->service("ALMemory")),
00044     session_(session),
00045     isStarted_(false),
00046     isPublishing_(false),
00047     isRecording_(false),
00048     isDumping_(false)
00049 {
00050   publisher_ = boost::make_shared<publisher::BasicPublisher<T> >( name );
00051   //recorder_ = boost::make_shared<recorder::BasicEventRecorder<T> >( name );
00052   converter_ = boost::make_shared<converter::TouchEventConverter<T> >( name, frequency, session );
00053 
00054   converter_->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::BasicPublisher<T>::publish, publisher_, _1) );
00055   //converter_->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicEventRecorder<T>::write, recorder_, _1) );
00056   //converter_->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicEventRecorder<T>::bufferize, recorder_, _1) );
00057 
00058   keys_.resize(keys.size());
00059   size_t i = 0;
00060   for(std::vector<std::string>::const_iterator it = keys.begin(); it != keys.end(); ++it, ++i)
00061     keys_[i] = *it;
00062 
00063   name_ = name;
00064 }
00065 
00066 template<class T>
00067 TouchEventRegister<T>::~TouchEventRegister()
00068 {
00069   stopProcess();
00070 }
00071 
00072 template<class T>
00073 void TouchEventRegister<T>::resetPublisher(ros::NodeHandle& nh)
00074 {
00075   publisher_->reset(nh);
00076 }
00077 
00078 template<class T>
00079 void TouchEventRegister<T>::resetRecorder( boost::shared_ptr<naoqi::recorder::GlobalRecorder> gr )
00080 {
00081   //recorder_->reset(gr, converter_->frequency());
00082 }
00083 
00084 template<class T>
00085 void TouchEventRegister<T>::startProcess()
00086 {
00087   boost::mutex::scoped_lock start_lock(mutex_);
00088   if (!isStarted_)
00089   {
00090     if(!serviceId)
00091     {
00092       //std::string serviceName = std::string("ROS-Driver-") + typeid(T).name();
00093       std::string serviceName = std::string("ROS-Driver-") + keys_[0];
00094       serviceId = session_->registerService(serviceName, this->shared_from_this());
00095       for(std::vector<std::string>::const_iterator it = keys_.begin(); it != keys_.end(); ++it) {
00096         std::cerr << *it << std::endl;
00097         p_memory_.call<void>("subscribeToEvent",it->c_str(), serviceName, "touchCallback");
00098       }
00099       std::cout << serviceName << " : Start" << std::endl;
00100     }
00101     isStarted_ = true;
00102   }
00103 }
00104 
00105 template<class T>
00106 void TouchEventRegister<T>::stopProcess()
00107 {
00108   boost::mutex::scoped_lock stop_lock(mutex_);
00109   if (isStarted_)
00110   {
00111     //std::string serviceName = std::string("ROS-Driver-") + typeid(T).name();
00112     std::string serviceName = std::string("ROS-Driver-") + keys_[0];
00113     if(serviceId){
00114       for(std::vector<std::string>::const_iterator it = keys_.begin(); it != keys_.end(); ++it) {
00115         p_memory_.call<void>("unsubscribeToEvent",it->c_str(), serviceName);
00116       }
00117       session_->unregisterService(serviceId);
00118       serviceId = 0;
00119     }
00120     std::cout << serviceName << " : Stop" << std::endl;
00121     isStarted_ = false;
00122   }
00123 }
00124 
00125 template<class T>
00126 void TouchEventRegister<T>::writeDump(const ros::Time& time)
00127 {
00128   if (isStarted_)
00129   {
00130     //recorder_->writeDump(time);
00131   }
00132 }
00133 
00134 template<class T>
00135 void TouchEventRegister<T>::setBufferDuration(float duration)
00136 {
00137   //recorder_->setBufferDuration(duration);
00138 }
00139 
00140 template<class T>
00141 void TouchEventRegister<T>::isRecording(bool state)
00142 {
00143   boost::mutex::scoped_lock rec_lock(mutex_);
00144   isRecording_ = state;
00145 }
00146 
00147 template<class T>
00148 void TouchEventRegister<T>::isPublishing(bool state)
00149 {
00150   boost::mutex::scoped_lock pub_lock(mutex_);
00151   isPublishing_ = state;
00152 }
00153 
00154 template<class T>
00155 void TouchEventRegister<T>::isDumping(bool state)
00156 {
00157   boost::mutex::scoped_lock dump_lock(mutex_);
00158   isDumping_ = state;
00159 }
00160 
00161 template<class T>
00162 void TouchEventRegister<T>::registerCallback()
00163 {
00164 }
00165 
00166 template<class T>
00167 void TouchEventRegister<T>::unregisterCallback()
00168 {
00169 }
00170 
00171 template<class T>
00172 void TouchEventRegister<T>::touchCallback(std::string &key, qi::AnyValue &value, qi::AnyValue &message)
00173 {
00174   T msg = T();
00175   
00176   bool state =  value.toFloat() > 0.5f;
00177 
00178   //std::cerr << key << " " << state << std::endl;
00179 
00180   touchCallbackMessage(key, state, msg);
00181 
00182   std::vector<message_actions::MessageAction> actions;
00183   boost::mutex::scoped_lock callback_lock(mutex_);
00184   if (isStarted_) {
00185     // CHECK FOR PUBLISH
00186     if ( isPublishing_ && publisher_->isSubscribed() )
00187     {
00188       actions.push_back(message_actions::PUBLISH);
00189     }
00190     // CHECK FOR RECORD
00191     if ( isRecording_ )
00192     {
00193       //actions.push_back(message_actions::RECORD);
00194     }
00195     if ( !isDumping_ )
00196     {
00197       //actions.push_back(message_actions::LOG);
00198     }
00199     if (actions.size() >0)
00200     {
00201       converter_->callAll( actions, msg );
00202     }
00203   }
00204 }
00205 
00206 template<class T>
00207 void TouchEventRegister<T>::touchCallbackMessage(std::string &key, bool &state, naoqi_bridge_msgs::Bumper &msg)
00208 {
00209   int i = 0;
00210   for(std::vector<std::string>::const_iterator it = keys_.begin(); it != keys_.end(); ++it, ++i)
00211   {
00212     if ( key == it->c_str() ) {
00213       msg.bumper = i;
00214       msg.state = state?(naoqi_bridge_msgs::Bumper::statePressed):(naoqi_bridge_msgs::Bumper::stateReleased);
00215     }
00216   }
00217 }
00218 
00219 template<class T>
00220 void TouchEventRegister<T>::touchCallbackMessage(std::string &key, bool &state, naoqi_bridge_msgs::HandTouch &msg)
00221 {
00222   int i = 0;
00223   for(std::vector<std::string>::const_iterator it = keys_.begin(); it != keys_.end(); ++it, ++i)
00224   {
00225     if ( key == it->c_str() ) {
00226       msg.hand = i;
00227       msg.state = state?(naoqi_bridge_msgs::HandTouch::statePressed):(naoqi_bridge_msgs::HandTouch::stateReleased);
00228     }
00229   }
00230 }
00231 
00232 template<class T>
00233 void TouchEventRegister<T>::touchCallbackMessage(std::string &key, bool &state, naoqi_bridge_msgs::HeadTouch &msg)
00234 {
00235   int i = 0;
00236   for(std::vector<std::string>::const_iterator it = keys_.begin(); it != keys_.end(); ++it, ++i)
00237   {
00238     if ( key == it->c_str() ) {
00239       msg.button = i;
00240       msg.state = state?(naoqi_bridge_msgs::HeadTouch::statePressed):(naoqi_bridge_msgs::HeadTouch::stateReleased);
00241     }
00242   }
00243 }
00244 
00245 // http://stackoverflow.com/questions/8752837/undefined-reference-to-template-class-constructor
00246 template class TouchEventRegister<naoqi_bridge_msgs::Bumper>;
00247 template class TouchEventRegister<naoqi_bridge_msgs::HandTouch>;
00248 template class TouchEventRegister<naoqi_bridge_msgs::HeadTouch>;
00249 
00250 }//namespace


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