Merge pull request #62 from suryaambrose/master

Fix termination issues

include/naoqi_driver/naoqi_driver.hpp

@@ -69,7 +69,7 @@ class Driver
   * @brief Constructor for naoqi driver
   * @param session[in] session pointer for naoqi2 service registration
   */
-  Driver( qi::SessionPtr& session, const std::string& prefix );
+  Driver( qi::SessionPtr session, const std::string& prefix );
 
   /**
   * @brief Destructor for naoqi driver,
@@ -148,7 +148,7 @@ class Driver
 
   std::string _whoIsYourDaddy()
   {
-    return "the coolest German in Paris";
+    return "A sugar bear";
   }
 
   /**

src/event/audio.cpp

@@ -86,7 +86,7 @@ void AudioEventRegister::resetRecorder( boost::shared_ptr<naoqi::recorder::Globa
 
 void AudioEventRegister::startProcess()
 {
-  boost::mutex::scoped_lock start_lock(mutex_);
+  boost::mutex::scoped_lock start_lock(subscription_mutex_);
   if (!isStarted_)
   {
     if(!serviceId)
@@ -108,7 +108,7 @@ void AudioEventRegister::startProcess()
 
 void AudioEventRegister::stopProcess()
 {
-  boost::mutex::scoped_lock stop_lock(mutex_);
+  boost::mutex::scoped_lock stop_lock(subscription_mutex_);
   if (isStarted_)
   {
     if(serviceId){
@@ -136,19 +136,19 @@ void AudioEventRegister::setBufferDuration(float duration)
 
 void AudioEventRegister::isRecording(bool state)
 {
-  boost::mutex::scoped_lock rec_lock(mutex_);
+  boost::mutex::scoped_lock rec_lock(processing_mutex_);
   isRecording_ = state;
 }
 
 void AudioEventRegister::isPublishing(bool state)
 {
-  boost::mutex::scoped_lock pub_lock(mutex_);
+  boost::mutex::scoped_lock pub_lock(processing_mutex_);
   isPublishing_ = state;
 }
 
 void AudioEventRegister::isDumping(bool state)
 {
-  boost::mutex::scoped_lock dump_lock(mutex_);
+  boost::mutex::scoped_lock dump_lock(processing_mutex_);
   isDumping_ = state;
 }
 
@@ -174,7 +174,7 @@ void AudioEventRegister::processRemote(int nbOfChannels, int samplesByChannel, q
   msg.data = std::vector<int16_t>(remoteBuffer, remoteBuffer+bufferSize);
 
   std::vector<message_actions::MessageAction> actions;
-  boost::mutex::scoped_lock callback_lock(mutex_);
+  boost::mutex::scoped_lock callback_lock(processing_mutex_);
   if (isStarted_) {
     // CHECK FOR PUBLISH
     if ( isPublishing_ && publisher_->isSubscribed() )

src/event/audio.hpp

@@ -94,7 +94,8 @@ class AudioEventRegister: public boost::enable_shared_from_this<AudioEventRegist
   std::vector<uint8_t> channelMap;
   unsigned int serviceId;
 
-  boost::mutex mutex_;
+  boost::mutex subscription_mutex_;
+  boost::mutex processing_mutex_;
 
   bool isStarted_;
   bool isPublishing_;

src/event/touch.cpp

@@ -111,7 +111,9 @@ void TouchEventRegister<T>::stopProcess()
     //std::string serviceName = std::string("ROS-Driver-") + typeid(T).name();
     std::string serviceName = std::string("ROS-Driver-") + keys_[0];
     if(serviceId){
-      p_memory_.call<void>("unsubscribeToEvent", serviceName, "touchCallback");
+      for(std::vector<std::string>::const_iterator it = keys_.begin(); it != keys_.end(); ++it) {
+        p_memory_.call<void>("unsubscribeToEvent",it->c_str(), serviceName);
+      }
       session_->unregisterService(serviceId);
       serviceId = 0;
     }

src/external_registration.cpp

@@ -27,9 +27,6 @@
 
 int main(int argc, char** argv)
 {
-  /* adjust the SDK prefix in case you compiled via catkin*/
-  naoqi::naoqi_env::adjustSDKPrefix();
-
   /* launch naoqi service */
   qi::ApplicationSession app(argc, argv);
   /* In case you launch via roslaunch/rosrun we remove the ros args */
