client2dview.c

robocup rcssmonitor-11.1.1.zip

class RobotFreiburg: public Robot {
public:
  Vector2d pos;
  Angle ang;
  static const double size= 1.25;
  void print_skin(std::ostream & out) {
    out << "INS FRAME id=" << frame << ";"
	<< "INS " << frame << " POLYGON fil=1 col=000099 "
	<< "(" << size * -0.145 << "," << size * 0.175 << ")"
	<< "(" << size *  0.145 << "," << size * 0.175 << ")"
	<< "(" << size *  0.160 << "," << size * 0.160 << ")"
	<< "(" << size *  0.160 << "," << size * 0.015 << ")"
	<< "(" << size *  0.145 << "," << size * 0.0 << ")"
	<< "(" << size *  0.145 << "," << size * -0.175 << ")"
	<< "(" << size *  -0.145 << "," << size * -0.175 << ")"
	<< "(" << size *  -0.145 << "," << size * 0.0 << ")"
	<< "(" << size *  -0.160 << "," << size * 0.015 << ")"
	<< "(" << size *  -0.160 << "," << size * 0.160 << ");"
      //	<< "(" << size *  -0.145 << "," << size * 0.175 << ")"
	<< "INS " << frame << " POLYGON fil=1 col=000000 "
	<< "(" << size *  -0.130 << "," << size * -0.005 << ")"
	<< "(" << size *   0.130 << "," << size * -0.005 << ")"
	<< "(" << size *   0.130 << "," << size *  0.155 << ")"
	<< "(" << size *  -0.130 << "," << size *  0.155 << ");"
      //kicker
	<< "INS " << frame << " FRAME id=" << frame*10 << " lay=1 (0,0.175);"
	<< "INS " << frame*10 << "0 POLYGON fil=1 col=000000 "
	<< "(" << size *  -0.125 << "," << size * 0.0 << ")"
	<< "(" << size *   0.125 << "," << size * 0.0 << ")"
	<< "(" << size *   0.125 << "," << size * 0.020 << ")"
	<< "(" << size *  -0.125 << "," << size * 0.020 << ");";
    print_pos(out);
  }
  void print_pos(std::ostream & out) {
    out << "MOV " << frame << " (" << pos.x << "," << pos.y << "," << ang.get_value()-M_PI*0.5 << ");";
  }
  void read_joystick(int num_axes, int * axis) {
    const double scale= 0.1;
    Vector2d tmp;
    tmp.init_polar(1,ang.get_value());
    pos+= scale * double(-axis[1])/32767.0 * tmp;
    ang+= scale * double(-axis[3])/ 32767.0;
  }
};

class RobotGolem: public Robot {
  void rotate(std::ostream & out,double r,double angle, int num, Vector2d  v0, Vector2d  v1, Vector2d  v2=Vector2d(0,0), Vector2d  v3=Vector2d(0,0)) {
    if (num>0) {
      v0.rotate(angle);
      v0= r*v0;
      out << "("<< v0.x << ","<< v0.y<<")";
    }
    if (num>1) {
      v1.rotate(angle);
      v1= r*v1;
      out << "("<< v1.x << ","<< v1.y<<")";
    }
    if (num>2) {
      v2.rotate(angle);
      v2= r*v2;
      out << "("<< v2.x << ","<< v2.y<<")";
    }
    if (num>3) {
      v3.rotate(angle);
      v3= r*v3;
      out << "("<< v3.x << ","<< v3.y<<")";
    }
  }
public:
  Vector2d pos;
  Angle ang;
  void print_skin(std::ostream & out) {
    double factor= 0.2;
    out << "\nINS FRAME id=" << frame << " (" << pos.x << "," << pos.y << "," << ang.get_value() << ");";
    out << "\nINS " << frame << " POLYGON fil=1 col=666666 ";
    Vector2d vec1L(-0.25,-1), vec1R(0.25,-1);
    rotate(out, factor, 0, 2, vec1L, vec1R);
    rotate(out, factor, M_PI * 2.0/3.0, 2, vec1L, vec1R);
    rotate(out, factor, -M_PI * 2.0/3.0, 2, vec1L, vec1R);
    Vector2d v0(-0.25,-1.1), v1(0.25,-1.1), v2( 0.25,-1.3), v3(-0.25,-1.3);
    out << ";";
    out << "\nINS " << frame << " POLYGON fil=1 col=000000 ";
    rotate(out, factor, 0, 4, v0,v1,v2,v3);
    out << ";";
    out << "\nINS " << frame << " POLYGON fil=1 col=000000 ";
    rotate(out, factor, M_PI * 2.0/3.0, 4, v0,v1,v2,v3);
    out << ";";
    out << "\nINS " << frame << " POLYGON fil=1 col=000000 ";
    rotate(out, factor, -M_PI * 2.0/3.0, 4, v0,v1,v2,v3);
    out << ";";
  }
  void print_pos(std::ostream & out) {
    out << "MOV " << frame << " (" << pos.x << "," << pos.y << "," << ang.get_value() << ");";
  }
  void read_joystick(int num_axes, int * axis) {
    const double scale= 0.1;
    pos.x+=  scale * double(axis[0])/ 32767.0;
    pos.y-=  scale * double(axis[1])/ 32767.0;
    ang+= scale * double(-axis[3])/ 32767.0;
  }
};

