#include <gazebo/gazebo.hh>
#include <gazebo/common/common.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/math/gzmath.hh>
#include "boost_server_framework.hh"
#include "break_usar_command_into_params.hh"
#include "get_topics_list.hh"
#include "flipper_control_msgs.hh"
#include "encoder_msgs.hh"

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <pthread.h>

#define _DEBUG_MSG_
#ifdef _DEBUG_MSG_
#define SCK_TXT(S,T) write(S, T, strlen(T))
#endif

#define strNcmp(B,C) strncasecmp(B,C,strlen(C))

#ifndef numof
#define numof(A)  (sizeof(A)/sizeof(A[0]))
#endif

#ifndef _MAX
#define _MAX(X,Y) (((X)>(Y))?(X):(Y))
#endif

#ifndef _MIN
#define _MIN(X,Y) (((X)<(Y))?(X):(Y))
#endif

#define MAX_StartPoses 10
#define MAX_StartPose_Length 1024

char Plugin_Option_Name_StartPose[][30]
              ={"StartPose_1", "StartPose_2", "StartPose_3"
               ,"StartPose_4", "StartPose_5", "StartPose_6"
               ,"StartPose_7", "StartPose_8", "StartPose_9"};

#define Plugin_Option_Name_GroundTruth "GroundTruth"

const char* get_string_between_slash_from_topic_name(char* dist, 
       int dist_size, int slash_num_from_right, const char* topic_name)
{
  const char* i;
  const char* start;
  const char* end;
  int         copy_size;
  if(1 > slash_num_from_right)
    return (const char*)NULL;
  i = topic_name;
  i+= strlen(topic_name);
  end = i;
  start = (const char*)NULL;
  for(i--; topic_name <= i; i--)
  {
    if('/' == *i)
    {
      start = (i + 1);
      slash_num_from_right--;
      if(0 < slash_num_from_right)
      {
        end = i;
        start = (const char*)NULL;
        continue;
      }
      break;
    }
  }
  if((const char*)NULL == start)
    return (const char*)NULL;
  dist_size--; 
  copy_size = (int)(end - start);
  strncpy(dist, start, (copy_size > dist_size)?dist_size:copy_size);
  dist[copy_size] = 0;
  return (const char*)dist;
}

const char* get_type_from_topic_name(char* dist, int dist_size, 
                                              const char* topic_name)
{
  return get_string_between_slash_from_topic_name(dist, dist_size, 1
                                                          , topic_name);
}

const char* get_name_from_topic_name(char* dist, int dist_size, 
                                              const char* topic_name)
{
  return get_string_between_slash_from_topic_name(dist, dist_size, 2
                                                          , topic_name);
}

struct Plugin_Option_Parameters
{
  int Num_Of_StartPoses;
  char StartPoses[MAX_StartPoses][MAX_StartPose_Length];
  char GroundTruth[MAX_StartPose_Length];
  int  GroundTruthIsOn;

  Plugin_Option_Parameters(void) : Num_Of_StartPoses(0) { }

  void Read_Option_Parameters(sdf::ElementPtr _sdf)
  {
    Read_Start_Pose_Parameters(_sdf);
    Read_GroundTruth_Parameter(_sdf);
  }

  void Read_Start_Pose_Parameters(sdf::ElementPtr _sdf)
  {
    int i;
    for(i = 0; i < numof(Plugin_Option_Name_StartPose); i++)
    {
      if(_sdf->HasElement(Plugin_Option_Name_StartPose[i]))
      {
        std::stringstream s;
        s << _sdf->GetElement(Plugin_Option_Name_StartPose[i])
                    ->Get<std::string>();
        strncat(StartPoses[Num_Of_StartPoses], s.str().c_str()
                 , MAX_StartPose_Length);
        Num_Of_StartPoses++;
      }
    }
  }

  void Read_GroundTruth_Parameter(sdf::ElementPtr _sdf)
  {
    int i;
    GroundTruthIsOn = 0;
    if(_sdf->HasElement(Plugin_Option_Name_GroundTruth))
    {
      std::stringstream s;
      s << _sdf->GetElement(Plugin_Option_Name_GroundTruth)
                  ->Get<std::string>();
      strncat(GroundTruth, s.str().c_str(), MAX_StartPose_Length);
      if(NULL != strcasestr(GroundTruth, "true")
      || NULL != strcasestr(GroundTruth, "1"))
        GroundTruthIsOn = 1;
    }
  }

  int Search_StartPose(char* target_name, float& _x, float& _y, float& _z,
                       float& _roll, float& _pitch, float& _yaw)
  {
    int   i, read_fields;
    char  name[100];
    float x, y, z, roll, pitch, yaw;
    for(i = 0; i < Num_Of_StartPoses; i++)
    {
      printf("%d %s\n", i, StartPoses[i]); 
      read_fields = sscanf(StartPoses[i], "%s %f,%f,%f %f,%f,%f"
                            , name, &x, &y, &z, &roll, &pitch, &yaw);
      if(7 != read_fields)
        return 0; 
      if(0 == strNcmp(target_name, name))
      {
        _x     = x;
        _y     = y;
        _z     = z;
        _roll  = roll;
        _pitch = pitch;
        _yaw   = yaw;
        return 1; 
      }
    }
    return 0; 
  }
};

Plugin_Option_Parameters POP;

struct RD
{
  char  Request;
  char  Done;
  void  Init(void) { }
  RD(void) : Request(0), Done(0) { Init(); }
};

enum ROBOT_TYPE
{
  RT_GROUNDVECHILE = 1,
  RT_LEGGEDROBOT   = 2,
  RT_NAUTICVEHICLE = 3,
  RT_AERIALVEHICLE = 4
};

#define RT_D_GROUNDVEHICLE "GroundVehicle"
#define RT_D_LEGGEDROBOT   "LeggedRobot"
#define RT_D_NAUTICVEHICLE "NauticVehicle"
#define RT_D_AERIALVEHICLE "AerialVehicle"
#define RT_D_UNKNOWN       "Unknown"

enum DRIVE_TYPE
{
  DT_DiffarentialDrive = 1,
  DT_SkidsteerDrive    = 2,
  DT_QuadRotor         = 3,
  DT_FlipperArm        = 4
};

struct ROBOT_DATABASE
{
  const char* Model_Name;
  const char* Type;
  const char* SteeringType;
  const float Mass;
  const float MaxSpeed;
  const float MaxTorque;
  const float MaxFrontSteer;
  const float MaxRearSteer;
  int         Battery;
  int         DriveType;
} Robot_DB[]
 = {{"pioneer3at_with_sensors", RT_D_GROUNDVEHICLE, "SkidSteered",

       20,    5,    10,   10,  10,   3600, DT_SkidsteerDrive}

   ,{"pioneer3at_with_sensors_r", RT_D_GROUNDVEHICLE, "SkidSteered",

       20,    5,    10,   10,  10,   3600, DT_SkidsteerDrive}

   ,{"pioneer3at_with_sensors_b", RT_D_GROUNDVEHICLE, "SkidSteered",

       20,    5,    10,   10,  10,   3600, DT_SkidsteerDrive}

   ,{"pioneer3at_with_sensors_g", RT_D_GROUNDVEHICLE, "SkidSteered",

       20,    5,    10,   10,  10,   3600, DT_SkidsteerDrive}

   ,{"pioneer3at_with_sensors_y", RT_D_GROUNDVEHICLE, "SkidSteered",

       20,    5,    10,   10,  10,   3600, DT_SkidsteerDrive}

   ,{"Crawler_Robot", RT_D_GROUNDVEHICLE, "SkidSteered",

       20,    5,    10,   10,  10,   3600, DT_SkidsteerDrive}

   ,{"pioneer3at", RT_D_GROUNDVEHICLE, "SkidSteered",

       20,    5,    10,   10,  10,   3600, DT_SkidsteerDrive}

   ,{"pioneer2dx_with_sensors", RT_D_GROUNDVEHICLE, "SkidSteered",

       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"pioneer2dx_with_sensors_r", RT_D_GROUNDVEHICLE, "SkidSteered",

       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"pioneer2dx_with_sensors_g", RT_D_GROUNDVEHICLE, "SkidSteered",

       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"pioneer2dx_with_sensors_b", RT_D_GROUNDVEHICLE, "SkidSteered",

       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"pioneer2dx_with_sensors_y", RT_D_GROUNDVEHICLE, "SkidSteered",

       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"turtlebot_with_sensors_r", RT_D_GROUNDVEHICLE, "SkidSteered",

       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"turtlebot_with_sensors_g", RT_D_GROUNDVEHICLE, "SkidSteered",

       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"turtlebot_with_sensors_b", RT_D_GROUNDVEHICLE, "SkidSteered",

       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"turtlebot_with_sensors_y", RT_D_GROUNDVEHICLE, "SkidSteered",

       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"quadrotor", RT_D_AERIALVEHICLE, "RotaryWing",

       20,    5,    10,   10,  10,   3600, DT_QuadRotor}

   ,{"noname", RT_D_UNKNOWN, "Unknown",

        0,    0,     0,    0,   0,      0, 0} };

int get_number_of_Robot_DB(const char* robot_model_name)
{
  int i;
  for(i = 0; (numof(Robot_DB) - 1) > i; i++)
    if(0 == strNcmp(robot_model_name, Robot_DB[i].Model_Name))
      break;
  return i;
}

#define get_type_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)].Type
#define get_steeringtype_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)]\
           .SteeringType
#define get_mass_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)]\
           .Mass
#define get_maxspeed_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)]\
           .MaxSpeed
#define get_maxtorque_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)]\
           .MaxTorque
#define get_maxfrontsteer_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)]\
           .MaxFrontSteer
#define get_maxrearsteer_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)]\
           .MaxRearSteer
#define get_battery_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)].Battery
#define get_drive_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)].DriveType

#define ET_D_G_RFID         "Unknown"
#define ET_D_U_RFID         "RFID"
#define ET_D_G_GPS          "gps"
#define ET_D_U_GPS          "GPS"
#define ET_D_G_IMU          "imu"
#define ET_D_U_IMU          "INS"
#define ET_D_G_CAMERA       "image"
#define ET_D_U_CAMERA       "Camera"
#define ET_D_G_LASERSCANNER "scan"
#define ET_D_U_LASERSCANNER "RangeScanner"
#define ET_D_G_ODOMETRY     "Odometry"
#define ET_D_U_ODOMETRY     "Odometry"
#define ET_D_G_GROUNDTRUTH  "GndTruth"
#define ET_D_U_GROUNDTRUTH  "GroundTruth"
#define ET_D_G_ENCODER      "Encoders"
#define ET_D_U_ENCODER      "Encoder"

#define ET_D_N_LASERSCANNER "lms200"

struct Gazebo_USARSim_DATABASE
{
  const char* Gazebo_name;
  const char* USARSim_name;
} Gazebo_USARSim_DB[]
 = {{ET_D_G_LASERSCANNER, ET_D_U_LASERSCANNER},
    {ET_D_G_CAMERA,       ET_D_U_CAMERA},
    {ET_D_G_RFID,         ET_D_U_RFID},
    {ET_D_G_GPS,          ET_D_U_GPS},
    {ET_D_G_IMU,          ET_D_U_IMU},
    {ET_D_G_ODOMETRY,     ET_D_U_ODOMETRY},
    {ET_D_G_GROUNDTRUTH,  ET_D_U_GROUNDTRUTH},
    {ET_D_G_ENCODER,      ET_D_U_ENCODER},
    {"Unknown",           "UnKnown"} };

int get_number_of_DB_From_Gazebo_name(const char* _name)
{
  int i;
  for(i = 0; (numof(Gazebo_USARSim_DB) - 1) > i; i++)
    if(0 == strNcmp(_name, Gazebo_USARSim_DB[i].Gazebo_name))
      break;
  return i;
}

int get_number_of_DB_From_USARSim_name(const char* _name)
{
  int i;
  for(i = 0; (numof(Gazebo_USARSim_DB) - 1) > i; i++)
    if(0 == strNcmp(_name, Gazebo_USARSim_DB[i].USARSim_name))
      break;
  return i;
}

#define get_USARSim_name_of(X) \
      Gazebo_USARSim_DB[get_number_of_DB_From_Gazebo_name(X)].USARSim_name
#define get_Gazebo_name_of(X) \
      Gazebo_USARSim_DB[get_number_of_DB_From_USARSim_name(X)].Gazebo_name

#define INIT_FLP (M_PI/4)

namespace gazebo
{

struct USARcommand
{

  Server_Framework<USARcommand>&_parent;
  boost::asio::io_service       _ioservice;
  boost::asio::ip::tcp::socket  _socket;
  boost::asio::streambuf        _buffer;
  boost::thread                 _thread;
  gazebo::transport::NodePtr    _node;

  float                         _left_power, _right_power;

  double                        flp_fr, flp_fl, flp_rr, flp_rl;

  int                           robot_was_spawned;
  char                          model_name[100], 
                                own_name[100], 
                                topic_root[100];
  char*                         ucbuf; 
  gazebo::math::Vector3         spawn_location;
  gazebo::math::Quaternion      spawn_direction;
  TopicsList                    current_topics_list,registered_topics_list;
  int                           full_battery, remain_battery;
  gazebo::transport::SubscriberPtr* _sub;
  std::vector<event::ConnectionPtr> connections;
  int                           STA_Disp_Counter, IMU_Disp_Counter;
  int                           STA_Last_sec, STA_Count_Per_1sec; 
  int                           ENCODER_Disp_counter; 

  void Init(void)
  {
    model_name[0] = own_name[0] = topic_root[0] = 0;
  }

  USARcommand(Server_Framework<USARcommand>&parent) : 
    _parent(parent), _socket(_ioservice), ucbuf(NULL), robot_was_spawned(0)
      , current_topics_list(), registered_topics_list()
      , STA_Disp_Counter(0), IMU_Disp_Counter(0), ENCODER_Disp_counter(0)
      , STA_Last_sec(0), STA_Count_Per_1sec(0)
      , _left_power(0), _right_power(0)
      , flp_fr(INIT_FLP), flp_fl(INIT_FLP)
      , flp_rr(INIT_FLP), flp_rl(INIT_FLP)

  { Init(); }

  ~USARcommand() 
  {
    connections.clear();
  }

  float set_current_time_in_form(char* buf)
  {
    float  Current_time = _parent._world->GetRealTime().Double();
    sprintf(buf, "%.4f", Current_time);
    return Current_time;
  }

  void Process_STA(void)
  {
    boost::asio::streambuf response;
    std::ostream           os_res(&response);
    char       Current_time_in_form[50];
    float      Current_time=set_current_time_in_form(Current_time_in_form);
    int        Current_sec = (int)Current_time;
    if(0 == robot_was_spawned)
      return;
    if(STA_Last_sec != Current_sec)
    {
      STA_Count_Per_1sec = 0;
      if(0 < remain_battery)
        remain_battery--;
    }
    if(0 > STA_Disp_Counter)
    {
      STA_Disp_Counter = 100; 
      os_res << "STA " << 
        "{Time " << Current_time_in_form << "}" << 
        "{Battery " << remain_battery << "}" << 
        "{Type " << get_type_of_robot(model_name) << "}" << 
        "{LightToggle False}" << 
        "{LightIntensity 100}" << 
        "{FrontSteer 0}" << 
        "{RearSteer 0}" << 
        "{CountPer1sec " << STA_Count_Per_1sec << "}" << 
        "\r\n";
    }
    else
      STA_Disp_Counter--;
    STA_Count_Per_1sec++;
    STA_Last_sec = Current_sec;
    boost::asio::write(_socket, response);
  }

  void Process_laser_scanner_callback(ConstLaserScanStampedPtr& _msg);
  void Process_gps_callback(ConstGPSPtr& _msg);
  void Process_imu_callback(ConstIMUPtr& _msg);
  void Process_encoder_callback(ConstEncoderPtr& _msg);
  void Send_odometry(ConstIMUPtr& _msg);

  void Send_STA_and_Search_Sensors_and_Register_Callbacks(void)
  {
    static int             refresh_cycle = 0;
    if(1 != robot_was_spawned)
      return;

    Process_STA();

    if(NULL == current_topics_list.Search(own_name) 
      ||  0 == current_topics_list.Size()
      || 700 < refresh_cycle++)
    {
      refresh_cycle = 0;
      current_topics_list.Refresh_Topics_List();
      current_topics_list.Filter(own_name);
      if(0 == current_topics_list.Size())
        return;

      for(typename std::set<char*>::iterator i 
                                 = current_topics_list._topics_list.begin()
            ; i != current_topics_list._topics_list.end(); i++)
      {

        if(NULL != strcasestr(*i, ET_D_G_LASERSCANNER) 
          && NULL != strcasestr(*i, ET_D_N_LASERSCANNER))
        {
          if(NULL == registered_topics_list.Search(*i))
          {
std::cout << "Laser cb registered" << std::endl;
            registered_topics_list.Add_a_topics(*i);
            _sub = new gazebo::transport::SubscriberPtr;

            *_sub = _node->Subscribe((const char*)*i
               , &USARcommand::Process_laser_scanner_callback, this);
          }
        }

        else if(NULL != strcasestr(*i, ET_D_G_GPS))
        {
          if(NULL == registered_topics_list.Search(*i))
          {
std::cout << "GPS cb registered" << std::endl;
            registered_topics_list.Add_a_topics(*i);
            _sub = new gazebo::transport::SubscriberPtr;

            *_sub = _node->Subscribe((const char*)*i
               , &USARcommand::Process_gps_callback, this);
          }
        }

        else if(NULL != strcasestr(*i, ET_D_G_IMU))
        {
          if(NULL == registered_topics_list.Search(*i))
          {
std::cout << "IMU cb registered" << std::endl;
            registered_topics_list.Add_a_topics(*i);
            _sub = new gazebo::transport::SubscriberPtr;

            *_sub = _node->Subscribe((const char*)*i
               , &USARcommand::Process_imu_callback, this);
          }
        }

        else if(NULL != strcasestr(*i, ET_D_G_ENCODER))
        {
          if(NULL == registered_topics_list.Search(*i))
          {
std::cout << "ENCODER cb registered" << std::endl;
            registered_topics_list.Add_a_topics(*i);
            _sub = new gazebo::transport::SubscriberPtr;

            *_sub = _node->Subscribe((const char*)*i
               , &USARcommand::Process_encoder_callback, this);
          }
        }
      }
    }
  }

  void Accept_Process(void)
  {
    _node = gazebo::transport::NodePtr(new gazebo::transport::Node());
    _node->Init();

    connections.push_back(event::Events::ConnectWorldUpdateBegin(
      boost::bind(&USARcommand
          ::Send_STA_and_Search_Sensors_and_Register_Callbacks, this)));
    UC_send_NFO(*this);

    while(1)
      UC_check_command_from_USARclient();
  }

