camera_commander.h

Go to the documentation of this file.

// Copyright 2020 Continental AG
// All rights reserved.
//
// Software License Agreement (BSD 2-Clause Simplified License)
//
// 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.
//
// 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 HOLDER 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 CAMERA_COMMANDER__CAMERA_COMMANDER_H_
#define CAMERA_COMMANDER__CAMERA_COMMANDER_H_

#include <hfl_driver/HFLConfig.h>
#include <hfl_interface.h>

#include <dynamic_reconfigure/server.h>
#include <nodelet/nodelet.h>

#include <vector>
#include <string>
#include <memory>

#include "udp_com/UdpPacket.h"
#include "udp_com/UdpSend.h"
#include "udp_com/UdpSocket.h"

#include "ros/ros.h"

namespace hfl
{
enum commander_states
{
  state_probe = 0,
  state_init,
  state_done,
  state_error
};

enum error_codes
{
  no_error = 0,
  frame_socket_error,
  pdm_socket_error,
  object_socket_error,
  tele_socket_error,
  slice_socket_error
};

class CameraCommander : public nodelet::Nodelet
{
public:
  CameraCommander();

  ~CameraCommander();

  void onInit();

  bool udpInit();

  bool createSocket(std::string computerAddr, std::string cameraAddr,
      uint16_t port, bool isMulticast);

private:
  ros::NodeHandle node_handler_;

  std::string namespace_;

  ros::Subscriber frame_data_subscriber_;

  ros::Subscriber pdm_data_subscriber_;

  ros::Subscriber object_data_subscriber_;

  ros::Subscriber tele_data_subscriber_;
  
  ros::Subscriber slice_data_subscriber_;
  
  ros::ServiceClient udp_send_service_client_;

  ros::ServiceClient udp_socket_creation_service_client_;

  std::shared_ptr<dynamic_reconfigure::Server<hfl_driver::HFLConfig> > dynamic_parameters_server_;

  ros::Timer timer_;

  commander_states current_state_;

  commander_states previous_state_;

  error_codes error_status_;

  std::string ethernet_interface_;

  std::string camera_address_;

  std::string computer_address_;

  int frame_data_port_;
  
  int pdm_data_port_;

  int object_data_port_;

  int tele_data_port_;
  
  int slice_data_port_;
  
  std::shared_ptr<hfl::HflInterface> flash_;

  void setCommanderState(const ros::TimerEvent& timer_event);

  bool setFlash();

  void frameDataCallback(const udp_com::UdpPacket& udp_packet);

  void pdmDataCallback(const udp_com::UdpPacket& udp_packet);
  
  void objectDataCallback(const udp_com::UdpPacket& udp_packet);

  void teleDataCallback(const udp_com::UdpPacket& udp_packet);
  
  void sliceDataCallback(const udp_com::UdpPacket& udp_packet);
  
  bool sendCommand(const std::vector<uint8_t>& data);

  void dynamicPametersCallback(hfl_driver::HFLConfig& config, uint32_t level);

  error_codes checkForError();


  bool fixError(error_codes error);
};

}  // namespace hfl

#endif  // CAMERA_COMMANDER__CAMERA_COMMANDER_H_