@@ -69,11 +66,10 @@ int main(int argc, char** argv)
 
   // everything's correctly parsed - let's start the app!
   app.start();
-  boost::shared_ptr<naoqi::Driver> bs = qi::import("naoqi_driver_module").call<qi::Object<naoqi::Driver> >("ROS-Driver", app.session(), vm["namespace"].as<std::string>()).asSharedPtr();
+  boost::shared_ptr<naoqi::Driver> bs = boost::make_shared<naoqi::Driver>(app.session(), vm["namespace"].as<std::string>());
 
   app.session()->registerService("ROS-Driver", bs);
 
-
   // set ros paramters directly upfront if available
   if ( vm.count("roscore_ip") )
   {
@@ -91,10 +87,8 @@ int main(int argc, char** argv)
     bs->init();
   }
 
-  //! @note Must call ow._stopService when the application stops to do the clean-up
-  app.atStop(boost::function<void()>(
-                 boost::bind(&naoqi::Driver::stopService, boost::ref(bs))));
-
   app.run();
+  bs->stopService();
   app.session()->close();
+  return 0;
 }

src/helpers/driver_helpers.cpp

@@ -29,14 +29,14 @@ namespace driver
 static naoqi_bridge_msgs::RobotInfo& getRobotInfoLocal( const qi::SessionPtr& session)
 {
   static naoqi_bridge_msgs::RobotInfo info;
-  static qi::SessionPtr session_old;
+  static qi::Url robot_url;
 
-  if (session_old == session)
+  if (robot_url == session->url())
   {
     return info;
   }
 
-  session_old = session;
+  robot_url = session->url();
 
   // Get the robot type
   std::cout << "Receiving information about robot model" << std::endl;

src/naoqi_driver.cpp

@@ -112,7 +112,7 @@
 namespace naoqi
 {
 
-Driver::Driver( qi::SessionPtr& session, const std::string& prefix )
+Driver::Driver( qi::SessionPtr session, const std::string& prefix )
   : sessionPtr_( session ),
   robot_( helpers::driver::getRobot(session) ),
   freq_(15),
@@ -588,12 +588,11 @@ void Driver::registerDefaultConverter()
    * topic, he/she will receive the information published before, as the publisher is latched.
    */
   /** Info publisher **/
-  boost::shared_ptr<converter::InfoConverter> inc = boost::make_shared<converter::InfoConverter>( "info", 0, sessionPtr_ );
-
   if ( info_enabled )
   {
     boost::shared_ptr<publisher::InfoPublisher> inp = boost::make_shared<publisher::InfoPublisher>( "info" , robot_);
     boost::shared_ptr<recorder::BasicRecorder<naoqi_bridge_msgs::StringStamped> > inr = boost::make_shared<recorder::BasicRecorder<naoqi_bridge_msgs::StringStamped> >( "info" );
+    boost::shared_ptr<converter::InfoConverter> inc = boost::make_shared<converter::InfoConverter>( "info", 0, sessionPtr_ );
     inc->registerCallback( message_actions::PUBLISH, boost::bind(&publisher::InfoPublisher::publish, inp, _1) );
     inc->registerCallback( message_actions::RECORD, boost::bind(&recorder::BasicRecorder<naoqi_bridge_msgs::StringStamped>::write, inr, _1) );
     inc->registerCallback( message_actions::LOG, boost::bind(&recorder::BasicRecorder<naoqi_bridge_msgs::StringStamped>::bufferize, inr, _1) );