  void UC_check_command_from_USARclient(void);
  void UC_send_NFO(USARcommand&);
};

#define GET_TYPE_FROM_REGISTERED_TOPIC(T,X) \
                              get_type_from_topic_name(T, sizeof(T),\
                                registered_topics_list.Search(X))
#define GET_NAME_FROM_REGISTERED_TOPIC(T,X) \
                              get_name_from_topic_name(T, sizeof(T),\
                                registered_topics_list.Search(X))

void USARcommand::Process_laser_scanner_callback(
                                          ConstLaserScanStampedPtr& _msg)
{
  boost::asio::streambuf sen;
  std::ostream os(&sen);
  char  Current_time_in_form[50];
  float Current_time=set_current_time_in_form(Current_time_in_form);
  int   Current_sec = (int)Current_time;
  int   angles;
  char  tmpbuf[100];
  float angle_width = _msg->scan().angle_max() - _msg->scan().angle_min();
  float steps       = angle_width / _msg->scan().angle_step();
  float resolution  = angle_width / steps;
  os << "SEN " <<  

        "{Time "<<_msg->time().sec()<<"."<<(int)_msg->time().nsec()<<"}"<< 
        "{Type RangeScanner}" <<
        "{Name " << GET_NAME_FROM_REGISTERED_TOPIC(tmpbuf, "scan")<<"}"<<
        "{Resolution " << resolution << "}" << 
        "{FOV " << angle_width << "}" << 
        "{Range ";

  angles = _msg->scan().ranges_size();
  if(0 < angles)
    for(int i = 0; angles > i; i++)
      os << _msg->scan().ranges((angles-1)-i) << (((angles-1)>i)?",":"");
  os << "}";
  os << "\r\n"; 
  boost::asio::write(_socket, sen);

}

void USARcommand::Process_gps_callback(ConstGPSPtr& _msg)
{
  char  Current_time_in_form[50];
  float Current_time=set_current_time_in_form(Current_time_in_form);
  int   Current_sec = (int)Current_time;
  int   iLon, iLat;
  float fLon, fLat;
  char  tmpbuf[100];
  boost::asio::streambuf sen;
  std::ostream os(&sen);
  iLat = (int)_msg->latitude_deg();
  iLon = (int)_msg->longitude_deg();
  fLat = (_msg->latitude_deg() - iLat) * 60.0;
  fLon = (_msg->longitude_deg() - iLon) * 60.0;
  os << "SEN " << 

        "{Time "<<_msg->time().sec()<<"."<<(int)_msg->time().nsec()<<"}"<< 
        "{Type GPS}" <<
        "{Name " << GET_TYPE_FROM_REGISTERED_TOPIC(tmpbuf, "gps")<<"}"<<
        "{Latitude " << iLat << "," << fLat << ",N}" << 
        "{Lonitude " << iLon << "," << fLon << ",E}" << 
        "{Satellites 8}{Fix 1}";
  os << "\r\n"; 
  boost::asio::write(_socket, sen);

}

void USARcommand::Process_imu_callback(ConstIMUPtr& _msg)
{
  char   Current_time_in_form[50];
  float  Current_time=set_current_time_in_form(Current_time_in_form);
  int    Current_sec = (int)Current_time;
  char   tmpbuf[100];
  static gazebo::math::Vector3 pose(0,0,0), vel(0,0,0), acl;
  double w, x, y, z, sqw, sqx, sqy, sqz, yaw, pitch, roll;
  float  dt;
  static float Last_time = 0;
  const gazebo::msgs::Quaternion& orientation = _msg->orientation();
  const gazebo::msgs::Vector3d& linear_acceleration = 
                                        _msg->linear_acceleration();

  _msg->stamp().sec();
  _msg->stamp().nsec();

  w = orientation.w();
  x = orientation.x();
  y = orientation.y();
  z = orientation.z();
  sqw = w*w;    
  sqx = x*x;    
  sqy = y*y;    
  sqz = z*z; 

  yaw   = atan2(2.0 * (x*y + z*w), (sqx - sqy - sqz + sqw));
  pitch = atan2(2.0 * (y*z + x*w), (-sqx - sqy + sqz + sqw));
  roll  = asin(-2.0 * (x*z - y*w));

  dt  = Current_time - Last_time;
  Last_time = Current_time;
  if(dt < 1)
  {
    acl.x = linear_acceleration.x();
    acl.y = linear_acceleration.y();

    float cz = cos(yaw), sz = sin(yaw);
    gazebo::math::Vector3 racl;
    racl.x = cz * acl.x - sz * acl.y;
    racl.y = sz * acl.x + cz * acl.y;

    vel.x += racl.x * dt;
    vel.y += racl.y * dt;
    vel.z +=  acl.z * dt;
    pose.x += vel.x * dt;
    pose.y += vel.y * dt;
    pose.z += vel.z * dt;

  }
  if(IMU_Disp_Counter < 0)
  {
    IMU_Disp_Counter = 10; 

    boost::asio::streambuf sen;
    std::ostream os(&sen);
    os << "SEN " << 
      "{Time "<<_msg->stamp().sec()<<"."<<(int)_msg->stamp().nsec()<<"}"<< 

          "{Type INS}" <<
          "{Name " << GET_NAME_FROM_REGISTERED_TOPIC(tmpbuf, "imu")<<"}"<<
          "{Location " << pose.x <<","<< pose.y <<","<< pose.z << "}" << 
          "{Orientation " << roll << "," << pitch << "," << yaw << "}";
    os << "\r\n"; 
    boost::asio::write(_socket, sen);

    boost::asio::streambuf sen_odo;
    std::ostream os_odo(&sen_odo);
    os_odo << "SEN " << 
      "{Time "<<_msg->stamp().sec()<<"."<<(int)_msg->stamp().nsec()<<"}"<< 

          "{Type " << ET_D_U_GROUNDTRUTH << "}" <<
          "{Name " << ET_D_G_GROUNDTRUTH << "}" <<
          "{Location " << pose.x <<","<< pose.y <<","<< pose.z << "}" << 
          "{Orientation " << roll << "," << pitch << "," << yaw << "}";
    os_odo << "\r\n"; 
    if(1 == POP.GroundTruthIsOn)
      boost::asio::write(_socket, sen_odo);
  }
  else
    IMU_Disp_Counter--;
}

#define RESOLUTION_1TICK (0.01745)
void USARcommand::Process_encoder_callback(ConstEncoderPtr& _msg)
{
  char  tmpbuf[100];
  boost::asio::streambuf sen;
  std::ostream os(&sen);
  char       Current_time_in_form[50];
  float      Current_time=set_current_time_in_form(Current_time_in_form);
  int        Current_sec = (int)Current_time;

  if(0 > ENCODER_Disp_counter)
  {
    ENCODER_Disp_counter = 10; 
    os << "SEN " << 
      "{Time " << Current_time_in_form << "}" << 
      "{Battery " << remain_battery << "}" << 
      "{Type " << ET_D_U_ENCODER << "}" << 
      "{Name " << "Lwheel" << " Tick " << (int)(_msg->left()/RESOLUTION_1TICK)  << "}" << 
      "{Name " << "Rwheel" << " Tick " << (int)(_msg->right()/RESOLUTION_1TICK) << "}" << 
      "\r\n";
    boost::asio::write(_socket, sen);
  }
  else
    ENCODER_Disp_counter--;
}

enum USAR_Command_Error_Code
{
  UCE_GOOD                        = 0,
  UCE_INCLUDING_BROKEN_BRACE      = 1,
  UCE_MISSING_NECESSARY_PARAMETER = 2,
  UCE_DO_NOT_CALL_AFTER_SPAWNED   = 3,
  UCE_NO_EFFECTER                 = 4
};

struct UC_INIT
{

  USARcommand& _parent;

  UC_INIT(USARcommand& parent) : _parent(parent)
  {

    if(1 == _parent.robot_was_spawned)
      return;
    if(UCE_GOOD == read_params_from_usar_command())
      spawn_a_robot();
  }

  int read_params_from_usar_command(void)
  {
    float                          x = 0, y = 0, z = 0, 
                                   r = 0, p = 0, yaw = 0;
    int                            battery = 3600;
    char*                          rtn;
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);

    BUCIP.Disp();

    if(1 == _parent.robot_was_spawned)
      return UCE_DO_NOT_CALL_AFTER_SPAWNED;

    if(BIE_GOOD != BUCIP.Error_code())
      return UCE_INCLUDING_BROKEN_BRACE;

    rtn = BUCIP.Search("ClassName");
    if(NULL == rtn)
      return UCE_MISSING_NECESSARY_PARAMETER; 

    rtn = BUCIP.Search("Location");
    if(NULL != rtn)
      sscanf(rtn, "%f,%f,%f", &x, &y, &z);

    rtn = BUCIP.Search("Rotation");
    if(NULL != rtn)
      sscanf(rtn, "%f,%f,%f", &r, &p, &yaw);

    rtn = BUCIP.Search("Start");
    if(NULL != rtn)
      POP.Search_StartPose(rtn, x, y, z, r, p, yaw);

    rtn = BUCIP.Search("Battery");
    if(NULL != rtn)
      sscanf(rtn, "%d", &battery);
    else 
      battery = get_battery_of_robot(BUCIP.Search("ClassName"));

    rtn = BUCIP.Search("Name");
    if(NULL != rtn)
      record_robot_param(BUCIP.Search("Name")
        , BUCIP.Search("ClassName"), x, y, z, r, p, yaw, battery);
    else
      record_robot_param(BUCIP.Search("ClassName")
        , BUCIP.Search("ClassName"), x, y, z, r, p, yaw, battery);
    return UCE_GOOD;
  }

  void spawn_a_robot(void)
  {
    char       model_cmd[100];
    TopicsList current_topics_list;

    if(1 == _parent.robot_was_spawned)
      return;

    gazebo::transport::PublisherPtr factoryPub
      = _parent._node->Advertise<gazebo::msgs::Factory>("~/factory");

    gazebo::msgs::Factory msg;

    sprintf(model_cmd, "model:

    msg.set_sdf_filename(model_cmd);

#if(GAZEBO_MAJOR_VERSION == 5)
    gazebo::msgs::Set(msg.mutable_pose()
     ,gazebo::math::Pose(_parent.spawn_location,_parent.spawn_direction));
#endif
#if(GAZEBO_MAJOR_VERSION == 7)
    ignition::math::Vector3d    locIgn(_parent.spawn_location.x,
                                    _parent.spawn_location.y,
                                    _parent.spawn_location.z);
    ignition::math::Quaterniond dirIgn(_parent.spawn_direction.x,
                                       _parent.spawn_direction.y,
                                       _parent.spawn_direction.z);
    ignition::math::Pose3d      poseIgn(locIgn, dirIgn);
    gazebo::msgs::Set(msg.mutable_pose(), poseIgn);
#endif

    factoryPub->Publish(msg);

    while (!_parent._parent._world->GetModel(_parent.model_name))
      usleep(1000); 

    gazebo::physics::ModelPtr mdl 
                 = _parent._parent._world->GetModel(_parent.model_name);
    mdl->SetName(_parent.own_name);

    _parent.robot_was_spawned = 1;
  }
#ifdef ____SAMMLE_____
  void spawn_a_robot(void)
  {
    int        spawned_check_loop = 20;
    char       model_cmd[100];
    TopicsList current_topics_list;

    if(1 == _parent.robot_was_spawned)
      return;

    gazebo::transport::PublisherPtr factoryPub
      = _parent._node->Advertise<gazebo::msgs::Factory>("~/factory");

    gazebo::msgs::Factory msg;

    sprintf(model_cmd, "model:

    msg.set_sdf_filename(model_cmd);

    gazebo::msgs::Set(msg.mutable_pose()
     ,gazebo::math::Pose(_parent.spawn_location,_parent.spawn_direction));

    factoryPub->Publish(msg);

    usleep(5000);
    gazebo::physics::ModelPtr mdl
                   = _parent._parent._world->GetModel(_parent.model_name);
    mdl->SetName(_parent.own_name);
    _parent.robot_was_spawned = 1;
  }

#endif
#define USARBOTCLASSNAMEPREFIX "USARBot."

  void record_robot_param(char* _own_name, char* _model_name
   , float x, float y, float z, float q1, float q2, float q3, int battery)
  {

    if(1 == _parent.robot_was_spawned)
      return;
    gazebo::math::Vector3 _location(x, y, z);
    gazebo::math::Quaternion _direction(q1, q2, q3);

    if(0 == strNcmp(_model_name, USARBOTCLASSNAMEPREFIX))
      _model_name = _model_name + strlen(USARBOTCLASSNAMEPREFIX);
    sprintf(_parent.topic_root, "~/%s", _own_name); 

    strcpy(_parent.own_name, _own_name); 
    strcpy(_parent.model_name, _model_name);
    _parent.spawn_location = _location;
    _parent.spawn_direction = _direction;
    _parent.full_battery = _parent.remain_battery = battery;
  }
};

struct UC_DRIVE
{

  USARcommand& _parent;

  UC_DRIVE(USARcommand& parent)
    : _parent(parent)
  {
    if(1 != _parent.robot_was_spawned)
      return;
    if(UCE_GOOD == read_params_from_usar_command())
      drive_a_robot();
  }

  int read_params_from_usar_command(void)
  {
    char*                          rtn;
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);

    if(BIE_GOOD != BUCIP.Error_code())
      return UCE_INCLUDING_BROKEN_BRACE;
    rtn = BUCIP.Search("LEFT");
    if(NULL != rtn)
      sscanf(rtn, "%f", &_parent._left_power);
    rtn = BUCIP.Search("RIGHT");
    if(NULL != rtn)
      sscanf(rtn, "%f", &_parent._right_power);
    return UCE_GOOD;
  }

  void drive_a_robot(void)
  {
    char  _TopicName[100];
    float _speed = 0, _turn = 0;
    int   _DriveType = get_drive_of_robot(_parent.model_name);

    if(1 != _parent.robot_was_spawned)
      return;

    sprintf(_TopicName, "~/%s/vel_cmd", _parent.own_name);

    gazebo::transport::PublisherPtr _pub_vel_cmd 
      = _parent._node->Advertise<gazebo::msgs::Pose>(_TopicName);

    _speed = (_parent._right_power + _parent._left_power) / 2.0;
    _turn  = (-_parent._right_power + _parent._left_power);
    _turn  = _MIN(_turn, M_PI * 2);
    _turn  = _MAX(_turn, -M_PI * 2);

    switch(_DriveType)
    {
      case DT_DiffarentialDrive:

        _turn = _turn;
        break;
      case DT_SkidsteerDrive   : 

        _turn = -_turn;
        break;
      case 0:
        return;
    }

#if(GAZEBO_MAJOR_VERSION == 5)
    gazebo::math::Pose pose(_speed, 0, 0, 0, 0, _turn);
#endif
#if(GAZEBO_MAJOR_VERSION == 7)
    ignition::math::Pose3d pose(_speed, 0, 0, 0, 0, _turn);
#endif
    gazebo::msgs::Pose msg;
    gazebo::msgs::Set(&msg, pose);

    _pub_vel_cmd->Publish(msg);
  }
};

struct UC_MULTIDRIVE
{

  USARcommand& _parent;

  UC_MULTIDRIVE(USARcommand& parent)
      : _parent(parent)
  {
    if(1 != _parent.robot_was_spawned)
      return;
    if(UCE_GOOD == read_params_from_usar_command())
      move_flippers();
  }

  int read_params_from_usar_command(void)
  {
    char*                          rtn;
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);

    if(BIE_GOOD != BUCIP.Error_code())
      return UCE_INCLUDING_BROKEN_BRACE;
    rtn = BUCIP.Search("FRFlipper");
    if(NULL != rtn)
      sscanf(rtn, "%lf", &_parent.flp_fr);
    rtn = BUCIP.Search("FLFlipper");
    if(NULL != rtn)
      sscanf(rtn, "%lf", &_parent.flp_fl);
    rtn = BUCIP.Search("RRFlipper");
    if(NULL != rtn)
      sscanf(rtn, "%lf", &_parent.flp_rr);
    rtn = BUCIP.Search("RLFlipper");
    if(NULL != rtn)
      sscanf(rtn, "%lf", &_parent.flp_rl);
    return UCE_GOOD;
  }

  void move_flippers(void)
  {
    flipper_control_msgs::msgs::FlipperControl flpmsg;
    char                                       _TopicName[100];

    if(1 != _parent.robot_was_spawned)
      return;

    sprintf(_TopicName, "~/%s/flp_cmd", _parent.own_name);

    gazebo::transport::PublisherPtr _pub_flp_cmd
     = _parent._node->Advertise<flipper_control_msgs::msgs::FlipperControl>
                                                             (_TopicName);

    _parent.flp_fr = _MIN(_parent.flp_fr, M_PI * 2); 
    _parent.flp_fr = _MAX(_parent.flp_fr, M_PI * -2);
    _parent.flp_fl = _MIN(_parent.flp_fl, M_PI * 2); 
    _parent.flp_fl = _MAX(_parent.flp_fl, M_PI * -2);
    _parent.flp_rr = _MIN(_parent.flp_rr, M_PI * 2); 
    _parent.flp_rr = _MAX(_parent.flp_rr, M_PI * -2);
    _parent.flp_rl = _MIN(_parent.flp_rl, M_PI * 2); 
    _parent.flp_rl = _MAX(_parent.flp_rl, M_PI * -2);
    flpmsg.set_fr(_parent.flp_fr);
    flpmsg.set_fl(_parent.flp_fl);
    flpmsg.set_rr(_parent.flp_rr);
    flpmsg.set_rl(_parent.flp_rl);
    _pub_flp_cmd->Publish(flpmsg);
  }
};

struct UC_GETSTARTPOSES
{

  USARcommand& _parent;
  int          NumOfStartPoses;

  UC_GETSTARTPOSES(USARcommand& parent) : _parent(parent)
  {
    int                    i;
    int                    nwritten;
    boost::asio::streambuf response;
    std::iostream          st_response(&response);

    st_response << "NFO {StartPoses " << POP.Num_Of_StartPoses << "}";
    for(i = 0; i < POP.Num_Of_StartPoses; i++)
      st_response << "{" << POP.StartPoses[i] << "}";
    st_response << "\r\n";

    nwritten = boost::asio::write(_parent._socket, response);
    if(0 >= nwritten)
    {
      perror("NFO message could not be written, check errno");
    }

  }
};

void USARcommand::UC_send_NFO(USARcommand& parent)
{
  UC_GETSTARTPOSES UC_getstartposes(parent);
}

#define UC_GET_TYPE_FROM_REGISTERED_TOPIC(T,X) \
                           get_type_from_topic_name(T, sizeof(T),\
                          _parent.registered_topics_list.Search(X))
#define UC_GET_NAME_FROM_REGISTERED_TOPIC(T,X) \
                           get_name_from_topic_name(T, sizeof(T),\
                          _parent.registered_topics_list.Search(X))

#define UC_GET_TYPE_FROM_TOPIC_NAME(T,S) \
                           get_type_from_topic_name(T, sizeof(T),S)
#define UC_GET_NAME_FROM_TOPIC_NAME(T,S) \
                           get_name_from_topic_name(T, sizeof(T),S)

struct UC_GETGEO
{

  USARcommand& _parent;

  UC_GETGEO(USARcommand& parent) : _parent(parent)
  {
    if(1 != _parent.robot_was_spawned)
      return;
    read_params_from_usar_command();
  }

  int GEO_set_effecters_params(std::iostream& st_response, 
                                             const char* _USARSim_name)
  {
    int         effecters;
    char        tmpbuf[100];
    int         flag_gps = 0;
    const char* topic_name;
    const char* Gazebo_name = get_Gazebo_name_of(_USARSim_name);
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    const char* effecter_name = BUCIP.Search("Name");
    st_response << "{Type " << _USARSim_name << "}";
    if(0 == strNcmp(Gazebo_name, ET_D_G_GPS))
      flag_gps = 1;
    if(NULL != effecter_name)
    {
      topic_name = _parent.registered_topics_list.Search(
                                            Gazebo_name, effecter_name);
      if(NULL == topic_name)
        return UCE_NO_EFFECTER;
      const char* dev_name = 
         ((flag_gps)?(UC_GET_TYPE_FROM_TOPIC_NAME(tmpbuf,topic_name)):
                     (UC_GET_NAME_FROM_TOPIC_NAME(tmpbuf,topic_name))); 
      st_response << 
        "{Name " << dev_name << "}" <<
        "{Location " << "0,0.1,0.1" << "}" <<
        "{Orientation " << "0,0.1,0.1" << "}" << 
        "{Mount " << _parent.own_name << "}";
      return UCE_GOOD;
    }
    for(int i = 0; 1; i++)
    {
      topic_name = _parent.registered_topics_list.Search_n(i,Gazebo_name);
      if(NULL == topic_name)
        if(0 == i)
          return UCE_NO_EFFECTER;
        else
          break;
      st_response << 
        "{Name " << 
         ((flag_gps)?(UC_GET_TYPE_FROM_TOPIC_NAME(tmpbuf,topic_name)):
                     (UC_GET_NAME_FROM_TOPIC_NAME(tmpbuf,topic_name)))<< 
        " Location " << "0,0.1,0.1" <<
        " Orientation " << "0,0.1,0.1" <<
        " Mount " << _parent.own_name << "}";
    }
    return UCE_GOOD;
  }

  int GEO_set_odometry_params(std::iostream& st_response, 
                                             const char* _USARSim_name)
  {
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    const char* effecter_name = BUCIP.Search("Name");
    st_response << "{Type " << _USARSim_name << "}";
    if(NULL != effecter_name)
    {
      st_response << 
        "{Name " << ET_D_G_ODOMETRY << "}" <<
        "{Location " << "0,0.1,0.1" << "}" <<
        "{Orientation " << "0,0.1,0.1" << "}" <<
        "{Mount " << _parent.own_name << "}";
    }
    else
    {
      st_response << 
        "{Name " << ET_D_G_ODOMETRY << 
        " Location " << "0,0.1,0.1" <<
        " Orientation " << "0,0.1,0.1" << 
        " Mount " << _parent.own_name << "}";
    }
    return UCE_GOOD;
  }

  int GEO_set_groundtruth_params(std::iostream& st_response, 
                                             const char* _USARSim_name)
  {
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    const char* effecter_name = BUCIP.Search("Name");
    st_response << "{Type " << _USARSim_name << "}";
    if(NULL != effecter_name)
    {
      st_response << 
        "{Name " << ET_D_G_GROUNDTRUTH << "}" << 
        "{Location " << "0,0.1,0.1" << "}" << 
        "{Orientation " << "0,0.1,0.1" << "}" << 
        "{Mount " << _parent.own_name << "}";
    }
    else
    {
      st_response << 
        "{Name " << ET_D_G_GROUNDTRUTH << 
        " Location " << "0,0.1,0.1" <<
        " Orientation " << "0,0.1,0.1" << 
        " Mount " << _parent.own_name << "}";
    }
    return UCE_GOOD;
  }

