naoqi_driver.hpp

Go to the documentation of this file.

/*
 * Copyright 2015 Aldebaran
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
 
 
#ifndef NAOQI_DRIVER_HPP
#define NAOQI_DRIVER_HPP
 
#include <vector>
#include <queue>
 
/*
* BOOST
*/
#include <boost/property_tree/ptree.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/scoped_ptr.hpp>
 
/*
* ALDEB
*/
#include <qi/session.hpp>
 
/*
* PUBLIC INTERFACE
*/
#include <naoqi_driver/converter/converter.hpp>
#include <naoqi_driver/publisher/publisher.hpp>
#include <naoqi_driver/subscriber/subscriber.hpp>
#include <naoqi_driver/service/service.hpp>
#include <naoqi_driver/recorder/recorder.hpp>
#include <naoqi_driver/event/event.hpp>
#include <naoqi_driver/recorder/globalrecorder.hpp>
 
namespace tf2_ros
{
  class Buffer;
}
 
namespace naoqi
{
 
namespace recorder
{
  class GlobalRecorder;
}
class Driver
{
public:
  Driver( qi::SessionPtr session, const std::string& prefix );
 
  ~Driver();
 
  void init();
 
  void startRosLoop();
  void stopRosLoop();
  std::string minidump(const std::string& prefix);
  std::string minidumpConverters(const std::string& prefix, const std::vector<std::string>& names);
 
  void setBufferDuration(float duration);
  float getBufferDuration();
 
  void registerConverter( converter::Converter& conv );
 
  void registerPublisher( const std::string& conv_name, publisher::Publisher& pub);
 
  void registerRecorder(const std::string& conv_name, recorder::Recorder& rec, float frequency);
 
  void registerConverter(converter::Converter conv, publisher::Publisher pub, recorder::Recorder rec );
 
  void registerPublisher(converter::Converter conv, publisher::Publisher pub );
 
  void registerRecorder(converter::Converter conv, recorder::Recorder rec );
 
  bool registerMemoryConverter(const std::string& key, float frequency, const dataType::DataType& type );
 
  bool registerEventConverter(const std::string& key, const dataType::DataType& type);
 
 
  std::vector<std::string> getAvailableConverters();
 
  std::vector<std::string> getSubscribedPublishers() const;
 
  std::string _whoIsYourDaddy()
  {
    return "A sugar bear";
  }
 
  void registerSubscriber( subscriber::Subscriber sub );
 
  void registerService( service::Service srv );
  std::string getMasterURI() const;
 
  void setMasterURINet( const std::string& uri, const std::string& network_interface );
 
  void setMasterURI( const std::string& uri );
 
  void startPublishing();
 
  void stopPublishing();
 
  void startRecording();
 
  void startRecordingConverters(const std::vector<std::string>& names);
 
  std::string stopRecording();
 
  void startLogging();
 
  void stopLogging();
 
  void addMemoryConverters(std::string filepath);
 
  void parseJsonFile(std::string filepath, boost::property_tree::ptree& pt);
 
  void stopService();
 
  std::vector<std::string> getFilesList();
 
  void removeAllFiles();
 
  void removeFiles(std::vector<std::string> files);
 
private:
  qi::SessionPtr sessionPtr_;
 
  const robot::Robot& robot_;
 
  bool publish_enabled_;
  bool record_enabled_;
  bool log_enabled_;
  bool keep_looping;
  bool has_stereo;
 
  const size_t freq_;
  boost::thread publisherThread_;
  //ros::Rate r_;
 
  boost::shared_ptr<recorder::GlobalRecorder> recorder_;
 
  /* boot config */
  boost::property_tree::ptree boot_config_;
  void loadBootConfig();
 
  void registerDefaultConverter();
  void registerDefaultSubscriber();
  void registerDefaultServices();
  void insertEventConverter(const std::string& key, event::Event event);
 
  template <typename T1, typename T2, typename T3>
  void _registerMemoryConverter( const std::string& key, float frequency ) {
    boost::shared_ptr<T1> mfp = boost::make_shared<T1>( key );
    boost::shared_ptr<T2> mfr = boost::make_shared<T2>( key );
    boost::shared_ptr<T3> mfc = boost::make_shared<T3>( key , frequency, sessionPtr_, key );
    mfc->registerCallback( message_actions::PUBLISH, boost::bind(&T1::publish, mfp, _1) );
    mfc->registerCallback( message_actions::RECORD, boost::bind(&T2::write, mfr, _1) );
    mfc->registerCallback( message_actions::LOG, boost::bind(&T2::bufferize, mfr, _1) );
    registerConverter( mfc, mfp, mfr );
  }
 
  void rosLoop();
 
  boost::scoped_ptr<ros::NodeHandle> nhPtr_;
  boost::mutex mutex_reinit_;
  boost::mutex mutex_conv_queue_;
  boost::mutex mutex_record_;
 
  std::vector< converter::Converter > converters_;
  std::map< std::string, publisher::Publisher > pub_map_;
  std::map< std::string, recorder::Recorder > rec_map_;
  std::map< std::string, event::Event > event_map_;
  typedef std::map< std::string, publisher::Publisher>::const_iterator PubConstIter;
  typedef std::map< std::string, publisher::Publisher>::iterator PubIter;
  typedef std::map< std::string, recorder::Recorder>::const_iterator RecConstIter;
  typedef std::map< std::string, recorder::Recorder>::iterator RecIter;
  typedef std::map< std::string, event::Event>::const_iterator EventConstIter;
  typedef std::map< std::string, event::Event>::iterator EventIter;
 
  std::vector< subscriber::Subscriber > subscribers_;
  std::vector< service::Service > services_;
 
  float buffer_duration_;
 
  struct ScheduledConverter {
    ScheduledConverter(const ros::Time& schedule, size_t conv_index) :
       schedule_(schedule), conv_index_(conv_index)
    {
    }
 
    bool operator < (const ScheduledConverter& sp_in) const {
      return schedule_ > sp_in.schedule_;
    }
    ros::Time schedule_;
    size_t conv_index_;
  };
 
  std::priority_queue<ScheduledConverter> conv_queue_;
 
  boost::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
};
 
} // naoqi
 
#endif