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