  int GEO_set_encoder_params(std::iostream& st_response, 
                                             const char* _USARSim_name)
  {
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    const char* effecter_name = BUCIP.Search("Name");
    st_response << "{Type " << _USARSim_name << "}";
    if(0 == strNcmp(effecter_name, "Lwheel"))
    {
      st_response << 
        "{Name " << "Lwheel" << "}" << 
        "{Location " << "0,0.1,0.1" << "}" << 
        "{Orientation " << "0,0.1,0.1" << "}" << 
        "{Mount " << _parent.own_name << "}";
    }
    else if(0 == strNcmp(effecter_name, "Rwheel"))
    {
      st_response << 
        "{Name " << "Rwheel" << "}" << 
        "{Location " << "0,0.1,0.1" << "}" << 
        "{Orientation " << "0,0.1,0.1" << "}" << 
        "{Mount " << _parent.own_name << "}";
    }
    else
    {
      st_response << 
        "{Name " << "Lwheel" << 
        " Location " << "0,0.1,0.1" <<
        " Orientation " << "0,0.1,0.1" << 
        " Mount " << _parent.own_name << "}" << 
        "{Name " << "Rwheel" << 
        " Location " << "0,0.1,0.1" <<
        " Orientation " << "0,0.1,0.1" << 
        " Mount " << _parent.own_name << "}";
    }
    return UCE_GOOD;
  }

  int read_params_from_usar_command(void)
  {
    char*                          rtn;
    char                           tmpbuf[100];
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    boost::asio::streambuf         response;
    std::iostream                  st_response(&response);
    int                            nwritten;

    if(BIE_GOOD != BUCIP.Error_code())
      return UCE_INCLUDING_BROKEN_BRACE;
    rtn = BUCIP.Search("Type");
    if(NULL != rtn)
    {
      st_response << "GEO ";
      if(NULL != strcasestr(rtn, ET_D_U_RFID))
        GEO_set_effecters_params(st_response, ET_D_U_RFID);
      else if(NULL != strcasestr(rtn, ET_D_U_CAMERA))
        GEO_set_effecters_params(st_response, ET_D_U_CAMERA);
      else if(NULL != strcasestr(rtn, ET_D_U_LASERSCANNER))
        GEO_set_effecters_params(st_response, ET_D_U_LASERSCANNER);
      else if(NULL != strcasestr(rtn, ET_D_U_GPS))
        GEO_set_effecters_params(st_response, ET_D_U_GPS);
      else if(NULL != strcasestr(rtn, ET_D_U_IMU))
        GEO_set_effecters_params(st_response, ET_D_U_IMU);
      else if(NULL != strcasestr(rtn, ET_D_U_ODOMETRY))
        GEO_set_odometry_params(st_response, ET_D_U_ODOMETRY);
      else if(NULL != strcasestr(rtn, ET_D_U_GROUNDTRUTH))
        GEO_set_groundtruth_params(st_response, ET_D_U_GROUNDTRUTH);
      else if(NULL != strcasestr(rtn, ET_D_U_ENCODER))
        GEO_set_encoder_params(st_response, ET_D_U_ENCODER);
      else if(NULL != strcasestr(rtn, "Robot"))
      {
        st_response << 
          "{Type " << get_type_of_robot(_parent.model_name) << "}" << 
          "{Name " << _parent.own_name << "}" <<
          "{Dimensions 0.7744,0.6318,0.5754}" << 
          "{COG 0.0000,0.0000,-0.1000}" <<
          "{WheelRadius 0.1922}" << 
          "{WheelSeparation 0.5120}" << 
          "{Wheelbase 0.3880}";
      }
      st_response << "\r\n";
      nwritten = boost::asio::write(_parent._socket, response);
      if(0 >= nwritten)
        perror("GEO message could not be written, check errno");
    }
    return UCE_GOOD;
  }
};

struct UC_GETCONF
{

  USARcommand& _parent;

  UC_GETCONF(USARcommand& parent) : _parent(parent)
  {
    if(1 != _parent.robot_was_spawned)
      return;
    read_params_from_usar_command();
  }

  int CONF_set_effecters_params(std::iostream& st_response, 
                                             const char* _USARSim_name)
  {
    int         effecters;
    char        tmpbuf[100];
    int         flag_gps = 0;
    const char* topic_name;
    const char* Gazebo_name = get_Gazebo_name_of(_USARSim_name);
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    const char* effecter_name = BUCIP.Search("Name");
    st_response << "{Type " << _USARSim_name << "}";
    if(0 == strNcmp(Gazebo_name, ET_D_G_GPS))
      flag_gps = 1;
    if(NULL != effecter_name)
    {
      topic_name = _parent.registered_topics_list.Search(
                                            Gazebo_name, effecter_name);
      if(NULL == topic_name)
        return UCE_NO_EFFECTER;
      const char* dev_name = 
         ((flag_gps)?(UC_GET_TYPE_FROM_TOPIC_NAME(tmpbuf,topic_name)):
                     (UC_GET_NAME_FROM_TOPIC_NAME(tmpbuf,topic_name))); 
      st_response << "{Name " << dev_name << "}";
      if(0 == strNcmp(Gazebo_name, ET_D_G_LASERSCANNER))
      {
        st_response<<"{MaxRange 10.0}" <<
                     "{MinRange 0.08}" <<
                     "{Resolution 0.0175508}" <<
                     "{Fov 3.1416}" <<
                     "{Panning False}" <<
                     "{Tilting True}";
      }
      return UCE_GOOD;
    }
    for(int i = 0; 1; i++)
    {
      topic_name = _parent.registered_topics_list.Search_n(i,Gazebo_name);
      if(NULL == topic_name)
        if(0 == i)
          return UCE_NO_EFFECTER;
        else
          break;
      const char* dev_name = 
         ((flag_gps)?(UC_GET_TYPE_FROM_TOPIC_NAME(tmpbuf,topic_name)):
                     (UC_GET_NAME_FROM_TOPIC_NAME(tmpbuf,topic_name))); 
      st_response << "{Name " << dev_name << " ";
      if(0 == strNcmp(Gazebo_name, ET_D_G_LASERSCANNER))
      {
        st_response<<" MaxRange 20.0" <<
                     " MinRange 0.08" <<
                     " Resolution 0.0175508"<<
                     " Fov 3.1416" <<
                     " Panning False" <<
                     " Tilting True}";
      }
    }
    return UCE_GOOD;
  }

  int CONF_set_odometry_params(std::iostream& st_response, 
                                             const char* _USARSim_name)
  {
    st_response << "{Type " << _USARSim_name << "}";
    st_response << 
      "{Name " << ET_D_G_ODOMETRY << 

      "}";
    return UCE_GOOD;
  }

  int CONF_set_groundtruth_params(std::iostream& st_response, 
                                             const char* _USARSim_name)
  {
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    const char* effecter_name = BUCIP.Search("Name");
    st_response << "{Type " << _USARSim_name << "}";
    if(NULL != effecter_name)
    {
      st_response << "{Name GndTruth}" <<
                     "{ScanInterval 0.1000}";
    }
    else
    {
      st_response << "{Name GndTruth" <<
                     " ScanInterval 0.1000}";
    }
    return UCE_GOOD;
  }

  int CONF_set_encoder_params(std::iostream& st_response, 
                                             const char* _USARSim_name)
  {
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    const char* effecter_name = BUCIP.Search("Name");
    st_response << "{Type " << _USARSim_name << "}";
    if(0 == strNcmp(effecter_name, "Lwheel"))
    {
      st_response << "{Name Lwheel}" <<
                     "{ScanInterval 1}";
    }
    else if(0 == strNcmp(effecter_name, "Rwheel"))
    {
      st_response << "{Name Rwheel}" <<
                     "{ScanInterval 1}";
    }
    else
    {
      st_response << "{Name Rwheel" <<
                     " ScanInterval 1}" << 
                     "{Name Lwheel" <<
                     " ScanInterval 1}";
    }
    return UCE_GOOD;
  }

  int read_params_from_usar_command(void)
  {
    char*                          rtn;
    char                           tmpbuf[100];
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    boost::asio::streambuf         response;
    std::iostream                  st_response(&response);
    int                            nwritten;

    if(BIE_GOOD != BUCIP.Error_code())
      return UCE_INCLUDING_BROKEN_BRACE;
    rtn = BUCIP.Search("Type");
    if(NULL != rtn)
    {
      st_response << "CONF ";
      if(NULL != strcasestr(rtn, ET_D_U_RFID))
        CONF_set_effecters_params(st_response, ET_D_U_RFID);
      else if(NULL != strcasestr(rtn, ET_D_U_CAMERA))
        CONF_set_effecters_params(st_response, ET_D_U_CAMERA);
      else if(NULL != strcasestr(rtn, ET_D_U_LASERSCANNER))
        CONF_set_effecters_params(st_response, ET_D_U_LASERSCANNER);
      else if(NULL != strcasestr(rtn, ET_D_U_GPS))
        CONF_set_effecters_params(st_response, ET_D_U_GPS);
      else if(NULL != strcasestr(rtn, ET_D_U_IMU))
        CONF_set_effecters_params(st_response, ET_D_U_IMU);
      else if(NULL != strcasestr(rtn, ET_D_U_ODOMETRY))
        CONF_set_odometry_params(st_response, ET_D_U_ODOMETRY);
      else if(NULL != strcasestr(rtn, ET_D_U_GROUNDTRUTH))
        CONF_set_groundtruth_params(st_response, ET_D_U_GROUNDTRUTH);
      else if(NULL != strcasestr(rtn, ET_D_U_ENCODER))
        CONF_set_encoder_params(st_response, ET_D_U_ENCODER);
      else if(NULL != strcasestr(rtn, "Robot"))
      {
        if(NULL != strcasestr(get_type_of_robot(_parent.model_name),
                                                 RT_D_GROUNDVEHICLE))
        {
          st_response << 
            "{Type " << get_type_of_robot(_parent.model_name) << "}" << 
            "{Name " << _parent.own_name << "}" <<
            "{SteeringType " << 
               get_steeringtype_of_robot(_parent.model_name) << "}" << 
            "{Mass " << 
               get_mass_of_robot(_parent.model_name) << "}" << 
            "{MaxSpeed " << 
               get_maxspeed_of_robot(_parent.model_name) << "}" << 
            "{MaxTorque " << 
               get_maxtorque_of_robot(_parent.model_name) << "}" << 
            "{MaxFrontSteer " << 
               get_maxfrontsteer_of_robot(_parent.model_name) << "}" << 
            "{MaxRearSteer " << 
               get_maxrearsteer_of_robot(_parent.model_name) << "}";
        }
      }
      st_response << "\r\n";
      nwritten = boost::asio::write(_parent._socket, response);
      if(0 >= nwritten)
        perror("GEO message could not be written, check errno");
    }
    return UCE_GOOD;
  }
};

void USARcommand::UC_check_command_from_USARclient(void)
{
  int nread;

  boost::system::error_code err;

  nread = boost::asio::read_until(_socket, _buffer, "\r\n", err);

  if(0 > nread)
  {
    perror("Could not read socket 3000");
    _parent.Remove_Child_Session(this);
    _thread.join();
  } 
  else if(0 == nread) 
  {
    perror("EOF, should break connection");
    perror("Turn this session into sleep to avoid decreasing performance.\n");
    for(;;)
      usleep(1000000);

  }
  std::istream is(&_buffer);
  std::string line;
  for(;1;)
  {

    std::getline(is, line);
    if(0 < line.length())
    {
      if(NULL != ucbuf)
        delete ucbuf;
      ucbuf = new char[line.length()+2];
      strcpy(ucbuf, (char*)line.c_str());

      printf("Received command string = %s\n", ucbuf);

      if(0 == strNcmp(ucbuf, "INIT"))
        UC_INIT UC_init(*this);
      else if(0 == strNcmp(ucbuf, "DRIVE"))
        UC_DRIVE UC_drive(*this);
      else if(0 == strNcmp(ucbuf, "MULTIDRIVE"))
        UC_MULTIDRIVE UC_multidrive(*this);
      else if(0 == strNcmp(ucbuf, "GETSTARTPOSES"))
        UC_GETSTARTPOSES UC_getstartposes(*this);
      else if(0 == strNcmp(ucbuf, "GETGEO"))
        UC_GETGEO UC_getgeo(*this);
      else if(0 == strNcmp(ucbuf, "GETCONF"))
        UC_GETCONF UC_getconf(*this);
      delete ucbuf;
      ucbuf = NULL;
    }
    else
      break;
  }
}

#define MAX_CAMERA_NUM 30
#define MAX_CAMERA_WINS 4
#define CAM_IMG_WIDTH 320
#define CAM_IMG_HEIGHT 240
#define LINESIZE (3*CAM_IMG_WIDTH)
#include <jpeglib.h>
#include <string.h>

#define CCO camera_connected_order

#define NO__CAMERA_IMAGE_TEST__

struct USARimage
{

  Server_Framework<USARimage>     &_parent;
  boost::asio::io_service          _ioservice;
  boost::asio::ip::tcp::socket     _socket;
  boost::asio::streambuf           _buffer;
  boost::thread                    _thread;
  gazebo::transport::NodePtr       _node;
  gazebo::transport::SubscriberPtr _sub_camera_image[MAX_CAMERA_NUM];

  void camera_callback_0(ConstImageStampedPtr& _msg);
  void camera_callback_1(ConstImageStampedPtr& _msg);
  void camera_callback_2(ConstImageStampedPtr& _msg);
  void camera_callback_3(ConstImageStampedPtr& _msg);
  void camera_callback_4(ConstImageStampedPtr& _msg);
  void camera_callback_5(ConstImageStampedPtr& _msg);
  void camera_callback_6(ConstImageStampedPtr& _msg);
  void camera_callback_7(ConstImageStampedPtr& _msg);
  void camera_callback_8(ConstImageStampedPtr& _msg);
  void camera_callback_9(ConstImageStampedPtr& _msg);
  void camera_callback_10(ConstImageStampedPtr& _msg);
  void camera_callback_11(ConstImageStampedPtr& _msg);
  void camera_callback_12(ConstImageStampedPtr& _msg);
  void camera_callback_13(ConstImageStampedPtr& _msg);
  void camera_callback_14(ConstImageStampedPtr& _msg);
  void camera_callback_15(ConstImageStampedPtr& _msg);
  void camera_callback_16(ConstImageStampedPtr& _msg);
  void camera_callback_17(ConstImageStampedPtr& _msg);
  void camera_callback_18(ConstImageStampedPtr& _msg);
  void camera_callback_19(ConstImageStampedPtr& _msg);
  void camera_callback_20(ConstImageStampedPtr& _msg);
  void camera_callback_21(ConstImageStampedPtr& _msg);
  void camera_callback_22(ConstImageStampedPtr& _msg);
  void camera_callback_23(ConstImageStampedPtr& _msg);
  void camera_callback_24(ConstImageStampedPtr& _msg);
  void camera_callback_25(ConstImageStampedPtr& _msg);
  void camera_callback_26(ConstImageStampedPtr& _msg);
  void camera_callback_27(ConstImageStampedPtr& _msg);
  void camera_callback_28(ConstImageStampedPtr& _msg);
  void camera_callback_29(ConstImageStampedPtr& _msg);
  void camera_callback_30(ConstImageStampedPtr& _msg);
  int camera_connected_order[MAX_CAMERA_NUM];
  int current_camera_order;
  JSAMPARRAY img;

  void Init(void)
  {
    int i;

    current_camera_order = 0;
    for(i = 0; i < MAX_CAMERA_NUM; i++)
      CCO[i] = -1;

    img = (JSAMPARRAY) new JSAMPROW[CAM_IMG_HEIGHT];
    for(i = 0; i < CAM_IMG_HEIGHT; i++)
      img[i] = (JSAMPROW) new JSAMPLE[3*CAM_IMG_WIDTH*MAX_CAMERA_WINS];
  }

  USARimage(Server_Framework<USARimage>&parent)
    : _parent(parent), _socket(_ioservice)

  { Init(); }

  ~USARimage()
  {

    for(int i = 0; i < CAM_IMG_HEIGHT; i++)
      delete img[i];
    delete img;
  }

  void send_camera_image_to_socket(void)
  {
    unsigned char *outbuffer_pt = NULL;
    unsigned long int outlen = 0;
    struct jpeg_compress_struct cinfo;
    struct jpeg_error_mgr jerr;
    cinfo.err = jpeg_std_error(&jerr);
    jpeg_create_compress(&cinfo);
    jpeg_mem_dest(&cinfo, &outbuffer_pt, &outlen);

    cinfo.image_width  = CAM_IMG_WIDTH * MAX_CAMERA_WINS;
    cinfo.image_height = CAM_IMG_HEIGHT;
    cinfo.input_components = 3;
    cinfo.in_color_space = JCS_RGB;
    jpeg_set_defaults(&cinfo);
    jpeg_set_quality(&cinfo, 75, TRUE);
    jpeg_start_compress(&cinfo, TRUE);
    jpeg_write_scanlines(&cinfo, img, CAM_IMG_HEIGHT);
    jpeg_finish_compress(&cinfo);
    jpeg_destroy_compress(&cinfo);
#ifdef __CAMERA_IMAGE_TEST__
    const char *filename = "test_jpeg_output.jpg";
    FILE *fp = fopen(filename, "wb");
    if (fp == NULL) {
        fprintf(stderr, "cannot open %s\n", filename);
        exit(EXIT_FAILURE);
    }
    fwrite((const void*)outbuffer_pt, outlen, 1, fp);
    fclose(fp);
#else
    unsigned char* ImageBuf = new unsigned char[5 + outlen];
    ImageBuf[0] = 3; 
    ImageBuf[1] = (unsigned char)((outlen >> 24) & (unsigned long)0x00FF);
    ImageBuf[2] = (unsigned char)((outlen >> 16) & (unsigned long)0x00FF);
    ImageBuf[3] = (unsigned char)((outlen >>  8) & (unsigned long)0x00FF);
    ImageBuf[4] = (unsigned char)(outlen         & (unsigned long)0x00FF);
    memcpy(&ImageBuf[5], outbuffer_pt, outlen);
    boost::asio::write(_socket
                 , boost::asio::buffer((void*)ImageBuf, 5 + outlen));
    delete ImageBuf;
#endif
    if(NULL != outbuffer_pt)
    {
      free(outbuffer_pt);
    }
  }

  void save_camera_image(int cam, ConstImageStampedPtr& _msg)
  {
    unsigned char* ip = (unsigned char*)_msg->image().data().c_str();
    if(-1 == CCO[cam])
    {
      if(MAX_CAMERA_WINS > current_camera_order)
        CCO[cam] = current_camera_order++;
      else
        return;
    }
    for(int i = 0; i < CAM_IMG_HEIGHT; i++)
      memcpy(&img[i][CCO[cam]*LINESIZE], &ip[i*LINESIZE], LINESIZE);
  }

  void Child_Session_Loop_Core(void)
  {
    boost::system::error_code err;
    boost::asio::read_until(_socket, _buffer , "\r\n", err);

    std::istream is(&_buffer);
    std::string  line;
    for(;1;)
    {
      std::getline(is, line);
      if(0 < line.length())
      {
        if(0 == strNcmp(line.c_str(), "OK"))
        {
          send_camera_image_to_socket();
        }

      }
      else
        break;
    }
  }

  void Accept_Process(void)
  {
    _node = gazebo::transport::NodePtr(new gazebo::transport::Node());
    _node->Init();

    _sub_camera_image[0] = _node->Subscribe(
      "/gazebo/default/pioneer3at_with_sensors/camera/link/camera/image",
       &USARimage::camera_callback_0, this);
    _sub_camera_image[1] = _node->Subscribe(
      "/gazebo/default/pioneer3at_with_sensors_b/camera/link/camera/image",
       &USARimage::camera_callback_1, this);
    _sub_camera_image[2] = _node->Subscribe(
      "/gazebo/default/pioneer3at_with_sensors_g/camera/link/camera/image",
       &USARimage::camera_callback_2, this);
    _sub_camera_image[3] = _node->Subscribe(
      "/gazebo/default/pioneer3at_with_sensors_y/camera/link/camera/image",
       &USARimage::camera_callback_3, this);
    _sub_camera_image[4] = _node->Subscribe(
      "/gazebo/default/pioneer3at_with_sensors_r/camera/link/camera/image",
       &USARimage::camera_callback_4, this);
    _sub_camera_image[5] = _node->Subscribe(
      "/gazebo/default/pioneer2dx_with_sensors_r/camera/link/camera/image",
       &USARimage::camera_callback_5, this);
    _sub_camera_image[6] = _node->Subscribe(
      "/gazebo/default/pioneer2dx_with_sensors_g/camera/link/camera/image",
       &USARimage::camera_callback_6, this);
    _sub_camera_image[7] = _node->Subscribe(
      "/gazebo/default/pioneer2dx_with_sensors_b/camera/link/camera/image",
       &USARimage::camera_callback_7, this);
    _sub_camera_image[8] = _node->Subscribe(
      "/gazebo/default/pioneer2dx_with_sensors_y/camera/link/camera/image",
       &USARimage::camera_callback_8, this);
    _sub_camera_image[9] = _node->Subscribe(
      "/gazebo/default/turtlebot_with_sensors_r/camera/link/camera/image",
       &USARimage::camera_callback_9, this);
    _sub_camera_image[10] = _node->Subscribe(
      "/gazebo/default/turtlebot_with_sensors_g/camera/link/camera/image",
       &USARimage::camera_callback_10, this);
    _sub_camera_image[11] = _node->Subscribe(
      "/gazebo/default/turtlebot_with_sensors_b/camera/link/camera/image",
       &USARimage::camera_callback_11, this);
    _sub_camera_image[12] = _node->Subscribe(
      "/gazebo/default/turtlebot_with_sensors_y/camera/link/camera/image",
       &USARimage::camera_callback_12, this);
    _sub_camera_image[13] = _node->Subscribe(
      "/gazebo/default/crawler_robot_r/camera/link/camera/image",
       &USARimage::camera_callback_12, this);
    _sub_camera_image[14] = _node->Subscribe(
      "/gazebo/default/crawler_robot_g/camera/link/camera/image",
       &USARimage::camera_callback_12, this);
    _sub_camera_image[15] = _node->Subscribe(
      "/gazebo/default/crawler_robot_b/camera/link/camera/image",
       &USARimage::camera_callback_12, this);
    _sub_camera_image[16] = _node->Subscribe(
      "/gazebo/default/crawler_robot_y/camera/link/camera/image",
       &USARimage::camera_callback_12, this);
    while(1)
      Child_Session_Loop_Core();

  }

};