class Joystick {
public:
  int joystick_fd;
  int num_axes;
  int num_buttons;
  int version;
  char name[MAX_NAME_LEN];
  int * button;
  int * axis;
  bool * button_chg;
  bool * axis_chg;

  Joystick() {
    joystick_fd= 0;
    num_axes= 0;
    num_buttons= 0;
    version= 0;
    button= 0;
    axis= 0;
    button_chg= 0;
    axis_chg= 0;
  }

  bool init(const char * joydev) {
    if ((joystick_fd = open(joydev, O_RDONLY)) < 0) {
      ERROR_OUT << "\ncould't open joystic device " << joydev;
      return false;
    }
    fcntl(joystick_fd, F_SETFL, O_NONBLOCK); //don't block

    unsigned char num;
    ioctl(joystick_fd, JSIOCGVERSION, &version);
    ioctl(joystick_fd, JSIOCGAXES, &num);
    num_axes= num;
    ioctl(joystick_fd, JSIOCGBUTTONS, &num);
    num_buttons= num;
    ioctl(joystick_fd, JSIOCGNAME(MAX_NAME_LEN), name);
    std::cout << "\nJoystick    = " << name
              << "\nversion     = " << version
              << "\nnum_axes    = " << num_axes
              << "\nnum_buttons = " << num_buttons
              << std::flush;

    button= new int[num_buttons];
    button_chg= new bool[num_buttons];
    axis= new int[num_axes];
    axis_chg= new bool[num_axes];

    for (int i=0; i<num_buttons; i++) {
      button[i]= 0;
      button_chg[i]= 0;
    }
    for (int i=0; i<num_axes; i++) {
      axis[i]= 0;
      axis_chg[i]= 0;
    }
    return true;
  }

  int read_all_events() {
    int got_data= 0;
    for (int i=0; i<num_buttons; i++)
      button_chg[i]= false;
    for (int i=0; i<num_axes; i++)
      axis_chg[i]= false;
    js_event js;

    while(1) { //read all joystick events
      int size= read(joystick_fd, &js, sizeof(struct js_event));
      if (size != sizeof(struct js_event))
	return got_data;

      got_data++;
      switch(js.type & ~JS_EVENT_INIT) {
      case JS_EVENT_BUTTON:
	button[js.number]= js.value;
	button_chg[js.number]= true;
	break;
      case JS_EVENT_AXIS:
	axis[js.number] = js.value;
	axis_chg[js.number]= true;
	break;
      }
    }
  }
};

