fx8_driver_nodelet.h

Go to the documentation of this file.

/*****************************************************************************
 *
 *  Copyright (C) 2013, NIPPON CONTROL SYSTEM Corporation
 *  All rights reserved.
 *  
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *  
 *    * Redistributions of source code must retain the above copyright
 *      notice, this list of conditions and the following disclaimer.
 *   
 *    * Redistributions in binary form must reproduce the above
 *      copyright notice, this list of conditions and the following
 *      disclaimer in the documentation and/or other materials provided
 *      with the distribution.
 *   
 *    * Neither the name of the NIPPON CONTROL SYSTEM Corporation nor
 *      the names of its contributors may be used to endorse or promote
 *      products derived from this software without specific prior
 *      written permission.
 *  
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *****************************************************************************/

#ifndef _FX8_DRIVER_H_
#define _FX8_DRIVER_H_

#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <dynamic_reconfigure/server.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>

#include <boost/thread.hpp>

#include <fx8.h>
#include <infinisoleil/FX8Config.h>

namespace infinisoleil
{
class FX8DriverNodelet : public nodelet::Nodelet
{
  typedef fx8_handle FX8Handle; 
  typedef fx8_measure_mode FX8MeasureMode; 
  typedef fx8_bool FX8Bool; 
  typedef fx8_sensor_info FX8SensorInfo; 
  typedef fx8_xy_data_element FX8XyDataElement; 
  typedef fx8_xy_data_surface FX8XyDataSurface; 
  typedef fx8_xy_data FX8XyData; 
  typedef dynamic_reconfigure::Server<infinisoleil::FX8Config> ReconfigureServer; 
  typedef std::vector<unsigned char> ScanDataPackets; 
  typedef std::pair<ros::Time, std::vector<unsigned char> > ReceivedErrorCodePackets; 
  typedef std::pair<ros::Time, std::string> ErrorInfo; 
  typedef diagnostic_updater::HeaderlessTopicDiagnostic TopicDiagnostic; 
  typedef boost::shared_ptr<TopicDiagnostic> TopicDiagnosticPtr; 

  struct FX8Scan
  {
    FX8XyData xy_data;    
    ScanDataPackets scan_data; 
    std::string range_frame_id; 
    std::string ir_frame_id; 
    std::string point_cloud_frame_id; 
  };

  struct FX8Info
  {
    FX8SensorInfo sensor_info; 
    int connect_timeout; 
    int send_timeout; 
    int receive_timeout; 
  };

  typedef FX8Info Config; 
  typedef FX8Scan Scan; 

public:
  FX8DriverNodelet();
  
  virtual ~FX8DriverNodelet();

private:
  virtual void onInit();

  void initializeNodelet();

  void shutdownNodelet();

  void initializeReconfigureServer(ros::NodeHandle param_nh);

  void shutdownReconfigureServer();

  void setupDiagnostics();

  void driverThreadFunc();

  bool initializeDevice();

  void shutdownDevice();

  bool setDeviceMeasureMode(const FX8Handle device, const FX8MeasureMode measure_mode,
  FX8SensorInfo* sensor_info, FX8XyData* xy_data);

  bool getDeviceSensorInfo(const FX8Handle device, FX8SensorInfo* sensor_info, FX8XyData* xy_data);

  void outputDeviceParameters();

  bool startScan();

  void spin();

  void publishScanData(const unsigned char* scan_data, size_t size);

  sensor_msgs::ImagePtr createRangeImageMessage();

  sensor_msgs::ImagePtr createIRImageMessage();

  sensor_msgs::PointCloud2Ptr createPointCloudMessage();
 
  void setMessageData(
    sensor_msgs::ImagePtr range_msg, sensor_msgs::ImagePtr ir_msg, sensor_msgs::PointCloud2Ptr point_cloud_msg,
    unsigned char surface_number);

  inline void extractRangeAndIntensityFromScanData(int index, const unsigned char* scan_data,
    unsigned short* range, unsigned short* intensity);

  void fillDiagnosticStatusByErrorInfo(diagnostic_updater::DiagnosticStatusWrapper &status);

  void fillDiagnosticStatusByReceivedErrorCode(diagnostic_updater::DiagnosticStatusWrapper &status);

  void addDiagnostics(const unsigned char* error_data, size_t size);

  void reconfigureFX8Callback(FX8Config config, uint32_t level);

  inline void updateTime();

  static void receiveScanDataCallback(const unsigned char* scan_data, size_t size, void* user_data);

  static void receiveErrorCodeCallback(const unsigned char* error_data, size_t size, void* user_data);

  // Device manager
  FX8Handle device_; 
  Scan scan_; 
  Config config_; 
  std::vector<ReceivedErrorCodePackets> error_code_; 
  std::vector<ErrorInfo> error_info_; 

  boost::thread driver_thread_; 
  bool device_running_; 

  // Publisher
  ros::Publisher range_publisher_; 
  ros::Publisher ir_publisher_; 
  ros::Publisher point_cloud_publisher_; 
  boost::mutex mutex_scan_; 
  ros::Time latest_update_time_; 

  // Diagnostic
  boost::shared_ptr<diagnostic_updater::Updater> diagnostic_updater_; 
  TopicDiagnosticPtr range_image_diagnostic_frequency_; 
  TopicDiagnosticPtr ir_image_diagnostic_frequency_; 
  TopicDiagnosticPtr point_cloud_diagnostic_frequency_; 
  boost::mutex mutex_diagnostics_; 
  bool diagnostics_enable_; 
  double target_frequency_; 

  // Reconfig
  boost::shared_ptr<ReconfigureServer> reconfigure_server_; 
};
} // namespace infinisoleil

#endif // _FX8_DRIVER_H_