void USARimage::camera_callback_0(ConstImageStampedPtr& _msg)
{
  save_camera_image(0, _msg);
}

void USARimage::camera_callback_1(ConstImageStampedPtr& _msg)
{
  save_camera_image(1, _msg);
}

void USARimage::camera_callback_2(ConstImageStampedPtr& _msg)
{
  save_camera_image(2, _msg);
}

void USARimage::camera_callback_3(ConstImageStampedPtr& _msg)
{
  save_camera_image(3, _msg);
}

void USARimage::camera_callback_4(ConstImageStampedPtr& _msg)
{
  save_camera_image(4, _msg);
}

void USARimage::camera_callback_5(ConstImageStampedPtr& _msg)
{
  save_camera_image(5, _msg);
}

void USARimage::camera_callback_6(ConstImageStampedPtr& _msg)
{
  save_camera_image(6, _msg);
}

void USARimage::camera_callback_7(ConstImageStampedPtr& _msg)
{
  save_camera_image(7, _msg);
}

void USARimage::camera_callback_8(ConstImageStampedPtr& _msg)
{
  save_camera_image(8, _msg);
}

void USARimage::camera_callback_9(ConstImageStampedPtr& _msg)
{
  save_camera_image(9, _msg);
}

void USARimage::camera_callback_10(ConstImageStampedPtr& _msg)
{
  save_camera_image(10, _msg);
}

void USARimage::camera_callback_11(ConstImageStampedPtr& _msg)
{
  save_camera_image(11, _msg);
}

void USARimage::camera_callback_12(ConstImageStampedPtr& _msg)
{
  save_camera_image(12, _msg);
}

void USARimage::camera_callback_13(ConstImageStampedPtr& _msg)
{
  save_camera_image(13, _msg);
}

void USARimage::camera_callback_14(ConstImageStampedPtr& _msg)
{
  save_camera_image(14, _msg);
}

void USARimage::camera_callback_15(ConstImageStampedPtr& _msg)
{
  save_camera_image(15, _msg);
}

void USARimage::camera_callback_16(ConstImageStampedPtr& _msg)
{
  save_camera_image(16, _msg);
}

void USARimage::camera_callback_17(ConstImageStampedPtr& _msg)
{
  save_camera_image(17, _msg);
}

void USARimage::camera_callback_18(ConstImageStampedPtr& _msg)
{
  save_camera_image(18, _msg);
}

void USARimage::camera_callback_19(ConstImageStampedPtr& _msg)
{
  save_camera_image(19, _msg);
}

void USARimage::camera_callback_20(ConstImageStampedPtr& _msg)
{
  save_camera_image(20, _msg);
}

void USARimage::camera_callback_21(ConstImageStampedPtr& _msg)
{
  save_camera_image(21, _msg);
}

void USARimage::camera_callback_22(ConstImageStampedPtr& _msg)
{
  save_camera_image(22, _msg);
}

void USARimage::camera_callback_23(ConstImageStampedPtr& _msg)
{
  save_camera_image(23, _msg);
}

void USARimage::camera_callback_24(ConstImageStampedPtr& _msg)
{
  save_camera_image(24, _msg);
}

void USARimage::camera_callback_25(ConstImageStampedPtr& _msg)
{
  save_camera_image(25, _msg);
}

void USARimage::camera_callback_26(ConstImageStampedPtr& _msg)
{
  save_camera_image(26, _msg);
}

void USARimage::camera_callback_27(ConstImageStampedPtr& _msg)
{
  save_camera_image(27, _msg);
}

void USARimage::camera_callback_28(ConstImageStampedPtr& _msg)
{
  save_camera_image(28, _msg);
}

void USARimage::camera_callback_29(ConstImageStampedPtr& _msg)
{
  save_camera_image(29, _msg);
}

void USARimage::camera_callback_30(ConstImageStampedPtr& _msg)
{
  save_camera_image(30, _msg);
}

#ifdef __DEBUG_SAMPLE__

void  SaveAsPPM(char* filename, ConstImageStampedPtr &_msg)
{
  unsigned char* ip = (unsigned char*)_msg->image().data().c_str();
  FILE* fp = fopen(filename, "wt");
  fprintf(fp, "P3\n%d %d\n255\n", _msg->image().width()
                                               , _msg->image().height());
  for(int i=0;i<_msg->image().width()*_msg->image().height();i++)
  fprintf(fp, "%3d %3d %3d\n", ip[i*3], ip[i*3+1], ip[i*3+2]);
  fclose(fp);
}

void USARimage::imageserver_callback(ConstImageStampedPtr& _msg)
{
  if(flag_OK)
    send_full_size_image(_msg);
  else if(flag_U)
    send_rectangle_area_image( _msg);
  else if(flag_SS)
  {
    flag_SS = 0;
    SaveAsPPM((char*)"/tmp/GazeboCameraImage.ppm", _msg);
  }
}

void USARimage::send_full_size_image(ConstImageStampedPtr& _msg)
{
  flag_OK = 0;
  int ImageSize = 3 * _msg->image().width() * _msg->image().height();
  unsigned char* ip = (unsigned char*)_msg->image().data().c_str();
  unsigned char* ImageBuf = new unsigned char[5 + ImageSize];

  ImageBuf[0] = 0; 

  *(unsigned long int*)&ImageBuf[1] = ImageSize; 

  for(int i = 0; i < _msg->image().width() * _msg->image().height(); i++)
  {
    ImageBuf[5+i*3+0]  = ip[i*3+2];
    ImageBuf[5+i*3+1]  = ip[i*3+0];
    ImageBuf[5+i*3+2]  = ip[i*3+1];
  }

  boost::asio::write(_socket, boost::asio::buffer((void*)ImageBuf
                     , 5 + ImageSize));

  delete ImageBuf;
printf("Sent a full size image in raw data, size = %d\n", 5+ImageSize);

}
#endif

  class USARGazebo : public WorldPlugin
  {

  public: USARGazebo(void):UCp(NULL),UIp(NULL) { }

  virtual ~USARGazebo()
  {
    if(NULL != UCp)
      delete UCp;
    if(NULL != UIp)
      delete UIp;
  }

  public: void Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
  {
    world = _parent;
    Init();
    POP.Read_Option_Parameters(_sdf);
  }

  private: void Init()
  {
    if(NULL == UCp)
      UCp = new Server_Framework<USARcommand>(3000, world);
    if(NULL == UIp)
      UIp = new Server_Framework<USARimage>(5003, world);
  }

private:

  gazebo::physics::WorldPtr world;
public:
  Server_Framework<USARcommand> *UCp;
  Server_Framework<USARimage>   *UIp;
  };

  GZ_REGISTER_WORLD_PLUGIN(USARGazebo)
}
// Desctiption: 
// This program's a translator between USARSim protocol and Gazebo protocol.
// This file is a primaly file of this program. 
// You can add some codes for treating USARsim command which you need in this file by copying and editing alread existing other codes of USARSim protcol.
// UC_INIT class is a good sample code for adding a new code for treating a new USARSim command.
//
// Author : Prof.Arnoud Visser, Dr.Nate Koenig, Masaru Shimizu
// E-Mail : shimizu@sist.chukyo-u.ac.jp
// Date   : Mar.2015-Feb.2016
//

#include <gazebo/gazebo.hh>
#include <gazebo/common/common.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/math/gzmath.hh>
#include "boost_server_framework.hh"
#include "break_usar_command_into_params.hh"
#include "get_topics_list.hh"
#include "flipper_control_msgs.hh"
#include "encoder_msgs.hh"

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <pthread.h>

#define _DEBUG_MSG_
#ifdef _DEBUG_MSG_
#define SCK_TXT(S,T) write(S, T, strlen(T))
#endif

#define strNcmp(B,C) strncasecmp(B,C,strlen(C))

#ifndef numof
#define numof(A)  (sizeof(A)/sizeof(A[0]))
#endif

#ifndef _MAX
#define _MAX(X,Y) (((X)>(Y))?(X):(Y))
#endif

#ifndef _MIN
#define _MIN(X,Y) (((X)<(Y))?(X):(Y))
#endif

//#######################################################################
//  USARGazebo Plugin Option Reader
//#######################################################################

#define MAX_StartPoses 10
#define MAX_StartPose_Length 1024

char Plugin_Option_Name_StartPose[][30]
              ={"StartPose_1", "StartPose_2", "StartPose_3"
               ,"StartPose_4", "StartPose_5", "StartPose_6"
               ,"StartPose_7", "StartPose_8", "StartPose_9"};

#define Plugin_Option_Name_GroundTruth "GroundTruth"

//////////////////////////////////////////////////////////////////
// Return device type in topic name
const char* get_string_between_slash_from_topic_name(char* dist, 
       int dist_size, int slash_num_from_right, const char* topic_name)
{
  const char* i;
  const char* start;
  const char* end;
  int         copy_size;
  if(1 > slash_num_from_right)
    return (const char*)NULL;
  i = topic_name;
  i+= strlen(topic_name);
  end = i;
  start = (const char*)NULL;
  for(i--; topic_name <= i; i--)
  {
    if('/' == *i)
    {
      start = (i + 1);
      slash_num_from_right--;
      if(0 < slash_num_from_right)
      {
        end = i;
        start = (const char*)NULL;
        continue;
      }
      break;
    }
  }
  if((const char*)NULL == start)
    return (const char*)NULL;
  dist_size--; // for 0x0(end of string)
  copy_size = (int)(end - start);
  strncpy(dist, start, (copy_size > dist_size)?dist_size:copy_size);
  dist[copy_size] = 0;
  return (const char*)dist;
}

//////////////////////////////////////////////////////////////////
// Return device type in topic name
const char* get_type_from_topic_name(char* dist, int dist_size, 
                                              const char* topic_name)
{
  return get_string_between_slash_from_topic_name(dist, dist_size, 1
                                                          , topic_name);
}
//////////////////////////////////////////////////////////////////
// Return device name in topic name
const char* get_name_from_topic_name(char* dist, int dist_size, 
                                              const char* topic_name)
{
  return get_string_between_slash_from_topic_name(dist, dist_size, 2
                                                          , topic_name);
}

//////////////////////////////////////////////////////////////////
// Plugin Optoin Parameters Reader from SDF 
struct Plugin_Option_Parameters
{
  int Num_Of_StartPoses;
  char StartPoses[MAX_StartPoses][MAX_StartPose_Length];
  char GroundTruth[MAX_StartPose_Length];
  int  GroundTruthIsOn;

  //////////////////////////////////////////////////////////////////
  // Constructor
  Plugin_Option_Parameters(void) : Num_Of_StartPoses(0) { }

  //////////////////////////////////////////////////////////////////
  // Read Option Parameters from SDF file
  void Read_Option_Parameters(sdf::ElementPtr _sdf)
  {
    Read_Start_Pose_Parameters(_sdf);
    Read_GroundTruth_Parameter(_sdf);
  }

  //////////////////////////////////////////////////////////////////
  // Read Start Pose Parameters into StartPose array
  void Read_Start_Pose_Parameters(sdf::ElementPtr _sdf)
  {
    int i;
    for(i = 0; i < numof(Plugin_Option_Name_StartPose); i++)
    {
      if(_sdf->HasElement(Plugin_Option_Name_StartPose[i]))
      {
        std::stringstream s;
        s << _sdf->GetElement(Plugin_Option_Name_StartPose[i])
                    ->Get<std::string>();
        strncat(StartPoses[Num_Of_StartPoses], s.str().c_str()
                 , MAX_StartPose_Length);
        Num_Of_StartPoses++;
      }
    }
  }

  //////////////////////////////////////////////////////////////////
  // Read Ground Truth Parameters
  void Read_GroundTruth_Parameter(sdf::ElementPtr _sdf)
  {
    int i;
    GroundTruthIsOn = 0;
    if(_sdf->HasElement(Plugin_Option_Name_GroundTruth))
    {
      std::stringstream s;
      s << _sdf->GetElement(Plugin_Option_Name_GroundTruth)
                  ->Get<std::string>();
      strncat(GroundTruth, s.str().c_str(), MAX_StartPose_Length);
      if(NULL != strcasestr(GroundTruth, "true")
      || NULL != strcasestr(GroundTruth, "1"))
        GroundTruthIsOn = 1;
    }
  }

  //////////////////////////////////////////////////////////////////
  // Search Start Pose from StartPose array and set parameters 
  int Search_StartPose(char* target_name, float& _x, float& _y, float& _z,
                       float& _roll, float& _pitch, float& _yaw)
  {
    int   i, read_fields;
    char  name[100];
    float x, y, z, roll, pitch, yaw;
    for(i = 0; i < Num_Of_StartPoses; i++)
    {
      printf("%d %s\n", i, StartPoses[i]); 
      read_fields = sscanf(StartPoses[i], "%s %f,%f,%f %f,%f,%f"
                            , name, &x, &y, &z, &roll, &pitch, &yaw);
      if(7 != read_fields)
        return 0; // Broken parameters.
      if(0 == strNcmp(target_name, name))
      {
        _x     = x;
        _y     = y;
        _z     = z;
        _roll  = roll;
        _pitch = pitch;
        _yaw   = yaw;
        return 1; // Found and There were all parameters
      }
    }
    return 0; // No same name parameter.
  }
};

Plugin_Option_Parameters POP;

//#######################################################################
//  USAR Command Server
//#######################################################################


//MEMO in changing camera and hokuyo
// Differances in tipic names between before and after
/*
BEFORE
/gazebo/default/pioneer3at_with_sensors_b/chassis/l_camera/image
/gazebo/default/pioneer3at_with_sensors_b/chassis/r_camera/image
/gazebo/default/pioneer3at_with_sensors_b/laser_tilt_mount_link/lms200/scan
/gazebo/default/pioneer3at_with_sensors_b/laser_tilt_mount_link/gps
/gazebo/default/pioneer3at_with_sensors_b/chassis/imu_sensor/imu
/gazebo/default/pioneer3at_with_sensors_b/joint_cmd
/gazebo/default/pioneer3at_with_sensors_b/vel_cmd
/gazebo/default/pioneer3at_with_sensors_b::chassis

AFTER
/gazebo/default/pioneer3at_with_sensors/camera/link/camera/image
/gazebo/default/pioneer3at_with_sensors/hokuyo/link/laser/scan
/gazebo/default/pioneer3at_with_sensors/chassis/gps
/gazebo/default/pioneer3at_with_sensors/chassis/imu_sensor/imu
/gazebo/default/pioneer3at_with_sensors/joint_cmd
/gazebo/default/pioneer3at_with_sensors/vel_cmd
/gazebo/default/pioneer3at_with_sensors::chassis
*/

//////////////////////////////////////////////////////////////////
// Useful flags 
struct RD
{
  char  Request;
  char  Done;
  void  Init(void) { }
  RD(void) : Request(0), Done(0) { Init(); }
};

//////////////////////////////////////////////////////////////////
// ROBOT_TYPE
enum ROBOT_TYPE
{
  RT_GROUNDVECHILE = 1,
  RT_LEGGEDROBOT   = 2,
  RT_NAUTICVEHICLE = 3,
  RT_AERIALVEHICLE = 4
};

//////////////////////////////////////////////////////////////////
// ROBOT_TYPE_NAME
#define RT_D_GROUNDVEHICLE "GroundVehicle"
#define RT_D_LEGGEDROBOT   "LeggedRobot"
#define RT_D_NAUTICVEHICLE "NauticVehicle"
#define RT_D_AERIALVEHICLE "AerialVehicle"
#define RT_D_UNKNOWN       "Unknown"

//////////////////////////////////////////////////////////////////
// DRIVE_TYPE for UC_DRIVE
enum DRIVE_TYPE
{
  DT_DiffarentialDrive = 1,
  DT_SkidsteerDrive    = 2,
  DT_QuadRotor         = 3,
  DT_FlipperArm        = 4
};

//////////////////////////////////////////////////////////////////
// Robot Database 
struct ROBOT_DATABASE
{
  const char* Model_Name;
  const char* Type;
  const char* SteeringType;
  const float Mass;
  const float MaxSpeed;
  const float MaxTorque;
  const float MaxFrontSteer;
  const float MaxRearSteer;
  int         Battery;
  int         DriveType;
} Robot_DB[]
 = {{"pioneer3at_with_sensors", RT_D_GROUNDVEHICLE, "SkidSteered",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_SkidsteerDrive}

   ,{"pioneer3at_with_sensors_r", RT_D_GROUNDVEHICLE, "SkidSteered",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_SkidsteerDrive}

   ,{"pioneer3at_with_sensors_b", RT_D_GROUNDVEHICLE, "SkidSteered",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_SkidsteerDrive}

   ,{"pioneer3at_with_sensors_g", RT_D_GROUNDVEHICLE, "SkidSteered",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_SkidsteerDrive}

   ,{"pioneer3at_with_sensors_y", RT_D_GROUNDVEHICLE, "SkidSteered",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_SkidsteerDrive}

   ,{"Crawler_Robot", RT_D_GROUNDVEHICLE, "SkidSteered",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_SkidsteerDrive}

   ,{"pioneer3at", RT_D_GROUNDVEHICLE, "SkidSteered",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_SkidsteerDrive}

   ,{"pioneer2dx_with_sensors", RT_D_GROUNDVEHICLE, "SkidSteered",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"pioneer2dx_with_sensors_r", RT_D_GROUNDVEHICLE, "SkidSteered",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"pioneer2dx_with_sensors_g", RT_D_GROUNDVEHICLE, "SkidSteered",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"pioneer2dx_with_sensors_b", RT_D_GROUNDVEHICLE, "SkidSteered",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"pioneer2dx_with_sensors_y", RT_D_GROUNDVEHICLE, "SkidSteered",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"turtlebot_with_sensors_r", RT_D_GROUNDVEHICLE, "SkidSteered",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"turtlebot_with_sensors_g", RT_D_GROUNDVEHICLE, "SkidSteered",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"turtlebot_with_sensors_b", RT_D_GROUNDVEHICLE, "SkidSteered",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"turtlebot_with_sensors_y", RT_D_GROUNDVEHICLE, "SkidSteered",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_DiffarentialDrive}

   ,{"quadrotor", RT_D_AERIALVEHICLE, "RotaryWing",
   //Mass Speed Torque Front Rear Battery  DT
       20,    5,    10,   10,  10,   3600, DT_QuadRotor}

   ,{"noname", RT_D_UNKNOWN, "Unknown",
   //Mass Speed Torque Front Rear Battery  DT
        0,    0,     0,    0,   0,      0, 0} };

int get_number_of_Robot_DB(const char* robot_model_name)
{
  int i;
  for(i = 0; (numof(Robot_DB) - 1) > i; i++)
    if(0 == strNcmp(robot_model_name, Robot_DB[i].Model_Name))
      break;
  return i;
}

#define get_type_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)].Type
#define get_steeringtype_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)]\
           .SteeringType
#define get_mass_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)]\
           .Mass
#define get_maxspeed_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)]\
           .MaxSpeed
#define get_maxtorque_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)]\
           .MaxTorque
#define get_maxfrontsteer_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)]\
           .MaxFrontSteer
#define get_maxrearsteer_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)]\
           .MaxRearSteer
#define get_battery_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)].Battery
#define get_drive_of_robot(X) Robot_DB[get_number_of_Robot_DB(X)].DriveType

//////////////////////////////////////////////////////////////////
// EFFECTER_TYPE_NAME
#define ET_D_G_RFID         "Unknown"
#define ET_D_U_RFID         "RFID"
#define ET_D_G_GPS          "gps"
#define ET_D_U_GPS          "GPS"
#define ET_D_G_IMU          "imu"
#define ET_D_U_IMU          "INS"
#define ET_D_G_CAMERA       "image"
#define ET_D_U_CAMERA       "Camera"
#define ET_D_G_LASERSCANNER "scan"
#define ET_D_U_LASERSCANNER "RangeScanner"
#define ET_D_G_ODOMETRY     "Odometry"
#define ET_D_U_ODOMETRY     "Odometry"
#define ET_D_G_GROUNDTRUTH  "GndTruth"
#define ET_D_U_GROUNDTRUTH  "GroundTruth"
#define ET_D_G_ENCODER      "Encoders"
#define ET_D_U_ENCODER      "Encoder"