bool joy_input(UDPsocket & sock) {
#ifdef HAVE_SSTREAM
  std::ostringstream out;
#else
  std::ostrstream out;
#endif

  //init field
  out << "EMP 0; VA (0,0,11,6); BG 009900;"
    //goals
      << "\nINS POLYGON lay=-1 col=ffcc00 fil= 1 (-5.25,-1)(-4.5,-1)(-4.5,1)(-5.25,1);"
      << "\nINS POLYGON lay=-1 col=0000cc fil= 1 ( 5.25,-1)( 4.5,-1)( 4.5,1)( 5.25,1);"
    //field lines and circles
      << "\nINS POLYGON lay=-1 col=ffffff (-4.5,-2.5)(4.5,-2.5)(4.5,2.5)(-4.5,2.5);"
      << "\nINS LINE lay=-1 col=ffffff (0,2.5,0,-2.5) (-4.5,-1.5,-3.5,-1.5) (-3.5,-1.5,-3.5,1.5) (-3.5,1.5,-4.5,1.5) (4.5,-1.5,3.5,-1.5) (3.5,-1.5,3.5,1.5) (3.5,1.5,4.5,1.5);"
      << "\nINS CIRCLE lay=-1 col=ffffff (0,0,1.0);"
      << "\nINS POINT lay=-1 col=ffffff (-2.5,0) (2.5,0);"
    //ball
      << "\nINS FRAME id=1 lay= 1 (1,0,0);"
      << "\nINS 1 CIRCLE col=cc0000 fil= 1 (0,0,0.15);";

  RobotFreiburg r1; r1.frame= 2;
  RobotGolem r2; r2.pos= Vector2d(2,0); r2.frame= 3;

  r1.print_skin(out);
  r2.print_skin(out);

#ifdef HAVE_SSTREAM
  sock.send_msg( out.str().c_str(), out.str().length() );
#else
  out << std::ends;
  sock.send_msg( out.str(), strlen( out.str() ) );
  out.freeze( false );
#endif

  //end of field init section
  /////////////////////////////////////////////////////////////////////////////

  Joystick js;
  if ( ! js.init("/dev/input/js0"))
    exit(1);

  //end of joystick init section
  /////////////////////////////////////////////////////////////////////////////
  Robot * robot= &r2;
//  double x= 0;
  while(1) {
#if 0
    //x+= 0.041666;
    //x+= 0.02;
    x+= 0.01;
    if (x>3)
      x=0.0;
    char msg[20];
    sprintf(msg,"MOV 1 (%lf,%lf); MOV 2 (%lf,2);MOV 3 (3,%lf);",1.2+x,x,x,-2.5+x*2);
    sock.send_msg(msg,strlen(msg));
    //usleep(41666);
    //usleep(20000);
    usleep(10000);
    continue;
#endif
    int num= js.read_all_events();
    std::cout << "\nread " << num << " joystick events";

    if (num) {
      if ( js.button_chg[0] && js.button[0] > 0)
        robot= &r1;
      if ( js.button_chg[1] && js.button[1] > 0)
        robot= &r2;

      robot->read_joystick(js.num_axes,js.axis );

      out.seekp(0);


      robot->print_pos(out);
#ifdef HAVE_SSTREAM
      sock.send_msg( out.str().c_str(), out.str().length() );
#else
      out << std::ends;
      sock.send_msg( out.str(), strlen( out.str() ) );
      out.freeze( false );
#endif


    }
    //usleep(100000);
    usleep(100000);
  }


  /////////////////////////////////////////////////////////////////////////////
}

#endif // HAVE_LINUX_JOSTICK_H

int  main (int  argc,char  *argv[]) {
  if (argc <= 1) {
    std::cerr << "\nno arguments specified";
    return 1;
  }
  else {
    argc--;
    argv++;
  }
  bool frame5= false;
  bool halma= false;
  bool command_line= false;
  bool loop= false;
  bool move_ascii_tree= false;
  bool joy_for_midsize= false;
  char file[255]="";
  char host[512]="localhost";
  int  port= 6010;

  bool tcp= false;
  ValueParser vp(argc,argv);
  if ( vp.get("frame5",frame5) == 0 ) frame5= true;
  if ( vp.get("halma",halma) == 0 ) halma= true;
  if ( vp.get("c",command_line) == 0 ) command_line= true;
  if ( vp.get("loop",loop) == 0 ) loop= true;
  if ( vp.get("tree",move_ascii_tree) == 0 ) move_ascii_tree= true;
  if ( vp.get("joy",joy_for_midsize) == 0 ) joy_for_midsize= true;
  vp.get("host",host,512);
  vp.get("port",port);

  vp.get("file",file,255);
  if ( vp.get("tcp_file",file,255) > 0 )
    tcp= true;

  UDPsocket sock;
  sock.init_socket_fd();
  std::cout << "\nhost= " << host << " port= " << port;
  sock.init_serv_addr(host,port);

  if (command_line)
    command_line_input(sock);
  else if (move_ascii_tree)
    move_ascii_tree_input(sock);
  else if (loop)
    loop_input(sock);
  else if (halma)
    halma_input(sock);
#ifdef HAVE_LINUX_JOYSTICK_H
  else if (joy_for_midsize)
    joy_input(sock);
#endif // HAVE_LINUX_JOYSTICK_H
  else if (frame5)
    move_frame_5(sock);
  else if ( strlen(file) ) {
    if (!tcp)
      file_input(sock,file);
    else
      file_input_over_tcp(file);
  }

  return 0;
}