//////////////////////////////////////////////////////////////////
// EFFECTER_NAME
#define ET_D_N_LASERSCANNER "lms200"

//////////////////////////////////////////////////////////////////
// Gazebo-USARSim Database 
struct Gazebo_USARSim_DATABASE
{
  const char* Gazebo_name;
  const char* USARSim_name;
} Gazebo_USARSim_DB[]
 = {{ET_D_G_LASERSCANNER, ET_D_U_LASERSCANNER},
    {ET_D_G_CAMERA,       ET_D_U_CAMERA},
    {ET_D_G_RFID,         ET_D_U_RFID},
    {ET_D_G_GPS,          ET_D_U_GPS},
    {ET_D_G_IMU,          ET_D_U_IMU},
    {ET_D_G_ODOMETRY,     ET_D_U_ODOMETRY},
    {ET_D_G_GROUNDTRUTH,  ET_D_U_GROUNDTRUTH},
    {ET_D_G_ENCODER,      ET_D_U_ENCODER},
    {"Unknown",           "UnKnown"} };

int get_number_of_DB_From_Gazebo_name(const char* _name)
{
  int i;
  for(i = 0; (numof(Gazebo_USARSim_DB) - 1) > i; i++)
    if(0 == strNcmp(_name, Gazebo_USARSim_DB[i].Gazebo_name))
      break;
  return i;
}

int get_number_of_DB_From_USARSim_name(const char* _name)
{
  int i;
  for(i = 0; (numof(Gazebo_USARSim_DB) - 1) > i; i++)
    if(0 == strNcmp(_name, Gazebo_USARSim_DB[i].USARSim_name))
      break;
  return i;
}

#define get_USARSim_name_of(X) \
      Gazebo_USARSim_DB[get_number_of_DB_From_Gazebo_name(X)].USARSim_name
#define get_Gazebo_name_of(X) \
      Gazebo_USARSim_DB[get_number_of_DB_From_USARSim_name(X)].Gazebo_name

#define INIT_FLP (M_PI/4)

namespace gazebo
{
//////////////////////////////////////////////////////////////////
// USARcommand 
struct USARcommand
{
  //////////////////////////////////////////////////////////////////
  // USARcommand.Variables
  Server_Framework<USARcommand>&_parent;
  boost::asio::io_service       _ioservice;
  boost::asio::ip::tcp::socket  _socket;
  boost::asio::streambuf        _buffer;
  boost::thread                 _thread;
  gazebo::transport::NodePtr    _node;
  // Wheel torque
  float                         _left_power, _right_power;
  // Flipper Angle
  double                        flp_fr, flp_fl, flp_rr, flp_rl;
  // Add your own variables here
//RD   Msg, Spawn;
  int                           robot_was_spawned;
  char                          model_name[100], 
                                own_name[100], 
                                topic_root[100];
  char*                         ucbuf; // Pointer of USARSim command string
  gazebo::math::Vector3         spawn_location;
  gazebo::math::Quaternion      spawn_direction;
  TopicsList                    current_topics_list,registered_topics_list;
  int                           full_battery, remain_battery;
  gazebo::transport::SubscriberPtr* _sub;
  std::vector<event::ConnectionPtr> connections;
  int                           STA_Disp_Counter, IMU_Disp_Counter;
  int                           STA_Last_sec, STA_Count_Per_1sec; 
  int                           ENCODER_Disp_counter; 

  //////////////////////////////////////////////////////////////////
  // Initialize something...
  void Init(void)
  {
    model_name[0] = own_name[0] = topic_root[0] = 0;
  }

  //////////////////////////////////////////////////////////////////
  // USARcommand.Constructor
  USARcommand(Server_Framework<USARcommand>&parent) : 
    _parent(parent), _socket(_ioservice), ucbuf(NULL), robot_was_spawned(0)
      , current_topics_list(), registered_topics_list()
      , STA_Disp_Counter(0), IMU_Disp_Counter(0), ENCODER_Disp_counter(0)
      , STA_Last_sec(0), STA_Count_Per_1sec(0)
      , _left_power(0), _right_power(0)
      , flp_fr(INIT_FLP), flp_fl(INIT_FLP)
      , flp_rr(INIT_FLP), flp_rl(INIT_FLP)
//    , Msg(), Spawn() 
  { Init(); }

  //////////////////////////////////////////////////////////////////
  // USARcommand.Desstructor
  ~USARcommand() 
  {
    connections.clear();
  }

  //////////////////////////////////////////////////////////////////
  // set_current_time_in_form
  float set_current_time_in_form(char* buf)
  {
    float  Current_time = _parent._world->GetRealTime().Double();
    sprintf(buf, "%.4f", Current_time);
    return Current_time;
  }

  //////////////////////////////////////////////////////////////////
  // USARcommand.Process_STA for USARcommand.Send_SENS
  void Process_STA(void)
  {
    boost::asio::streambuf response;
    std::ostream           os_res(&response);
    char       Current_time_in_form[50];
    float      Current_time=set_current_time_in_form(Current_time_in_form);
    int        Current_sec = (int)Current_time;
    if(0 == robot_was_spawned)
      return;
    if(STA_Last_sec != Current_sec)
    {
      STA_Count_Per_1sec = 0;
      if(0 < remain_battery)
        remain_battery--;
    }
    if(0 > STA_Disp_Counter)
    {
      STA_Disp_Counter = 100; // Display STA 1 times per 100 loop times
      os_res << "STA " << 
        "{Time " << Current_time_in_form << "}" << 
        "{Battery " << remain_battery << "}" << 
        "{Type " << get_type_of_robot(model_name) << "}" << 
        "{LightToggle False}" << 
        "{LightIntensity 100}" << 
        "{FrontSteer 0}" << 
        "{RearSteer 0}" << 
        "{CountPer1sec " << STA_Count_Per_1sec << "}" << 
        "\r\n";
    }
    else
      STA_Disp_Counter--;
    STA_Count_Per_1sec++;
    STA_Last_sec = Current_sec;
    boost::asio::write(_socket, response);
  }

/* THIS IS OLD ONE. NOW SIMPLY MEMO...
  //////////////////////////////////////////////////////////////////
  // USARcommand.Process_SEN_ENCODER for USARcommand.Send_SENS
  void Process_ENCODER(void)
  {
    boost::asio::streambuf response;
    std::ostream           os_res(&response);
    char       Current_time_in_form[50];
    float      Current_time=set_current_time_in_form(Current_time_in_form);
    int        Current_sec = (int)Current_time;
    if(0 == robot_was_spawned)
      return;
    if(0 > ENCODER_Disp_counter)
    {
      ENCODER_Disp_counter = 100; // Display ENCODER 1 times per 100 loop times
      os_res << "SEN " << 
        "{Time " << Current_time_in_form << "}" << 
        "{Battery " << remain_battery << "}" << 
        "{Type " << ET_D_U_ENCODER << "}" << 
        "{Name " << "Lwheel" << " Tick " << "0"  << "}" << 
        "{Name " << "Rwheel" << " Tick " << "0"  << "}" << 
        "\r\n";
    }
    else
      ENCODER_Disp_counter--;
    boost::asio::write(_socket, response);
  }
*/

  //////////////////////////////////////////////////////////////////
  // Prototypes of callback functions
  void Process_laser_scanner_callback(ConstLaserScanStampedPtr& _msg);
  void Process_gps_callback(ConstGPSPtr& _msg);
  void Process_imu_callback(ConstIMUPtr& _msg);
  void Process_encoder_callback(ConstEncoderPtr& _msg);
  void Send_odometry(ConstIMUPtr& _msg);
  
  //////////////////////////////////////////////////////////////////
  // USARcommand.Send_SENS   ## NEED YOUR HELP TO INCREASE SENSORS ##
  void Send_STA_and_Search_Sensors_and_Register_Callbacks(void)
  {
    static int             refresh_cycle = 0;
    if(1 != robot_was_spawned)
      return;
    // 1. send STA of this robot
    Process_STA();
//    Process_ENCODER();
    // 2. Check robot's sensors from topics list
    if(NULL == current_topics_list.Search(own_name) 
      ||  0 == current_topics_list.Size()
      || 700 < refresh_cycle++)
    {
      refresh_cycle = 0;
      current_topics_list.Refresh_Topics_List();
      current_topics_list.Filter(own_name);
      if(0 == current_topics_list.Size())
        return;
    //registered_topics_list.Disp_topics_list();
    // 3. Register callback functions for sensors
      for(typename std::set<char*>::iterator i 
                                 = current_topics_list._topics_list.begin()
            ; i != current_topics_list._topics_list.end(); i++)
      {
        // At only first time, register a callback function for the sensor.
        // RangeFinder 
        if(NULL != strcasestr(*i, ET_D_G_LASERSCANNER) 
          && NULL != strcasestr(*i, ET_D_N_LASERSCANNER))
        {
          if(NULL == registered_topics_list.Search(*i))
          {
std::cout << "Laser cb registered" << std::endl;
            registered_topics_list.Add_a_topics(*i);
            _sub = new gazebo::transport::SubscriberPtr;
    // The function Subscribe needs a valiable which be assigned 
    //  a return value from the function Subscribe. Without the assigning,
    //   a call-back function  which was registered 
    //    by the function Subscribe will NOT be call-backed.
    // DO NOT REMOVE *_sub
            *_sub = _node->Subscribe((const char*)*i
               , &USARcommand::Process_laser_scanner_callback, this);
          }
        }
        // GPS 
        else if(NULL != strcasestr(*i, ET_D_G_GPS))
        {
          if(NULL == registered_topics_list.Search(*i))
          {
std::cout << "GPS cb registered" << std::endl;
            registered_topics_list.Add_a_topics(*i);
            _sub = new gazebo::transport::SubscriberPtr;
    // The function Subscribe needs a valiable which be assigned 
    //  a return value from the function Subscribe. Without the assigning,
    //   a call-back function  which was registered 
    //    by the function Subscribe will NOT be call-backed.
    // DO NOT REMOVE *_sub
            *_sub = _node->Subscribe((const char*)*i
               , &USARcommand::Process_gps_callback, this);
          }
        }
        // IMU 
        else if(NULL != strcasestr(*i, ET_D_G_IMU))
        {
          if(NULL == registered_topics_list.Search(*i))
          {
std::cout << "IMU cb registered" << std::endl;
            registered_topics_list.Add_a_topics(*i);
            _sub = new gazebo::transport::SubscriberPtr;
    // The function Subscribe needs a valiable which be assigned 
    //  a return value from the function Subscribe. Without the assigning,
    //   a call-back function  which was registered 
    //    by the function Subscribe will NOT be call-backed.
    // DO NOT REMOVE *_sub
            *_sub = _node->Subscribe((const char*)*i
               , &USARcommand::Process_imu_callback, this);
          }
        }
        // Encoder 
        else if(NULL != strcasestr(*i, ET_D_G_ENCODER))
        {
          if(NULL == registered_topics_list.Search(*i))
          {
std::cout << "ENCODER cb registered" << std::endl;
            registered_topics_list.Add_a_topics(*i);
            _sub = new gazebo::transport::SubscriberPtr;
    // The function Subscribe needs a valiable which be assigned 
    //  a return value from the function Subscribe. Without the assigning,
    //   a call-back function  which was registered 
    //    by the function Subscribe will NOT be call-backed.
    // DO NOT REMOVE *_sub
            *_sub = _node->Subscribe((const char*)*i
               , &USARcommand::Process_encoder_callback, this);
          }
        }
      }
    }
  }

  //////////////////////////////////////////////////////////////////
  // USARcommand.Accept_Process
  void Accept_Process(void)
  {
    _node = gazebo::transport::NodePtr(new gazebo::transport::Node());
    _node->Init(/*_parent._world->GetName()*/);
    // Binding a sensor functions with cyclic timer
    connections.push_back(event::Events::ConnectWorldUpdateBegin(
      boost::bind(&USARcommand
          ::Send_STA_and_Search_Sensors_and_Register_Callbacks, this)));
    UC_send_NFO(*this);
//std::cout << "Accept_Process address [" << this << "]" << std::endl;
    while(1)
      UC_check_command_from_USARclient();
  }

  //////////////////////////////////////////////////////////////////
  // USARcommand.functions which need to be defined 
  //  at outside of this class definition
  void UC_check_command_from_USARclient(void);
  void UC_send_NFO(USARcommand&);
};

#define GET_TYPE_FROM_REGISTERED_TOPIC(T,X) \
                              get_type_from_topic_name(T, sizeof(T),\
                                registered_topics_list.Search(X))
#define GET_NAME_FROM_REGISTERED_TOPIC(T,X) \
                              get_name_from_topic_name(T, sizeof(T),\
                                registered_topics_list.Search(X))

//////////////////////////////////////////////////////////////////////
// Process_laser_scanner_callback
//   Defined on USARSim Manual P.53-54
// This function is called everytime a message is received on topics
//  See following page or source files about type of _msg
//   /usr/include/gazebo-5.1/gazebo/msgs/laserscan.pb.h
//   https://osrf-distributions.s3.amazonaws.com/gazebo/api/dev/classgazebo_1_1sensors_1_1GpuRaySensor.html
//   https://osrf-distributions.s3.amazonaws.com/gazebo/api/dev/classgazebo_1_1sensors_1_1RaySensor.html
//   gazebo-5.0.1/gazebo/sensors/Raysensor.{cc|hh}
//   gazebo-5.0.1/gazebo/msgs/sensor.proto
// 
//  BUT FINALLY, I FOUND THE CORRECT DATA TYPE FROM:
//   gz topic -i /gazebo/default/pioneer3at_with_sensors/laser_tilt_mount_link/laser_tilt/scan
//
// About ConstLaserScanStampedPtr
//   https://www.google.co.jp/webhp?sourceid=chrome-instant&ion=1&espv=2&ie=UTF-8#q=ConstLaserScanStampedPtr
//   http://docs.ros.org/indigo/api/gazebo_plugins/html/classgazebo_1_1GazeboRosLaser-members.html
//   http://docs.ros.org/indigo/api/gazebo_plugins/html/classgazebo_1_1GazeboRosLaser.html#af46b29ad8afebfb8d96edcda9ecc58ad
//   http://docs.ros.org/indigo/api/gazebo_plugins/html/gazebo__ros__gpu__laser_8cpp_source.html
//#include <gazebo/physics/World.hh>
//#include <gazebo/physics/HingeJoint.hh>
//#include <gazebo/sensors/Sensor.hh>
//#include <gazebo/common/Exception.hh>
//#include <gazebo/sensors/GpuRaySensor.hh>
//#include <gazebo/sensors/SensorTypes.hh>
//#include <gazebo/transport/transport.hh>
//#include <tf/tf.h>
//#include <tf/transform_listener.h>
void USARcommand::Process_laser_scanner_callback(
                                          ConstLaserScanStampedPtr& _msg)
{
  boost::asio::streambuf sen;
  std::ostream os(&sen);
  char  Current_time_in_form[50];
  float Current_time=set_current_time_in_form(Current_time_in_form);
  int   Current_sec = (int)Current_time;
  int   angles;
  char  tmpbuf[100];
  float angle_width = _msg->scan().angle_max() - _msg->scan().angle_min();
  float steps       = angle_width / _msg->scan().angle_step();
  float resolution  = angle_width / steps;
  os << "SEN " <<  
//        "{Time " << Current_time_in_form << "}" << 
        "{Time "<<_msg->time().sec()<<"."<<(int)_msg->time().nsec()<<"}"<< 
        "{Type RangeScanner}" <<
        "{Name " << GET_NAME_FROM_REGISTERED_TOPIC(tmpbuf, "scan")<<"}"<<
        "{Resolution " << resolution << "}" << 
        "{FOV " << angle_width << "}" << 
        "{Range ";
  /* MEMO
  for(typename std::set<double>::iterator i=_msg->scan().ranges().begin()
        ; i != _msg->scan().ranges().end(); i++)
  */
  angles = _msg->scan().ranges_size();
  if(0 < angles)
    for(int i = 0; angles > i; i++)
      os << _msg->scan().ranges((angles-1)-i) << (((angles-1)>i)?",":"");
  os << "}";
  os << "\r\n"; 
  boost::asio::write(_socket, sen);
//  std::cout << _msg->DebugString();
  /*
  _msg->time().sec();
  _msg->time().nsec();
  _msg->scan().angle_min();
  _msg->scan().angle_max();
  _msg->scan().angle_step();
  _msg->scan().range_min();
  _msg->scan().range_max();
  _msg->scan().ranges_size();
  _msg->scan().ranges(i)
  _msg->scan().intensities_size();
  _msg->scan().intensities(i)
  */
}

//////////////////////////////////////////////////////////////////////
// Process_gps_callback
//   Defined on USARSim Manual P.54-55
// This function is called everytime a message is received on topics
//  See following page or source files about type of _msg
// About ConstGPSPtr
//   https://github.com/arpg/Gazebo/blob/master/gazebo/msgs/gps.proto
//    message GPS
//    {
//      required Time time                    = 1;
//      required string link_name             = 2;
//      required double latitude_deg          = 3;
//      required double longitude_deg         = 4;
//      required double altitude              = 5;
//      optional double velocity_east         = 6;
//      optional double velocity_north        = 7;
//      optional double velocity_up           = 8;
//    }
void USARcommand::Process_gps_callback(ConstGPSPtr& _msg)
{
  char  Current_time_in_form[50];
  float Current_time=set_current_time_in_form(Current_time_in_form);
  int   Current_sec = (int)Current_time;
  int   iLon, iLat;
  float fLon, fLat;
  char  tmpbuf[100];
  boost::asio::streambuf sen;
  std::ostream os(&sen);
  iLat = (int)_msg->latitude_deg();
  iLon = (int)_msg->longitude_deg();
  fLat = (_msg->latitude_deg() - iLat) * 60.0;
  fLon = (_msg->longitude_deg() - iLon) * 60.0;
  os << "SEN " << 
//        "{Time " << Current_time_in_form << "}" << 
        "{Time "<<_msg->time().sec()<<"."<<(int)_msg->time().nsec()<<"}"<< 
        "{Type GPS}" <<
        "{Name " << GET_TYPE_FROM_REGISTERED_TOPIC(tmpbuf, "gps")<<"}"<<
        "{Latitude " << iLat << "," << fLat << ",N}" << 
        "{Lonitude " << iLon << "," << fLon << ",E}" << 
        "{Satellites 8}{Fix 1}";
  os << "\r\n"; 
  boost::asio::write(_socket, sen);
//  std::cout << _msg->DebugString();
  /*
  _msg->time().sec();
  _msg->time().nsec();
  _msg->latitude_deg();
  _msg->longitude_deg();
  _msg->altitude();
  _msg->velocity_east();
  _msg->velocity_north();
  _msg->velocity_up();
  */
}

//////////////////////////////////////////////////////////////////////
// Process_imu_callback
//   Defined on USARSim Manual P.55-56
// This function is called everytime a message is received on topics
//  See following page or source files about type of _msg
//   https://github.com/UWARG/simSPIKE/blob/master/GazeboToArduino/imutest
//   https://github.com/arpg/Gazebo/blob/master/gazebo/msgs/imu.proto
//    message IMU
//    {
//      required Time stamp                   = 1;
//      required string entity_name           = 2;
//      required Quaternion orientation       = 3;
//      required Vector3d angular_velocity    = 4;
//      required Vector3d linear_acceleration = 5;
//    }
void USARcommand::Process_imu_callback(ConstIMUPtr& _msg)
{
  char   Current_time_in_form[50];
  float  Current_time=set_current_time_in_form(Current_time_in_form);
  int    Current_sec = (int)Current_time;
  char   tmpbuf[100];
  static gazebo::math::Vector3 pose(0,0,0), vel(0,0,0), acl;
  double w, x, y, z, sqw, sqx, sqy, sqz, yaw, pitch, roll;
  float  dt;
  static float Last_time = 0;
  const gazebo::msgs::Quaternion& orientation = _msg->orientation();
  const gazebo::msgs::Vector3d& linear_acceleration = 
                                        _msg->linear_acceleration();
//std::cout << _msg->DebugString();
  _msg->stamp().sec();
  _msg->stamp().nsec();
  //Break out the values from the Quarternion and convert to YPR
  w = orientation.w();
  x = orientation.x();
  y = orientation.y();
  z = orientation.z();
  sqw = w*w;    
  sqx = x*x;    
  sqy = y*y;    
  sqz = z*z; 
   //YPR conversions from Quarternion
  yaw   = atan2(2.0 * (x*y + z*w), (sqx - sqy - sqz + sqw));
  pitch = atan2(2.0 * (y*z + x*w), (-sqx - sqy + sqz + sqw));
  roll  = asin(-2.0 * (x*z - y*w));
  // Calc Pose
  dt  = Current_time - Last_time;
  Last_time = Current_time;
  if(dt < 1)
  {
    acl.x = linear_acceleration.x();
    acl.y = linear_acceleration.y();
//    acl.z = linear_acceleration.z(); // It must have gravity value;
    // Reverse Rotation around Z axis
    float cz = cos(yaw), sz = sin(yaw);
    gazebo::math::Vector3 racl;
    racl.x = cz * acl.x - sz * acl.y;
    racl.y = sz * acl.x + cz * acl.y;
    // Integration
    vel.x += racl.x * dt;
    vel.y += racl.y * dt;
    vel.z +=  acl.z * dt;
    pose.x += vel.x * dt;
    pose.y += vel.y * dt;
    pose.z += vel.z * dt;
//printf("IMU: X,Y,YAW=%f,%f,%f\n", pose.x, pose.y, yaw);
  }
  if(IMU_Disp_Counter < 0)
  {
    IMU_Disp_Counter = 10; // Display STA 1 times per 100 loop times
    ///////////////////////////////////////////////////////////////
    // Send INS
    boost::asio::streambuf sen;
    std::ostream os(&sen);
    os << "SEN " << 
      "{Time "<<_msg->stamp().sec()<<"."<<(int)_msg->stamp().nsec()<<"}"<< 
    //      "{Time " << Current_time_in_form << "}" << 
          "{Type INS}" <<
          "{Name " << GET_NAME_FROM_REGISTERED_TOPIC(tmpbuf, "imu")<<"}"<<
          "{Location " << pose.x <<","<< pose.y <<","<< pose.z << "}" << 
          "{Orientation " << roll << "," << pitch << "," << yaw << "}";
    os << "\r\n"; 
    boost::asio::write(_socket, sen);
    ///////////////////////////////////////////////////////////////
    // Send Odometory 
    boost::asio::streambuf sen_odo;
    std::ostream os_odo(&sen_odo);
    os_odo << "SEN " << 
      "{Time "<<_msg->stamp().sec()<<"."<<(int)_msg->stamp().nsec()<<"}"<< 
    //      "{Time " << Current_time_in_form << "}" << 
    //      "{Type Odometry}" <<
    //      "{Name " << ET_D_G_ODOMETRY << "}" <<
          "{Type " << ET_D_U_GROUNDTRUTH << "}" <<
          "{Name " << ET_D_G_GROUNDTRUTH << "}" <<
          "{Location " << pose.x <<","<< pose.y <<","<< pose.z << "}" << 
          "{Orientation " << roll << "," << pitch << "," << yaw << "}";
    os_odo << "\r\n"; 
    if(1 == POP.GroundTruthIsOn)
      boost::asio::write(_socket, sen_odo);
  }
  else
    IMU_Disp_Counter--;
}

//////////////////////////////////////////////////////////////////////
// Process_encoder_callback
//   Defined on USARSim Manual P.82
// This function is called everytime a message is received on topics
//  See following page or source files about type of _msg
// About ConstEncoderPtr
//    double get_right();
//    double get_left();
#define RESOLUTION_1TICK (0.01745)
void USARcommand::Process_encoder_callback(ConstEncoderPtr& _msg)
{
  char  tmpbuf[100];
  boost::asio::streambuf sen;
  std::ostream os(&sen);
  char       Current_time_in_form[50];
  float      Current_time=set_current_time_in_form(Current_time_in_form);
  int        Current_sec = (int)Current_time;

  if(0 > ENCODER_Disp_counter)
  {
    ENCODER_Disp_counter = 10; // Display ENCODER 1 times per 50 loop times
    os << "SEN " << 
      "{Time " << Current_time_in_form << "}" << 
      "{Battery " << remain_battery << "}" << 
      "{Type " << ET_D_U_ENCODER << "}" << 
      "{Name " << "Lwheel" << " Tick " << (int)(_msg->left()/RESOLUTION_1TICK)  << "}" << 
      "{Name " << "Rwheel" << " Tick " << (int)(_msg->right()/RESOLUTION_1TICK) << "}" << 
      "\r\n";
    boost::asio::write(_socket, sen);
  }
  else
    ENCODER_Disp_counter--;
}

//////////////////////////////////////////////////////////////////
// Error Code Definition for UC_**** functions
enum USAR_Command_Error_Code
{
  UCE_GOOD                        = 0,
  UCE_INCLUDING_BROKEN_BRACE      = 1,
  UCE_MISSING_NECESSARY_PARAMETER = 2,
  UCE_DO_NOT_CALL_AFTER_SPAWNED   = 3,
  UCE_NO_EFFECTER                 = 4
};

//////////////////////////////////////////////////////////////////
// UC_INIT
struct UC_INIT
{
  //////////////////////////////////////////////////////////////////
  //  Variables
  USARcommand& _parent;

  //////////////////////////////////////////////////////////////////
  //  The constructor
  UC_INIT(USARcommand& parent) : _parent(parent)
  {
      // Already a robot has been spawned, then return
    if(1 == _parent.robot_was_spawned)
      return;
    if(UCE_GOOD == read_params_from_usar_command())
      spawn_a_robot();
  }

  //////////////////////////////////////////////////////////////////
  //  read_params_from_usar_command
  int read_params_from_usar_command(void)
  {
    float                          x = 0, y = 0, z = 0, 
                                   r = 0, p = 0, yaw = 0;
    int                            battery = 3600;
    char*                          rtn;
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    // Display Init parameters for debug.
    BUCIP.Disp();
    // Already a robot has been spawned, then return
    if(1 == _parent.robot_was_spawned)
      return UCE_DO_NOT_CALL_AFTER_SPAWNED;
    // If there are any error in received init paramers, return error code.
    if(BIE_GOOD != BUCIP.Error_code())
      return UCE_INCLUDING_BROKEN_BRACE;
    // Read Robot ClassName parameter (Required)
    rtn = BUCIP.Search("ClassName");
    if(NULL == rtn)
      return UCE_MISSING_NECESSARY_PARAMETER; // FATAL ERROR: NO CLASSNAME
    // If a location parameter was given....(Option)
    rtn = BUCIP.Search("Location");
    if(NULL != rtn)
      sscanf(rtn, "%f,%f,%f", &x, &y, &z);
    // If a rotation parameter was given....(Option)
    rtn = BUCIP.Search("Rotation");
    if(NULL != rtn)
      sscanf(rtn, "%f,%f,%f", &r, &p, &yaw);
    // If a Start parameter was given....(Option)
    rtn = BUCIP.Search("Start");
    if(NULL != rtn)
      POP.Search_StartPose(rtn, x, y, z, r, p, yaw);
    // If a battery parameter was given....(Option)
    rtn = BUCIP.Search("Battery");
    if(NULL != rtn)
      sscanf(rtn, "%d", &battery);
    else // If no battery parameter, get it from Robot_DB
      battery = get_battery_of_robot(BUCIP.Search("ClassName"));
    // Read own robot name parameter (Option) AND set parameters
    rtn = BUCIP.Search("Name");
    if(NULL != rtn)
      record_robot_param(BUCIP.Search("Name")
        , BUCIP.Search("ClassName"), x, y, z, r, p, yaw, battery);
    else
      record_robot_param(BUCIP.Search("ClassName")
        , BUCIP.Search("ClassName"), x, y, z, r, p, yaw, battery);
    return UCE_GOOD;
  }

  //////////////////////////////////////////////////////////////////
  //  spawn_a_robot
  void spawn_a_robot(void)
  {
    char       model_cmd[100];
    TopicsList current_topics_list;
    // Already a robot has been spawned, then return
    if(1 == _parent.robot_was_spawned)
      return;
    // Create a publisher on the ~/factory topic
    gazebo::transport::PublisherPtr factoryPub
      = _parent._node->Advertise<gazebo::msgs::Factory>("~/factory");
    // Create the message
    gazebo::msgs::Factory msg;
    // Prepare command
    sprintf(model_cmd, "model://%s", _parent.model_name);

    // Get the path to the model sdf file.
    /*std::string modelFile = gazebo::common::SystemPaths::Instance()->FindFile(model_cmd);
    // Open the sdf file
    std::ifstream modelIn(modelFile.c_str());

    // 1: Read modelIn into a string.
    std::string modelStr;
    while (!modelIn.eof())
    {
      std::string line;
      std::getline(modelIn, line);
      modelStr += line;
    }

    // 2: Change the name to something new
    */


    // Model file to load
    msg.set_sdf_filename(model_cmd);
    // Set this robot's own name
/*msg.set_edit_name is not working in Gazebo5.
    msg.set_edit_name(_parent.own_name); */
    // Pose to initialize the model to
#if(GAZEBO_MAJOR_VERSION == 5)
    gazebo::msgs::Set(msg.mutable_pose()
     ,gazebo::math::Pose(_parent.spawn_location,_parent.spawn_direction));
#endif
#if(GAZEBO_MAJOR_VERSION == 7)
    ignition::math::Vector3d    locIgn(_parent.spawn_location.x,
                                    _parent.spawn_location.y,
                                    _parent.spawn_location.z);
    ignition::math::Quaterniond dirIgn(_parent.spawn_direction.x,
                                       _parent.spawn_direction.y,
                                       _parent.spawn_direction.z);
    ignition::math::Pose3d      poseIgn(locIgn, dirIgn);
    gazebo::msgs::Set(msg.mutable_pose(), poseIgn);
#endif
    // Send the message (SPAWN A ROBOT!!)
    factoryPub->Publish(msg);

    while (!_parent._parent._world->GetModel(_parent.model_name))
      usleep(1000); // Wait for finishing spawn job

    gazebo::physics::ModelPtr mdl 
                 = _parent._parent._world->GetModel(_parent.model_name);
    mdl->SetName(_parent.own_name);

    // set_edit_name should equal the name of a model that already exists
    // in Gazebo
    //msg.set_edit_name(_parent.model_name);
    // Now set a new SDF, that will change the model.
    //factoryPub->Publish(msg);
    //  checking loop to get topics of the robot
    //   if any topics of the robot could be got, 
    //    set _parent.robot_was_spawned with "1".
    /*
    for(int check_loop_cnt=0; 10 > check_loop_cnt; check_loop_cnt++)
    {
      current_topics_list.Refresh_Topics_List();
      // The following line is tempolary method
      current_topics_list.Filter(_parent.model_name);
      // The following line is correct
      // ,but now set_edit_name() is not working,then we can not use this.
      //current_topics_list.Filter(_parent.own_name);
      if(0 < current_topics_list.Size())
      {
        _parent.robot_was_spawned = 1;
        break;
      }
      usleep(100); // Wait more for finishing spawn job
    }
    */
    _parent.robot_was_spawned = 1;
  }
#ifdef ____SAMMLE_____
  void spawn_a_robot(void)
  {
    int        spawned_check_loop = 20;
    char       model_cmd[100];
    TopicsList current_topics_list;
    // Already a robot has been spawned, then return
    if(1 == _parent.robot_was_spawned)
      return;
    // Create a publisher on the ~/factory topic
    gazebo::transport::PublisherPtr factoryPub
      = _parent._node->Advertise<gazebo::msgs::Factory>("~/factory");
    // Create the message
    gazebo::msgs::Factory msg;
    // Prepare command
    sprintf(model_cmd, "model://%s", _parent.model_name);

    // Get the path to the model sdf file.
    /*std::string modelFile = gazebo::common::SystemPaths::Instance()->FindFile(model_cmd);
    // Open the sdf file
    std::ifstream modelIn(modelFile.c_str());
    // 1: Read modelIn into a string.
    std::string modelStr;
    while (!modelIn.eof())
    {
      std::string line;
      std::getline(modelIn, line);
      modelStr += line;
    }
    // 2: Change the name to something new
    */

    // Model file to load
    msg.set_sdf_filename(model_cmd);
    // Set this robot's own name
/*msg.set_edit_name is not working in Gazebo5.
    msg.set_edit_name(_parent.own_name);*/
    // Pose to initialize the model to
    gazebo::msgs::Set(msg.mutable_pose()
     ,gazebo::math::Pose(_parent.spawn_location,_parent.spawn_direction));
    // Send the message (SPAWN A ROBOT!!)
    factoryPub->Publish(msg);
    /*
    while(NULL != _parent._parent._world->GetModel(_parent.model_name))
    {
printf("Now waiting : %d\n", spawned_check_loop);
      if(0 == spawned_check_loop--)
        return;
      usleep(1000); // Wait for finishing spawn job
    }
    usleep(1000); // Wait for finishing spawn job
    */
    // set_edit_name should equal the name of a model that already exists
    // in Gazebo
    //msg.set_edit_name(_parent.model_name);
    // Now set a new SDF, that will change the model.
    //factoryPub->Publish(msg);
    //  checking loop to get topics of the robot
    //   if any topics of the robot could be got, 
    //    set _parent.robot_was_spawned with "1".
    /*
    for(int check_loop_cnt=0; 10 > check_loop_cnt; check_loop_cnt++)
    {
      current_topics_list.Refresh_Topics_List();
      // The following line is tempolary method
      current_topics_list.Filter(_parent.model_name);
      // The following line is correct
      // ,but now set_edit_name() is not working,then we can not use this.
      //current_topics_list.Filter(_parent.own_name);
      if(0 < current_topics_list.Size())
      {
        _parent.robot_was_spawned = 1;
        break;
      }
      usleep(100); // Wait more for finishing spawn job
    }
    */
    usleep(5000);
    gazebo::physics::ModelPtr mdl
                   = _parent._parent._world->GetModel(_parent.model_name);
    mdl->SetName(_parent.own_name);
    _parent.robot_was_spawned = 1;
  }

#endif
#define USARBOTCLASSNAMEPREFIX "USARBot."

  //////////////////////////////////////////////////////////////////
  //  record_robot_param
  void record_robot_param(char* _own_name, char* _model_name
   , float x, float y, float z, float q1, float q2, float q3, int battery)
  {
      // Already a robot has been spawned, then return
    if(1 == _parent.robot_was_spawned)
      return;
    gazebo::math::Vector3 _location(x, y, z);
    gazebo::math::Quaternion _direction(q1, q2, q3);
    // Remove "USARBot." from the model name, if it exists.
    if(0 == strNcmp(_model_name, USARBOTCLASSNAMEPREFIX))
      _model_name = _model_name + strlen(USARBOTCLASSNAMEPREFIX);
    sprintf(_parent.topic_root, "~/%s", _own_name); // ~ = /gazebo/default
    // The following line is tempolary method
    //  until set_edit_name() be fixed
    //strcpy(_parent.own_name, _model_name /*_own_name*/);
    // The following line is correct
    // ,but now set_edit_name() was out of order,then we can not use this.
    strcpy(_parent.own_name, _own_name); 
    strcpy(_parent.model_name, _model_name);
    _parent.spawn_location = _location;
    _parent.spawn_direction = _direction;
    _parent.full_battery = _parent.remain_battery = battery;
  }
};

//////////////////////////////////////////////////////////////////
// UC_DRIVE
//                          (GOOD TEMPLATE TO ADD ANOTHER COMMAND)
struct UC_DRIVE
{
  //////////////////////////////////////////////////////////////////
  //  Variables
  USARcommand& _parent;

  //////////////////////////////////////////////////////////////////
  //  The constructor
  UC_DRIVE(USARcommand& parent)
    : _parent(parent)
  {
    if(1 != _parent.robot_was_spawned)
      return;
    if(UCE_GOOD == read_params_from_usar_command())
      drive_a_robot();
  }

  //////////////////////////////////////////////////////////////////
  //  read_params_from_usar_command
  int read_params_from_usar_command(void)
  {
    char*                          rtn;
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
//  BUCIP.Disp();
    if(BIE_GOOD != BUCIP.Error_code())
      return UCE_INCLUDING_BROKEN_BRACE;
    rtn = BUCIP.Search("LEFT");
    if(NULL != rtn)
      sscanf(rtn, "%f", &_parent._left_power);
    rtn = BUCIP.Search("RIGHT");
    if(NULL != rtn)
      sscanf(rtn, "%f", &_parent._right_power);
    return UCE_GOOD;
  }

  //////////////////////////////////////////////////////////////////
  //  drive_a_robot
  void drive_a_robot(void)
  {
    char  _TopicName[100];
    float _speed = 0, _turn = 0;
    int   _DriveType = get_drive_of_robot(_parent.model_name);
    // Already a robot has been spawned, then return
    if(1 != _parent.robot_was_spawned)
      return;
    // Prepare topic name for drive command
    sprintf(_TopicName, "~/%s/vel_cmd", _parent.own_name);
 // sprintf(_TopicName, "~/%s/vel_cmd", _parent.model_name);
    // Create a publisher on the ~/factory topic
    gazebo::transport::PublisherPtr _pub_vel_cmd 
      = _parent._node->Advertise<gazebo::msgs::Pose>(_TopicName);
    // Wait for finishing the connection
 // _pub_vel_cmd->WaitForConnection();
    // Calc speed and turn
    _speed = (_parent._right_power + _parent._left_power) / 2.0;
    _turn  = (-_parent._right_power + _parent._left_power);
    _turn  = _MIN(_turn, M_PI * 2);
    _turn  = _MAX(_turn, -M_PI * 2);
    // Adjust parameters by robot drive type
    switch(_DriveType)
    {
      case DT_DiffarentialDrive:
        // Pioneer 2DX(Skidsteer); Turn > 0 then robot turn clock wise
        _turn = _turn;
        break;
      case DT_SkidsteerDrive   : 
        // Pioneer 3AT(Diffarential); Turn < 0 then robot turn clock wise
        _turn = -_turn;
        break;
      case 0:
        return;
    }
    // Set the message
#if(GAZEBO_MAJOR_VERSION == 5)
    gazebo::math::Pose pose(_speed, 0, 0, 0, 0, _turn);
#endif
#if(GAZEBO_MAJOR_VERSION == 7)
    ignition::math::Pose3d pose(_speed, 0, 0, 0, 0, _turn);
#endif
    gazebo::msgs::Pose msg;
    gazebo::msgs::Set(&msg, pose);
    // Send the message
    _pub_vel_cmd->Publish(msg);
  }
};

//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
// UC_MULTIDRIVE
// Front flipper: MULTIDRIVE {FRFlipper float } {FLflipper float }
// Rear  flipper: MULTIDRIVE {RRFlipper float } {RLflipper float }

struct UC_MULTIDRIVE
{
  //////////////////////////////////////////////////////////////////
  //  Variables
  USARcommand& _parent;

  //////////////////////////////////////////////////////////////////
  //  The constructor
  UC_MULTIDRIVE(USARcommand& parent)
      : _parent(parent)
  {
    if(1 != _parent.robot_was_spawned)
      return;
    if(UCE_GOOD == read_params_from_usar_command())
      move_flippers();
  }

  //////////////////////////////////////////////////////////////////
  //  read_params_from_usar_command
  int read_params_from_usar_command(void)
  {
    char*                          rtn;
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
//  BUCIP.Disp();
    if(BIE_GOOD != BUCIP.Error_code())
      return UCE_INCLUDING_BROKEN_BRACE;
    rtn = BUCIP.Search("FRFlipper");
    if(NULL != rtn)
      sscanf(rtn, "%lf", &_parent.flp_fr);
    rtn = BUCIP.Search("FLFlipper");
    if(NULL != rtn)
      sscanf(rtn, "%lf", &_parent.flp_fl);
    rtn = BUCIP.Search("RRFlipper");
    if(NULL != rtn)
      sscanf(rtn, "%lf", &_parent.flp_rr);
    rtn = BUCIP.Search("RLFlipper");
    if(NULL != rtn)
      sscanf(rtn, "%lf", &_parent.flp_rl);
    return UCE_GOOD;
  }

  //////////////////////////////////////////////////////////////////
  //  drive_a_robot
  void move_flippers(void)
  {
    flipper_control_msgs::msgs::FlipperControl flpmsg;
    char                                       _TopicName[100];
    // Already a robot has been spawned, then return
    if(1 != _parent.robot_was_spawned)
      return;
    // Prepare topic name for drive command
    sprintf(_TopicName, "~/%s/flp_cmd", _parent.own_name);
 // sprintf(_TopicName, "~/%s/vel_cmd", _parent.model_name);
    // Create a publisher on the ~/factory topic
    gazebo::transport::PublisherPtr _pub_flp_cmd
     = _parent._node->Advertise<flipper_control_msgs::msgs::FlipperControl>
                                                             (_TopicName);
    // Check angle value limitation
    _parent.flp_fr = _MIN(_parent.flp_fr, M_PI * 2); 
    _parent.flp_fr = _MAX(_parent.flp_fr, M_PI * -2);
    _parent.flp_fl = _MIN(_parent.flp_fl, M_PI * 2); 
    _parent.flp_fl = _MAX(_parent.flp_fl, M_PI * -2);
    _parent.flp_rr = _MIN(_parent.flp_rr, M_PI * 2); 
    _parent.flp_rr = _MAX(_parent.flp_rr, M_PI * -2);
    _parent.flp_rl = _MIN(_parent.flp_rl, M_PI * 2); 
    _parent.flp_rl = _MAX(_parent.flp_rl, M_PI * -2);
    flpmsg.set_fr(_parent.flp_fr);
    flpmsg.set_fl(_parent.flp_fl);
    flpmsg.set_rr(_parent.flp_rr);
    flpmsg.set_rl(_parent.flp_rl);
    _pub_flp_cmd->Publish(flpmsg);
  }
};

//////////////////////////////////////////////////////////////////
//  UC_GETSTARTPOSES
//   Defined on USARSim Manual P.36-45
struct UC_GETSTARTPOSES
{
  //////////////////////////////////////////////////////////////////
  //  Variables
  USARcommand& _parent;
  int          NumOfStartPoses;

  //////////////////////////////////////////////////////////////////
  //  The constructor
  UC_GETSTARTPOSES(USARcommand& parent) : _parent(parent)
  {
    int                    i;
    int                    nwritten;
    boost::asio::streambuf response;
    std::iostream          st_response(&response);
//  printf("%d", POP.Num_Of_StartPoses);
    st_response << "NFO {StartPoses " << POP.Num_Of_StartPoses << "}";
    for(i = 0; i < POP.Num_Of_StartPoses; i++)
      st_response << "{" << POP.StartPoses[i] << "}";
    st_response << "\r\n";
//  std::cout << st_response;
    nwritten = boost::asio::write(_parent._socket, response);
    if(0 >= nwritten)
    {
      perror("NFO message could not be written, check errno");
    }
/*  else if (nwritten != response.length()) {
      perror("not whole NFO message written");
    }*/
    // Should also convert the Quaternion spawn_direction.
    // Could StartPoses be defined in a Gazebo map?
    //  No, but they can be written as options of USARGazebo plugin in sdf.
  }
};

//////////////////////////////////////////////////////////////////
//  Send NFO            ## DO NOT MOVE THIS FUNCTION FROM HERE ##
void USARcommand::UC_send_NFO(USARcommand& parent)
{
  UC_GETSTARTPOSES UC_getstartposes(parent);
}

#define UC_GET_TYPE_FROM_REGISTERED_TOPIC(T,X) \
                           get_type_from_topic_name(T, sizeof(T),\
                          _parent.registered_topics_list.Search(X))
#define UC_GET_NAME_FROM_REGISTERED_TOPIC(T,X) \
                           get_name_from_topic_name(T, sizeof(T),\
                          _parent.registered_topics_list.Search(X))

#define UC_GET_TYPE_FROM_TOPIC_NAME(T,S) \
                           get_type_from_topic_name(T, sizeof(T),S)
#define UC_GET_NAME_FROM_TOPIC_NAME(T,S) \
                           get_name_from_topic_name(T, sizeof(T),S)

//////////////////////////////////////////////////////////////////
// UC_GETGEO
//  GETGEO is defined in the USARSim manual P.77
//  GEO is define in the USARSim manual P.58 - P.61
struct UC_GETGEO
{
  //////////////////////////////////////////////////////////////////
  //  Variables
  USARcommand& _parent;

  //////////////////////////////////////////////////////////////////
  //  The constructor
  UC_GETGEO(USARcommand& parent) : _parent(parent)
  {
    if(1 != _parent.robot_was_spawned)
      return;
    read_params_from_usar_command();
  }

  //////////////////////////////////////////////////////////////////
  //  GEO_set_effecters_params
  int GEO_set_effecters_params(std::iostream& st_response, 
                                             const char* _USARSim_name)
  {
    int         effecters;
    char        tmpbuf[100];
    int         flag_gps = 0;
    const char* topic_name;
    const char* Gazebo_name = get_Gazebo_name_of(_USARSim_name);
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    const char* effecter_name = BUCIP.Search("Name");
    st_response << "{Type " << _USARSim_name << "}";
    if(0 == strNcmp(Gazebo_name, ET_D_G_GPS))
      flag_gps = 1;
    if(NULL != effecter_name)
    {
      topic_name = _parent.registered_topics_list.Search(
                                            Gazebo_name, effecter_name);
      if(NULL == topic_name)
        return UCE_NO_EFFECTER;
      const char* dev_name = 
         ((flag_gps)?(UC_GET_TYPE_FROM_TOPIC_NAME(tmpbuf,topic_name)):
                     (UC_GET_NAME_FROM_TOPIC_NAME(tmpbuf,topic_name))); 
      st_response << 
        "{Name " << dev_name << "}" <<
        "{Location " << "0,0.1,0.1" << "}" <<
        "{Orientation " << "0,0.1,0.1" << "}" << 
        "{Mount " << _parent.own_name << "}";
      return UCE_GOOD;
    }
    for(int i = 0; 1; i++)
    {
      topic_name = _parent.registered_topics_list.Search_n(i,Gazebo_name);
      if(NULL == topic_name)
        if(0 == i)
          return UCE_NO_EFFECTER;
        else
          break;
      st_response << 
        "{Name " << 
         ((flag_gps)?(UC_GET_TYPE_FROM_TOPIC_NAME(tmpbuf,topic_name)):
                     (UC_GET_NAME_FROM_TOPIC_NAME(tmpbuf,topic_name)))<< 
        " Location " << "0,0.1,0.1" <<
        " Orientation " << "0,0.1,0.1" <<
        " Mount " << _parent.own_name << "}";
    }
    return UCE_GOOD;
  }

  //////////////////////////////////////////////////////////////////
  //  GEO_set_odometry_params
  int GEO_set_odometry_params(std::iostream& st_response, 
                                             const char* _USARSim_name)
  {
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    const char* effecter_name = BUCIP.Search("Name");
    st_response << "{Type " << _USARSim_name << "}";
    if(NULL != effecter_name)
    {
      st_response << 
        "{Name " << ET_D_G_ODOMETRY << "}" <<
        "{Location " << "0,0.1,0.1" << "}" <<
        "{Orientation " << "0,0.1,0.1" << "}" <<
        "{Mount " << _parent.own_name << "}";
    }
    else
    {
      st_response << 
        "{Name " << ET_D_G_ODOMETRY << 
        " Location " << "0,0.1,0.1" <<
        " Orientation " << "0,0.1,0.1" << 
        " Mount " << _parent.own_name << "}";
    }
    return UCE_GOOD;
  }

  //////////////////////////////////////////////////////////////////
  //  GEO_set_groundtruth_params
  int GEO_set_groundtruth_params(std::iostream& st_response, 
                                             const char* _USARSim_name)
  {
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    const char* effecter_name = BUCIP.Search("Name");
    st_response << "{Type " << _USARSim_name << "}";
    if(NULL != effecter_name)
    {
      st_response << 
        "{Name " << ET_D_G_GROUNDTRUTH << "}" << 
        "{Location " << "0,0.1,0.1" << "}" << 
        "{Orientation " << "0,0.1,0.1" << "}" << 
        "{Mount " << _parent.own_name << "}";
    }
    else
    {
      st_response << 
        "{Name " << ET_D_G_GROUNDTRUTH << 
        " Location " << "0,0.1,0.1" <<
        " Orientation " << "0,0.1,0.1" << 
        " Mount " << _parent.own_name << "}";
    }
    return UCE_GOOD;
  }

  //////////////////////////////////////////////////////////////////
  //  GEO_set_groundtruth_params
  int GEO_set_encoder_params(std::iostream& st_response, 
                                             const char* _USARSim_name)
  {
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    const char* effecter_name = BUCIP.Search("Name");
    st_response << "{Type " << _USARSim_name << "}";
    if(0 == strNcmp(effecter_name, "Lwheel"))
    {
      st_response << 
        "{Name " << "Lwheel" << "}" << 
        "{Location " << "0,0.1,0.1" << "}" << 
        "{Orientation " << "0,0.1,0.1" << "}" << 
        "{Mount " << _parent.own_name << "}";
    }
    else if(0 == strNcmp(effecter_name, "Rwheel"))
    {
      st_response << 
        "{Name " << "Rwheel" << "}" << 
        "{Location " << "0,0.1,0.1" << "}" << 
        "{Orientation " << "0,0.1,0.1" << "}" << 
        "{Mount " << _parent.own_name << "}";
    }
    else
    {
      st_response << 
        "{Name " << "Lwheel" << 
        " Location " << "0,0.1,0.1" <<
        " Orientation " << "0,0.1,0.1" << 
        " Mount " << _parent.own_name << "}" << 
        "{Name " << "Rwheel" << 
        " Location " << "0,0.1,0.1" <<
        " Orientation " << "0,0.1,0.1" << 
        " Mount " << _parent.own_name << "}";
    }
    return UCE_GOOD;
  }

  //////////////////////////////////////////////////////////////////
  //  read_params_from_usar_command
  int read_params_from_usar_command(void)
  {
    char*                          rtn;
    char                           tmpbuf[100];
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    boost::asio::streambuf         response;
    std::iostream                  st_response(&response);
    int                            nwritten;
//  BUCIP.Disp();
    if(BIE_GOOD != BUCIP.Error_code())
      return UCE_INCLUDING_BROKEN_BRACE;
    rtn = BUCIP.Search("Type");
    if(NULL != rtn)
    {
      st_response << "GEO ";
      if(NULL != strcasestr(rtn, ET_D_U_RFID))
        GEO_set_effecters_params(st_response, ET_D_U_RFID);
      else if(NULL != strcasestr(rtn, ET_D_U_CAMERA))
        GEO_set_effecters_params(st_response, ET_D_U_CAMERA);
      else if(NULL != strcasestr(rtn, ET_D_U_LASERSCANNER))
        GEO_set_effecters_params(st_response, ET_D_U_LASERSCANNER);
      else if(NULL != strcasestr(rtn, ET_D_U_GPS))
        GEO_set_effecters_params(st_response, ET_D_U_GPS);
      else if(NULL != strcasestr(rtn, ET_D_U_IMU))
        GEO_set_effecters_params(st_response, ET_D_U_IMU);
      else if(NULL != strcasestr(rtn, ET_D_U_ODOMETRY))
        GEO_set_odometry_params(st_response, ET_D_U_ODOMETRY);
      else if(NULL != strcasestr(rtn, ET_D_U_GROUNDTRUTH))
        GEO_set_groundtruth_params(st_response, ET_D_U_GROUNDTRUTH);
      else if(NULL != strcasestr(rtn, ET_D_U_ENCODER))
        GEO_set_encoder_params(st_response, ET_D_U_ENCODER);
      else if(NULL != strcasestr(rtn, "Robot"))
      {
        st_response << 
          "{Type " << get_type_of_robot(_parent.model_name) << "}" << 
          "{Name " << _parent.own_name << "}" <<
          "{Dimensions 0.7744,0.6318,0.5754}" << 
          "{COG 0.0000,0.0000,-0.1000}" <<
          "{WheelRadius 0.1922}" << 
          "{WheelSeparation 0.5120}" << 
          "{Wheelbase 0.3880}";
      }
      st_response << "\r\n";
      nwritten = boost::asio::write(_parent._socket, response);
      if(0 >= nwritten)
        perror("GEO message could not be written, check errno");
    }
    return UCE_GOOD;
  }
};

//////////////////////////////////////////////////////////////////
// UC_GETCONF
//  GETCONF is defined in the USARSim manual P.77
//  CONF is define in the USARSim manual P.61 - P.64
struct UC_GETCONF
{
  //////////////////////////////////////////////////////////////////
  //  Variables
  USARcommand& _parent;

  //////////////////////////////////////////////////////////////////
  //  The constructor
  UC_GETCONF(USARcommand& parent) : _parent(parent)
  {
    if(1 != _parent.robot_was_spawned)
      return;
    read_params_from_usar_command();
  }

  //////////////////////////////////////////////////////////////////
  //  CONF_set_effecters_params
  int CONF_set_effecters_params(std::iostream& st_response, 
                                             const char* _USARSim_name)
  {
    int         effecters;
    char        tmpbuf[100];
    int         flag_gps = 0;
    const char* topic_name;
    const char* Gazebo_name = get_Gazebo_name_of(_USARSim_name);
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    const char* effecter_name = BUCIP.Search("Name");
    st_response << "{Type " << _USARSim_name << "}";
    if(0 == strNcmp(Gazebo_name, ET_D_G_GPS))
      flag_gps = 1;
    if(NULL != effecter_name)
    {
      topic_name = _parent.registered_topics_list.Search(
                                            Gazebo_name, effecter_name);
      if(NULL == topic_name)
        return UCE_NO_EFFECTER;
      const char* dev_name = 
         ((flag_gps)?(UC_GET_TYPE_FROM_TOPIC_NAME(tmpbuf,topic_name)):
                     (UC_GET_NAME_FROM_TOPIC_NAME(tmpbuf,topic_name))); 
      st_response << "{Name " << dev_name << "}";
      if(0 == strNcmp(Gazebo_name, ET_D_G_LASERSCANNER))
      {
        st_response<<"{MaxRange 10.0}" <<
                     "{MinRange 0.08}" <<
                     "{Resolution 0.0175508}" <<
                     "{Fov 3.1416}" <<
                     "{Panning False}" <<
                     "{Tilting True}";
      }
      return UCE_GOOD;
    }
    for(int i = 0; 1; i++)
    {
      topic_name = _parent.registered_topics_list.Search_n(i,Gazebo_name);
      if(NULL == topic_name)
        if(0 == i)
          return UCE_NO_EFFECTER;
        else
          break;
      const char* dev_name = 
         ((flag_gps)?(UC_GET_TYPE_FROM_TOPIC_NAME(tmpbuf,topic_name)):
                     (UC_GET_NAME_FROM_TOPIC_NAME(tmpbuf,topic_name))); 
      st_response << "{Name " << dev_name << " ";
      if(0 == strNcmp(Gazebo_name, ET_D_G_LASERSCANNER))
      {
        st_response<<" MaxRange 20.0" <<
                     " MinRange 0.08" <<
                     " Resolution 0.0175508"<<
                     " Fov 3.1416" <<
                     " Panning False" <<
                     " Tilting True}";
      }
    }
    return UCE_GOOD;
  }

  //////////////////////////////////////////////////////////////////
  //  CONF_set_odometry_params
  int CONF_set_odometry_params(std::iostream& st_response, 
                                             const char* _USARSim_name)
  {
    st_response << "{Type " << _USARSim_name << "}";
    st_response << 
      "{Name " << ET_D_G_ODOMETRY << 
//      " Location " << "0,0.1,0.1" <<
//      " Orientation " << "0,0.1,0.1" << 
//      " Mount " << _parent.own_name << 
      "}";
    return UCE_GOOD;
  }

  //////////////////////////////////////////////////////////////////
  //  CONF_set_groundtruth_params
  int CONF_set_groundtruth_params(std::iostream& st_response, 
                                             const char* _USARSim_name)
  {
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    const char* effecter_name = BUCIP.Search("Name");
    st_response << "{Type " << _USARSim_name << "}";
    if(NULL != effecter_name)
    {
      st_response << "{Name GndTruth}" <<
                     "{ScanInterval 0.1000}";
    }
    else
    {
      st_response << "{Name GndTruth" <<
                     " ScanInterval 0.1000}";
    }
    return UCE_GOOD;
  }

  //////////////////////////////////////////////////////////////////
  //  CONF_set_encoder_params
  int CONF_set_encoder_params(std::iostream& st_response, 
                                             const char* _USARSim_name)
  {
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    const char* effecter_name = BUCIP.Search("Name");
    st_response << "{Type " << _USARSim_name << "}";
    if(0 == strNcmp(effecter_name, "Lwheel"))
    {
      st_response << "{Name Lwheel}" <<
                     "{ScanInterval 1}";
    }
    else if(0 == strNcmp(effecter_name, "Rwheel"))
    {
      st_response << "{Name Rwheel}" <<
                     "{ScanInterval 1}";
    }
    else
    {
      st_response << "{Name Rwheel" <<
                     " ScanInterval 1}" << 
                     "{Name Lwheel" <<
                     " ScanInterval 1}";
    }
    return UCE_GOOD;
  }

  //////////////////////////////////////////////////////////////////
  //  read_params_from_usar_command
  int read_params_from_usar_command(void)
  {
    char*                          rtn;
    char                           tmpbuf[100];
    Break_USAR_Command_Into_Params BUCIP(_parent.ucbuf);
    boost::asio::streambuf         response;
    std::iostream                  st_response(&response);
    int                            nwritten;
//  BUCIP.Disp();
    if(BIE_GOOD != BUCIP.Error_code())
      return UCE_INCLUDING_BROKEN_BRACE;
    rtn = BUCIP.Search("Type");
    if(NULL != rtn)
    {
      st_response << "CONF ";
      if(NULL != strcasestr(rtn, ET_D_U_RFID))
        CONF_set_effecters_params(st_response, ET_D_U_RFID);
      else if(NULL != strcasestr(rtn, ET_D_U_CAMERA))
        CONF_set_effecters_params(st_response, ET_D_U_CAMERA);
      else if(NULL != strcasestr(rtn, ET_D_U_LASERSCANNER))
        CONF_set_effecters_params(st_response, ET_D_U_LASERSCANNER);
      else if(NULL != strcasestr(rtn, ET_D_U_GPS))
        CONF_set_effecters_params(st_response, ET_D_U_GPS);
      else if(NULL != strcasestr(rtn, ET_D_U_IMU))
        CONF_set_effecters_params(st_response, ET_D_U_IMU);
      else if(NULL != strcasestr(rtn, ET_D_U_ODOMETRY))
        CONF_set_odometry_params(st_response, ET_D_U_ODOMETRY);
      else if(NULL != strcasestr(rtn, ET_D_U_GROUNDTRUTH))
        CONF_set_groundtruth_params(st_response, ET_D_U_GROUNDTRUTH);
      else if(NULL != strcasestr(rtn, ET_D_U_ENCODER))
        CONF_set_encoder_params(st_response, ET_D_U_ENCODER);
      else if(NULL != strcasestr(rtn, "Robot"))
      {
        if(NULL != strcasestr(get_type_of_robot(_parent.model_name),
                                                 RT_D_GROUNDVEHICLE))
        {
          st_response << 
            "{Type " << get_type_of_robot(_parent.model_name) << "}" << 
            "{Name " << _parent.own_name << "}" <<
            "{SteeringType " << 
               get_steeringtype_of_robot(_parent.model_name) << "}" << 
            "{Mass " << 
               get_mass_of_robot(_parent.model_name) << "}" << 
            "{MaxSpeed " << 
               get_maxspeed_of_robot(_parent.model_name) << "}" << 
            "{MaxTorque " << 
               get_maxtorque_of_robot(_parent.model_name) << "}" << 
            "{MaxFrontSteer " << 
               get_maxfrontsteer_of_robot(_parent.model_name) << "}" << 
            "{MaxRearSteer " << 
               get_maxrearsteer_of_robot(_parent.model_name) << "}";
        }
      }
      st_response << "\r\n";
      nwritten = boost::asio::write(_parent._socket, response);
      if(0 >= nwritten)
        perror("GEO message could not be written, check errno");
    }
    return UCE_GOOD;
  }
};

//////////////////////////////////////////////////////////////////
//  USAR Command fetch    ## DO NOT MOVE THIS FUNCTION FROM HERE ##
void USARcommand::UC_check_command_from_USARclient(void)
{
  int nread;
  // error routine : anyone write this with boost....
  boost::system::error_code err;
  // Get an USAR command string
  nread = boost::asio::read_until(_socket, _buffer, "\r\n", err);
  // DO NOT USE _buffer, including just reading _buffer.
  //  Ex) After reading _buffer, values were erased in _buffer.
//std::cout << "Child_Session_Loop address [" << this << "]" << std::endl;
//std::cout << "nread = " << nread << std::endl;
  if(0 > nread)
  {
    perror("Could not read socket 3000");
    _parent.Remove_Child_Session(this);
    _thread.join();
  } 
  else if(0 == nread) 
  {
    perror("EOF, should break connection");
    perror("Turn this session into sleep to avoid decreasing performance.\n");
    for(;;)
      usleep(1000000);
//    _parent.Remove_Child_Session(this);
//    _thread.join();
  }
  std::istream is(&_buffer);
  std::string line;
  for(;1;)
  {
    // Copy 1 line by 1 line from _buffer into "line"
    //  to keep the USAR command string values
//  line.getline(is);
    std::getline(is, line);
    if(0 < line.length())
    {
      if(NULL != ucbuf)
        delete ucbuf;
      ucbuf = new char[line.length()+2];
      strcpy(ucbuf, (char*)line.c_str());
      // Display DEBUG information
      printf("Received command string = %s\n", ucbuf);
      // Call each USAR command programs
      if(0 == strNcmp(ucbuf, "INIT"))
        UC_INIT UC_init(*this);
      else if(0 == strNcmp(ucbuf, "DRIVE"))
        UC_DRIVE UC_drive(*this);
      else if(0 == strNcmp(ucbuf, "MULTIDRIVE"))
        UC_MULTIDRIVE UC_multidrive(*this);
      else if(0 == strNcmp(ucbuf, "GETSTARTPOSES"))
        UC_GETSTARTPOSES UC_getstartposes(*this);
      else if(0 == strNcmp(ucbuf, "GETGEO"))
        UC_GETGEO UC_getgeo(*this);
      else if(0 == strNcmp(ucbuf, "GETCONF"))
        UC_GETCONF UC_getconf(*this);
      delete ucbuf;
      ucbuf = NULL;
    }
    else
      break;
  }
}

//#######################################################################
//  Image Server
//#######################################################################

#define MAX_CAMERA_NUM 30
#define MAX_CAMERA_WINS 4
#define CAM_IMG_WIDTH 320
#define CAM_IMG_HEIGHT 240
#define LINESIZE (3*CAM_IMG_WIDTH)
#include <jpeglib.h>
#include <string.h>

#define CCO camera_connected_order

#define NO__CAMERA_IMAGE_TEST__

//////////////////////////////////////////////////////////////////
// USARimage : A structure for transferring image data
struct USARimage
{
  //////////////////////////////////////////////////////////////////
  // USARimage.Variables
  Server_Framework<USARimage>     &_parent;
  boost::asio::io_service          _ioservice;
  boost::asio::ip::tcp::socket     _socket;
  boost::asio::streambuf           _buffer;
  boost::thread                    _thread;
  gazebo::transport::NodePtr       _node;
  gazebo::transport::SubscriberPtr _sub_camera_image[MAX_CAMERA_NUM];
  // Add your own variables here
//  int  flag_OK, flag_U, flag_SS;
//  char model_name[200], own_name[200], topic_camera[300];
//  void send_full_size_image(ConstImageStampedPtr& _msg);
//  void imageserver_callback(ConstImageStampedPtr& _msg);
  void camera_callback_0(ConstImageStampedPtr& _msg);
  void camera_callback_1(ConstImageStampedPtr& _msg);
  void camera_callback_2(ConstImageStampedPtr& _msg);
  void camera_callback_3(ConstImageStampedPtr& _msg);
  void camera_callback_4(ConstImageStampedPtr& _msg);
  void camera_callback_5(ConstImageStampedPtr& _msg);
  void camera_callback_6(ConstImageStampedPtr& _msg);
  void camera_callback_7(ConstImageStampedPtr& _msg);
  void camera_callback_8(ConstImageStampedPtr& _msg);
  void camera_callback_9(ConstImageStampedPtr& _msg);
  void camera_callback_10(ConstImageStampedPtr& _msg);
  void camera_callback_11(ConstImageStampedPtr& _msg);
  void camera_callback_12(ConstImageStampedPtr& _msg);
  void camera_callback_13(ConstImageStampedPtr& _msg);
  void camera_callback_14(ConstImageStampedPtr& _msg);
  void camera_callback_15(ConstImageStampedPtr& _msg);
  void camera_callback_16(ConstImageStampedPtr& _msg);
  void camera_callback_17(ConstImageStampedPtr& _msg);
  void camera_callback_18(ConstImageStampedPtr& _msg);
  void camera_callback_19(ConstImageStampedPtr& _msg);
  void camera_callback_20(ConstImageStampedPtr& _msg);
  void camera_callback_21(ConstImageStampedPtr& _msg);
  void camera_callback_22(ConstImageStampedPtr& _msg);
  void camera_callback_23(ConstImageStampedPtr& _msg);
  void camera_callback_24(ConstImageStampedPtr& _msg);
  void camera_callback_25(ConstImageStampedPtr& _msg);
  void camera_callback_26(ConstImageStampedPtr& _msg);
  void camera_callback_27(ConstImageStampedPtr& _msg);
  void camera_callback_28(ConstImageStampedPtr& _msg);
  void camera_callback_29(ConstImageStampedPtr& _msg);
  void camera_callback_30(ConstImageStampedPtr& _msg);
  int camera_connected_order[MAX_CAMERA_NUM];
  int current_camera_order;
  JSAMPARRAY img;
//  TopicsList topics_list;

  //////////////////////////////////////////////////////////////////
  // Initialize some things..
  void Init(void)
  {
    int i;
    // Initiarize camera callback function parameters
    current_camera_order = 0;
    for(i = 0; i < MAX_CAMERA_NUM; i++)
      CCO[i] = -1;
    // Get Camera Image Array Memory
    img = (JSAMPARRAY) new JSAMPROW[CAM_IMG_HEIGHT];
    for(i = 0; i < CAM_IMG_HEIGHT; i++)
      img[i] = (JSAMPROW) new JSAMPLE[3*CAM_IMG_WIDTH*MAX_CAMERA_WINS];
  }

  //////////////////////////////////////////////////////////////////
  // USARimage.Constructor
  USARimage(Server_Framework<USARimage>&parent)
    : _parent(parent), _socket(_ioservice)
//     , flag_OK(0), flag_U(0), flag_SS(0)
//      , topics_list()
  { Init(); }

  //////////////////////////////////////////////////////////////////
  // USARimage.Destructor
  ~USARimage()
  {
    // Release Camera Image Array Memory
    for(int i = 0; i < CAM_IMG_HEIGHT; i++)
      delete img[i];
    delete img;
  }

  //////////////////////////////////////////////////////////////////
  // Send combined camera images to client in jpeg
  void send_camera_image_to_socket(void)
  {
    unsigned char *outbuffer_pt = NULL;
    unsigned long int outlen = 0;
    struct jpeg_compress_struct cinfo;
    struct jpeg_error_mgr jerr;
    cinfo.err = jpeg_std_error(&jerr);
    jpeg_create_compress(&cinfo);
    jpeg_mem_dest(&cinfo, &outbuffer_pt, &outlen);
//  jpeg_stdio_dest(&cinfo, fp);
    cinfo.image_width  = CAM_IMG_WIDTH * MAX_CAMERA_WINS;
    cinfo.image_height = CAM_IMG_HEIGHT;
    cinfo.input_components = 3;
    cinfo.in_color_space = JCS_RGB;
    jpeg_set_defaults(&cinfo);
    jpeg_set_quality(&cinfo, 75, TRUE);
    jpeg_start_compress(&cinfo, TRUE);
    jpeg_write_scanlines(&cinfo, img, CAM_IMG_HEIGHT);
    jpeg_finish_compress(&cinfo);
    jpeg_destroy_compress(&cinfo);
#ifdef __CAMERA_IMAGE_TEST__
    const char *filename = "test_jpeg_output.jpg";
    FILE *fp = fopen(filename, "wb");
    if (fp == NULL) {
        fprintf(stderr, "cannot open %s\n", filename);
        exit(EXIT_FAILURE);
    }
    fwrite((const void*)outbuffer_pt, outlen, 1, fp);
    fclose(fp);
#else
    unsigned char* ImageBuf = new unsigned char[5 + outlen];
    ImageBuf[0] = 3; // Jpeg Data with normal image quality
    ImageBuf[1] = (unsigned char)((outlen >> 24) & (unsigned long)0x00FF);
    ImageBuf[2] = (unsigned char)((outlen >> 16) & (unsigned long)0x00FF);
    ImageBuf[3] = (unsigned char)((outlen >>  8) & (unsigned long)0x00FF);
    ImageBuf[4] = (unsigned char)(outlen         & (unsigned long)0x00FF);
    memcpy(&ImageBuf[5], outbuffer_pt, outlen);
    boost::asio::write(_socket
                 , boost::asio::buffer((void*)ImageBuf, 5 + outlen));
    delete ImageBuf;
#endif
    if(NULL != outbuffer_pt)
    {
      free(outbuffer_pt);
    }
  }

  //////////////////////////////////////////////////////////////////
  // Save camera image to the combination camera image buffer
  void save_camera_image(int cam, ConstImageStampedPtr& _msg)
  {
    unsigned char* ip = (unsigned char*)_msg->image().data().c_str();
    if(-1 == CCO[cam])
    {
      if(MAX_CAMERA_WINS > current_camera_order)
        CCO[cam] = current_camera_order++;
      else
        return;
    }
    for(int i = 0; i < CAM_IMG_HEIGHT; i++)
      memcpy(&img[i][CCO[cam]*LINESIZE], &ip[i*LINESIZE], LINESIZE);
  }

  //////////////////////////////////////////////////////////////////
  // USARimage.Child_Session_Loop_Core
  void Child_Session_Loop_Core(void)
  {
    boost::system::error_code err;
    boost::asio::read_until(_socket, _buffer , "\r\n", err);
  // DO NOT USE _buffer, including just reading _buffer.
  //  Ex) After reading _buffer, values were erased in _buffer.
//  std::cout << "Child_Session_Loop_Core address ["<<this<<"]"<<std::endl;
    // OK or U command should be checked here.
    //  U is underconstruction, not working.
    // SS is for debug.
    std::istream is(&_buffer);
    std::string  line;
    for(;1;)
    {
      std::getline(is, line);
      if(0 < line.length())
      {
        if(0 == strNcmp(line.c_str(), "OK"))
        {
          send_camera_image_to_socket();
        }
          //flag_OK = 1;
   //     else if(0 == strNcmp(line.c_str(), "U["))
          //flag_U = 1;
    //    else if(0 == strNcmp(line.c_str(), "SS"))
          //flag_SS = 1;
      }
      else
        break;
    }
  }

  //////////////////////////////////////////////////////////////////
  // USARimage.Accept_Process
  void Accept_Process(void)
  {
    _node = gazebo::transport::NodePtr(new gazebo::transport::Node());
    _node->Init();
//printf("Image Server was connected with your client\n");
    // The function Subscribe needs a valiable which be assigned 
    //  a return value from the function Subscribe. Without the assigning,
    //   a call-back function  which was registered 
    //    by the function Subscribe will NOT be call-backed.
    // DO NOT REMOVE "_sub_camera_image[?] ="
    _sub_camera_image[0] = _node->Subscribe(
      "/gazebo/default/pioneer3at_with_sensors/camera/link/camera/image",
       &USARimage::camera_callback_0, this);
    _sub_camera_image[1] = _node->Subscribe(
      "/gazebo/default/pioneer3at_with_sensors_b/camera/link/camera/image",
       &USARimage::camera_callback_1, this);
    _sub_camera_image[2] = _node->Subscribe(
      "/gazebo/default/pioneer3at_with_sensors_g/camera/link/camera/image",
       &USARimage::camera_callback_2, this);
    _sub_camera_image[3] = _node->Subscribe(
      "/gazebo/default/pioneer3at_with_sensors_y/camera/link/camera/image",
       &USARimage::camera_callback_3, this);
    _sub_camera_image[4] = _node->Subscribe(
      "/gazebo/default/pioneer3at_with_sensors_r/camera/link/camera/image",
       &USARimage::camera_callback_4, this);
    _sub_camera_image[5] = _node->Subscribe(
      "/gazebo/default/pioneer2dx_with_sensors_r/camera/link/camera/image",
       &USARimage::camera_callback_5, this);
    _sub_camera_image[6] = _node->Subscribe(
      "/gazebo/default/pioneer2dx_with_sensors_g/camera/link/camera/image",
       &USARimage::camera_callback_6, this);
    _sub_camera_image[7] = _node->Subscribe(
      "/gazebo/default/pioneer2dx_with_sensors_b/camera/link/camera/image",
       &USARimage::camera_callback_7, this);
    _sub_camera_image[8] = _node->Subscribe(
      "/gazebo/default/pioneer2dx_with_sensors_y/camera/link/camera/image",
       &USARimage::camera_callback_8, this);
    _sub_camera_image[9] = _node->Subscribe(
      "/gazebo/default/turtlebot_with_sensors_r/camera/link/camera/image",
       &USARimage::camera_callback_9, this);
    _sub_camera_image[10] = _node->Subscribe(
      "/gazebo/default/turtlebot_with_sensors_g/camera/link/camera/image",
       &USARimage::camera_callback_10, this);
    _sub_camera_image[11] = _node->Subscribe(
      "/gazebo/default/turtlebot_with_sensors_b/camera/link/camera/image",
       &USARimage::camera_callback_11, this);
    _sub_camera_image[12] = _node->Subscribe(
      "/gazebo/default/turtlebot_with_sensors_y/camera/link/camera/image",
       &USARimage::camera_callback_12, this);
    _sub_camera_image[13] = _node->Subscribe(
      "/gazebo/default/crawler_robot_r/camera/link/camera/image",
       &USARimage::camera_callback_12, this);
    _sub_camera_image[14] = _node->Subscribe(
      "/gazebo/default/crawler_robot_g/camera/link/camera/image",
       &USARimage::camera_callback_12, this);
    _sub_camera_image[15] = _node->Subscribe(
      "/gazebo/default/crawler_robot_b/camera/link/camera/image",
       &USARimage::camera_callback_12, this);
    _sub_camera_image[16] = _node->Subscribe(
      "/gazebo/default/crawler_robot_y/camera/link/camera/image",
       &USARimage::camera_callback_12, this);
    while(1)
      Child_Session_Loop_Core();
    // Set topic name of camera
    // Now the camera topic name is set as : 
    // "~/pioneer3at_with_sensors/chassis/r_camera/image" .
    // You can change camera by each child session, 
    //  if we could change the hand-shaking rule of sending 
    //   cameara image data.
    // After connecting, the camera topic name can be tranfered 
    //  by codes like following, but it's a future protocol.
    /*
    boost::system::error_code err;
    boost::asio::read_until(_socket, _buffer, "\r\n", err);
    std::istream is(&_buffer);
    std::string  line;
    for(;1;)
    {
      std::getline(is, line);
      if(0 < line.length())
      {
        if(0 == strNcmp(line.c_str(), "TP"))
        sprintf(camera_topic_name, "TP %s", line.c_str());
      }
      else
        break;
    }
    */
    // Now we can not get camera topic name by current hand-shaking rule
    // , then I wrote following line instead.
/* Sample Code for using topics list to decide cameara topic name automatically
    if(NULL == topics_list.Search((char*)"image"))
    {
      topics_list.Get_Topics_List();
      topics_list.Filter(own_name);
      if(NULL == topics_list.Search((char*)"image"))
        return;
    }
    sprintf(camera_topic_name, "%s"
                      , topics_list.Search((char*)"image"));
    */
    /*
    sprintf(topic_camera, "%s"
                     , "~/pioneer3at_with_sensors/chassis/r_camera/image");
    _sub_camera_image 
      = _node->Subscribe(topic_camera, &USARimage::imageserver_callback
                                                                  , this);
    */
  }

};

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_0(ConstImageStampedPtr& _msg)
{
  save_camera_image(0, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_1(ConstImageStampedPtr& _msg)
{
  save_camera_image(1, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_2(ConstImageStampedPtr& _msg)
{
  save_camera_image(2, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_3(ConstImageStampedPtr& _msg)
{
  save_camera_image(3, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_4(ConstImageStampedPtr& _msg)
{
  save_camera_image(4, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_5(ConstImageStampedPtr& _msg)
{
  save_camera_image(5, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_6(ConstImageStampedPtr& _msg)
{
  save_camera_image(6, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_7(ConstImageStampedPtr& _msg)
{
  save_camera_image(7, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_8(ConstImageStampedPtr& _msg)
{
  save_camera_image(8, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_9(ConstImageStampedPtr& _msg)
{
  save_camera_image(9, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_10(ConstImageStampedPtr& _msg)
{
  save_camera_image(10, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_11(ConstImageStampedPtr& _msg)
{
  save_camera_image(11, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_12(ConstImageStampedPtr& _msg)
{
  save_camera_image(12, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_13(ConstImageStampedPtr& _msg)
{
  save_camera_image(13, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_14(ConstImageStampedPtr& _msg)
{
  save_camera_image(14, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_15(ConstImageStampedPtr& _msg)
{
  save_camera_image(15, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_16(ConstImageStampedPtr& _msg)
{
  save_camera_image(16, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_17(ConstImageStampedPtr& _msg)
{
  save_camera_image(17, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_18(ConstImageStampedPtr& _msg)
{
  save_camera_image(18, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_19(ConstImageStampedPtr& _msg)
{
  save_camera_image(19, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_20(ConstImageStampedPtr& _msg)
{
  save_camera_image(20, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_21(ConstImageStampedPtr& _msg)
{
  save_camera_image(21, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_22(ConstImageStampedPtr& _msg)
{
  save_camera_image(22, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_23(ConstImageStampedPtr& _msg)
{
  save_camera_image(23, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_24(ConstImageStampedPtr& _msg)
{
  save_camera_image(24, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_25(ConstImageStampedPtr& _msg)
{
  save_camera_image(25, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_26(ConstImageStampedPtr& _msg)
{
  save_camera_image(26, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_27(ConstImageStampedPtr& _msg)
{
  save_camera_image(27, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_28(ConstImageStampedPtr& _msg)
{
  save_camera_image(28, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_29(ConstImageStampedPtr& _msg)
{
  save_camera_image(29, _msg);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::camera_callback_30(ConstImageStampedPtr& _msg)
{
  save_camera_image(30, _msg);
}

#ifdef __DEBUG_SAMPLE__
//////////////////////////////////////////////////////////////////
// SaveAsPPM saves an image for debug
//  See following page about ConstImageStampedPtr
//  https://osrf-distributions.s3.amazonaws.com/gazebo/api/dev/classgazebo_1_1gui_1_1ImageView.html#a7c1d7a627bb39bfd216b1bef4e45035d
void  SaveAsPPM(char* filename, ConstImageStampedPtr &_msg)
{
  unsigned char* ip = (unsigned char*)_msg->image().data().c_str();
  FILE* fp = fopen(filename, "wt");
  fprintf(fp, "P3\n%d %d\n255\n", _msg->image().width()
                                               , _msg->image().height());
  for(int i=0;i<_msg->image().width()*_msg->image().height();i++)
  fprintf(fp, "%3d %3d %3d\n", ip[i*3], ip[i*3+1], ip[i*3+2]);
  fclose(fp);
}

//////////////////////////////////////////////////////////////////
// Function is called everytime a message is received on topics
void USARimage::imageserver_callback(ConstImageStampedPtr& _msg)
{
  if(flag_OK)
    send_full_size_image(_msg);
  else if(flag_U)
    send_rectangle_area_image( _msg);
  else if(flag_SS)
  {
    flag_SS = 0;
    SaveAsPPM((char*)"/tmp/GazeboCameraImage.ppm", _msg);
  }
}

//////////////////////////////////////////////////////////////////
// Send camera image
void USARimage::send_full_size_image(ConstImageStampedPtr& _msg)
{
  flag_OK = 0;
  int ImageSize = 3 * _msg->image().width() * _msg->image().height();
  unsigned char* ip = (unsigned char*)_msg->image().data().c_str();
  unsigned char* ImageBuf = new unsigned char[5 + ImageSize];
    // Set Image format type <= 0
  ImageBuf[0] = 0; // Raw Data
    // Set imagesize
  *(unsigned long int*)&ImageBuf[1] = ImageSize; // Image Size
    // Repacking into USARImage Raw Format
  for(int i = 0; i < _msg->image().width() * _msg->image().height(); i++)
  {
    ImageBuf[5+i*3+0] /*Blue */ = ip[i*3+2]/*Blue*/;
    ImageBuf[5+i*3+1] /*Red  */ = ip[i*3+0]/*Red*/;
    ImageBuf[5+i*3+2] /*Green*/ = ip[i*3+1]/*Green*/;
  }
    // Send USAR Image Data
  boost::asio::write(_socket, boost::asio::buffer((void*)ImageBuf
                     , 5 + ImageSize));
    // Free ImageBuf
  delete ImageBuf;
printf("Sent a full size image in raw data, size = %d\n", 5+ImageSize);
/* For checking to be able to get an image data
  static int filenumber=0;
  char filename[100];
  if(filenumber>10)
    return;
  sprintf(filename, "./tmp%02d.PPM", (filenumber++)%10);
  SaveAsPPM(filename, _msg);
*/
}
#endif

//#######################################################################
//  World Plugin
//#######################################################################

  class USARGazebo : public WorldPlugin
  {

  /////////////////////////////////////////////
  /// Constructor
  public: USARGazebo(void):UCp(NULL),UIp(NULL) { }

  /////////////////////////////////////////////
  /// Destructor
  virtual ~USARGazebo()
  {
    if(NULL != UCp)
      delete UCp;
    if(NULL != UIp)
      delete UIp;
  }

  ///////////////////////////////////////////////
  // Load the robocup rescue plugin
  public: void Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
  {
    world = _parent;
    Init();
    POP.Read_Option_Parameters(_sdf);
  }

  /////////////////////////////////////////////
  // Called once after Load
  private: void Init()
  {
    if(NULL == UCp)
      UCp = new Server_Framework<USARcommand>(3000, world);
    if(NULL == UIp)
      UIp = new Server_Framework<USARimage>(5003, world);
  }

private:
  /// Keep a pointer to the world.
  gazebo::physics::WorldPtr world;
public:
  Server_Framework<USARcommand> *UCp;
  Server_Framework<USARimage>   *UIp;
  };
  // Register this plugin with the simulator
  GZ_REGISTER_WORLD_PLUGIN(USARGazebo)
}
