static void ws_client_read_task_code(ws_client_read_task_args *args)
{
  ulapi_task_struct *task;
  int id;
  enum {INBUF_SIZE = 1024};
  char inbuf[INBUF_SIZE];
  char *ptr;
  char *endptr;
  int nchars;
  ros::NodeHandle nh;
  ros::Publisher pub;
  nist_kitting::ws_cmd ws_cmd;
  int serial_number = 1;

  task = args->task;
  id = args->id;
  free(args);
  
  pub = nh.advertise<nist_kitting::ws_cmd>(KITTING_WS_CMD_TOPIC, TOPIC_QUEUE_LEN);

  while (true) {
    nchars = ulapi_socket_read(id, inbuf, sizeof(inbuf));
    if (nchars <= 0) break;
    if (nchars < sizeof(inbuf)) inbuf[nchars] = 0;
    else inbuf[sizeof(inbuf)-1] = 0;

    ptr = inbuf;
    // strip off leading and trailing whitespace
    while (isspace(*ptr)) ptr++;
    endptr = ptr + strlen(ptr);
    while (isspace(*endptr)) *endptr-- = 0;
    if (0 == *ptr) continue;

    ws_cmd.cmd.type = KITTING_WS_ASSEMBLE_KIT;
    ws_cmd.assemble_kit.name = std::string(ptr);
    ws_cmd.assemble_kit.quantity = 1;
    ws_cmd.cmd.serial_number = serial_number++;
    pub.publish(ws_cmd);
  }

  ulapi_socket_close(id);

  if (debug) printf("ws client read handler %d done\n", id);

  ulapi_task_delete(task);

  return;
}
예제 #2
0
int main(int argc, char *argv[])
{
  double myJoints[ROBOT_DOF] = {1,2,3,4,5,6};
  double jointsIn[ROBOT_DOF];
  robotPose myPose, poseIn;
  int kukaConnection;
  RCS_TIMER *cycleBlock = new RCS_TIMER(KUKA_DEFAULT_CYCLE);
  TiXmlDocument kukaStatus;
  TiXmlHandle toSendHandle(&kukaStatus);
  TiXmlElement *cartesianStatus;
  TiXmlElement *cartesianUpdate;
  TiXmlElement *jointStatus;
  TiXmlElement *jointUpdate;
  TiXmlElement *IPOCUpdate;
  int nchars;
  TiXmlElement *cartesian;
  int debug = 0;
  int option;
  int counter = 0;
  double jointMotorScale[ROBOT_DOF], cmdMotorScale[ROBOT_DOF];

  jointMotorScale[0] = 80.;
  jointMotorScale[1] = 100.;
  jointMotorScale[2] = 80.;
  jointMotorScale[3] = 80.;
  jointMotorScale[4] = 80.;
  jointMotorScale[5] = 40.5;

  cmdMotorScale[0] = 1.4;
  cmdMotorScale[1] = 1.74;
  cmdMotorScale[2] = 1.4;
  cmdMotorScale[3] = 1.4;
  cmdMotorScale[4] = 1.4;
  cmdMotorScale[5] = 0.87;

  while (true) 
    {
      option = getopt(argc, argv, ":d");
      if (option == -1) break;
      switch (option) 
	{
	case 'd':
	  debug = 1;
	  break;
	case ':':
	  fprintf(stderr, "missing value for -%c\n", optopt);
	  return 1;
	  break;
	default:
	  fprintf (stderr, "unrecognized option -%c\n", optopt);
	  return 1;
	  break;
	} // switch (option)
    }   // while (true) for getopt


  if( !kukaStatus.LoadFile(DEFAULT_FROM_KUKA))
    {
      printf( "kukaRobot:: fatal error on load of %s\n", DEFAULT_FROM_KUKA);
      exit(1);
    }
  kukaConnection = ulapi_socket_get_client_id(KUKA_PORT, "localhost");
  if( kukaConnection < 0 )
    return -1;

  myPose.x = 0.1;
  myPose.y = 0.2;
  myPose.z = 0.3;
  myPose.xrot = 0.4;
  myPose.yrot = 0.5;
  myPose.zrot = 0.6;

  while(true)
    {
      TiXmlDocument kukaCorrections;
      TiXmlHandle correctionsHandle(&kukaCorrections);
      std::string str;
      enum {INBUF_LEN = 2048};
      char inbuf[INBUF_LEN];
      std::ostringstream s;
      cartesianStatus =
	toSendHandle.FirstChild("Rob").FirstChild("Dat").Child(1).ToElement();
      IPOCUpdate = toSendHandle.FirstChild("Rob").FirstChild("Dat").Child(9).
	ToElement();
      jointStatus =
	toSendHandle.FirstChild("Rob").FirstChild("Dat").Child(3).ToElement();
      cartesianStatus->SetDoubleAttribute("X", myPose.x); 
      cartesianStatus->SetDoubleAttribute ("Y", myPose.y);
      cartesianStatus->SetDoubleAttribute ("Z", myPose.z);
      cartesianStatus->SetDoubleAttribute ("A", myPose.xrot);
      cartesianStatus->SetDoubleAttribute ("B", myPose.yrot);
      cartesianStatus->SetDoubleAttribute ("C", myPose.zrot);
      jointStatus->SetDoubleAttribute("A1", myJoints[0]);
      jointStatus->SetDoubleAttribute("A2", myJoints[1]);
      jointStatus->SetDoubleAttribute("A3", myJoints[2]);
      jointStatus->SetDoubleAttribute("A4", myJoints[3]);
      jointStatus->SetDoubleAttribute("A5", myJoints[4]);
      jointStatus->SetDoubleAttribute("A6", myJoints[5]);

      s << counter++;
      s << '\0';
      TiXmlText *text = new TiXmlText((s.str()).c_str());
      IPOCUpdate->Clear();
      IPOCUpdate->LinkEndChild(text);
      //      kukaStatus.Print();
      str << kukaStatus;
      ulapi_socket_write(kukaConnection, str.c_str(), str.length());
      inbuf[0] = '\0';
      nchars = ulapi_socket_read(kukaConnection, inbuf, sizeof(inbuf)-1);
      if (nchars <= 0) 
	{
	  printf("kukaRobot::status client disconnected\n");
	  break;
	}
      else
	{
	inbuf[nchars] = '\0';
	}
      kukaCorrections.Parse(inbuf);
      if(debug) kukaCorrections.Print();
      cartesianUpdate =
	correctionsHandle.FirstChild("Sen").FirstChild("Dat").
	Child(1).ToElement();
      jointUpdate =
	correctionsHandle.FirstChild("Sen").FirstChild("Dat").
	Child(2).ToElement();
      cartesianUpdate->QueryDoubleAttribute("X", &(poseIn.x));
      cartesianUpdate->QueryDoubleAttribute("Y", &(poseIn.y));
      cartesianUpdate->QueryDoubleAttribute("Z", &(poseIn.z));
      cartesianUpdate->QueryDoubleAttribute("A", &(poseIn.xrot));
      cartesianUpdate->QueryDoubleAttribute("B", &(poseIn.yrot));
      cartesianUpdate->QueryDoubleAttribute("C", &(poseIn.zrot));
      jointUpdate->QueryDoubleAttribute("A1", &(jointsIn[0]));
      jointUpdate->QueryDoubleAttribute("A2", &(jointsIn[1]));
      jointUpdate->QueryDoubleAttribute("A3", &(jointsIn[2]));
      jointUpdate->QueryDoubleAttribute("A4", &(jointsIn[3]));
      jointUpdate->QueryDoubleAttribute("A5", &(jointsIn[4]));
      jointUpdate->QueryDoubleAttribute("A6", &(jointsIn[5]));
      myPose.x += poseIn.x;
      myPose.y += poseIn.y;
      myPose.z += poseIn.z;
      myPose.xrot += poseIn.xrot;
      myPose.yrot += poseIn.yrot;
      myPose.zrot += poseIn.zrot;
      for( int i=0; i<ROBOT_DOF; i++ )
	{
	  myJoints[i] += jointsIn[i] * cmdMotorScale[i] / jointMotorScale[i];
	  if( debug )
	    printf( "J%d <%lf %lf> ", i+1, myJoints[i], jointsIn[i] );
	}
      if(debug)
      printf( "\nkukaRobot Status: <%4.2f, %4.2f, %4.2f> <%4.2f, %4.2f, %4.2f>\n\n",
	      myPose.x,
	      myPose.y,
	      myPose.z,
	      myPose.xrot,
	      myPose.yrot,
	      myPose.zrot);
      cycleBlock->wait();
    }
}
예제 #3
0
rtapi_integer rtapi_socket_read(rtapi_integer id,
				char *buf,
				rtapi_integer len)
{
  return ulapi_socket_read(id, buf, len);
}
예제 #4
0
void client_code(void *args)
{
  void *client_task;
  ulapi_integer client_id;
  client_db_struct *client_db_ptr;
  ulapi_integer debug;
  enum {BUFFERLEN = 256};
  char inbuf[BUFFERLEN];
  char outbuf[BUFFERLEN];
  ulapi_integer nchars;
  int number;
  int lastnumber = 0;

  client_task = ((client_args *) args)->client_task;
  client_id = ((client_args *) args)->client_id;
  client_db_ptr = ((client_args *) args)->client_db_ptr;
  debug = ((client_args *) args)->debug;
  free(args);

  /*
    The client thread asks for an update every second, blocks until
    it gets a response, prints it, and loops again.
  */

  for (;;) {
    ulapi_mutex_take(client_db_ptr->mutex);
    number = client_db_ptr->number;
    ulapi_mutex_give(client_db_ptr->mutex);

    if (number != lastnumber) {
      ulapi_snprintf(outbuf, sizeof(outbuf), "write %d", number);
      lastnumber = number;
    } else {
      ulapi_snprintf(outbuf, sizeof(outbuf), "read");
    }
    ulapi_socket_write(client_id, outbuf, strlen(outbuf)+1);

    nchars = ulapi_socket_read(client_id, inbuf, sizeof(inbuf)-1);

    if (-1 == nchars) {
      if (debug) printf("connection closed\n");
      break;
    }

    if (0 == nchars) {
      if (debug) printf("end of file\n");
      break;
    }
    inbuf[nchars] = 0;


    /*
      Parse and handle the message here as your application requires.
    */
    printf("%s\n", inbuf);

    ulapi_wait(1000000000);
  }

  ulapi_socket_close(client_id);
}
int main(int argc, char *argv[])
{
    enum {HOST_LEN = 256};
    char host[HOST_LEN] = "localhost";
    int state_port = STATE_PORT_DEFAULT;
    int cognex_port = COGNEX_PORT_DEFAULT;
    const double confidence = 0.95;
    double period = 1;
    int option;
    int ival;
    double dval;
    int state_client_id;
    ulapi_task_struct cognex_client_thread;
    cognex_object_info_db db;
    ulapi_task_struct *server_task;
    ulapi_integer retval;

    opterr = 0;
    while (true) {
        option = getopt(argc, argv, ":h:s:p:t:d");
        if (option == -1) break;

        switch (option) {
        case 'h':
            strncpy(host, optarg, sizeof(host));
            host[sizeof(host)-1] = 0;
            break;

        case 's':
            ival = atoi(optarg);
            state_port = ival;
            break;

        case 'p':
            ival = atoi(optarg);
            cognex_port = ival;
            break;

        case 't':
            dval = atof(optarg);
            period = dval;
            break;

        case 'd':
            debug = true;
            break;

        case ':':
            fprintf(stderr, "missing value for -%c\n", optopt);
            return 1;
            break;

        default:
            fprintf (stderr, "unrecognized option -%c\n", optopt);
            return 1;
            break;
        } // switch (option)
    }   // while (true) for getopt

    // set up mapping of integer ids with object names
    object_names[1] = std::string("small_gear");
    object_names[2] = std::string("medium_gear");
    object_names[3] = std::string("large_gear");
    object_names[4] = std::string("bottom_cover");
    object_names[5] = std::string("top_cover");

    // connect to state server
    state_client_id = ulapi_socket_get_client_id(state_port, host);
    if (state_client_id < 0) {
        fprintf(stderr, "can't connect to %s on port %d\n", host, state_port);
        return 1;
    }

    server_task = db.serve(cognex_port, period * 1e9);
    if (server_task == NULL) {
        fprintf(stderr, "can't serve port %d\n", cognex_port);
        return 1;
    }

    while (true) {
        enum {INBUF_LEN = 1024};
        char inbuf[INBUF_LEN];
        char *ptr;
        int nchars;
        int nleft;
        int length;
        int message_type;
        object_state_message obj_state;

        nchars = ulapi_socket_read(state_client_id, inbuf, sizeof(inbuf));
        if (nchars <= 0) break;
        inbuf[sizeof(inbuf)-1] = 0;
        ptr = inbuf;
        nleft = nchars;

        while (nleft > 0) {
            // get the length and type of the message
            memcpy(&length, ptr, sizeof(length));
            memcpy(&message_type, ptr + sizeof(length), sizeof(message_type));

            // switch on the message type and handle it
            switch (message_type) {
            case MESSAGE_ROBOT_STATUS:
            case MESSAGE_JOINT_POSITION:
                break;

            case MESSAGE_OBJECT_STATE:
                if (0 == obj_state.read_object_state(ptr)) {
                    if (debug) obj_state.print_object_state();
                    for (int t = 0; t < obj_state.number; t++) {
                        db.add(obj_state.objects[t].id, cognex_object_info(object_names[obj_state.objects[t].id].c_str(), 2*atan2(obj_state.objects[t].qz, obj_state.objects[t].qw), obj_state.objects[t].x, obj_state.objects[t].y, confidence));
                    }
                }
                break;

            default:
                // unknown message
                break;
            } // switch (message type)
            nleft -= (sizeof(length) + length);
            ptr += (sizeof(length) + length);
        } // while (nleft)
    }   // while (true)

    ulapi_task_join(server_task, &retval);

    return 0;
}
예제 #6
0
static void request_connection_thread_code(request_connection_thread_args *args)
{
  void *thread;
  int id;
  enum {INBUF_LEN = 1024};
  char inbuf[INBUF_LEN];
  enum {OUTBUF_LEN = 1024};
  char outbuf[OUTBUF_LEN];
  char *ptr;
  int nchars;
  int nleft;
  int length;
  int message_type;
  ping_reply_message pingrep;
  joint_traj_pt_request_message jtreq;
  joint_traj_pt_reply_message jtrep;
  joint_info jinfo;
  cart_traj_pt_request_message ctreq;
  cart_traj_pt_reply_message ctrep;
  float f1, f2, f3, f4, f5, f6, f7;

  thread = args->thread;
  id = args->id;
  free(args);

  bool done = false;
  while (! done) {
    nchars = ulapi_socket_read(id, inbuf, sizeof(inbuf));
    if (nchars <= 0) break;
    inbuf[sizeof(inbuf)-1] = 0;
    ptr = inbuf;
    nleft = nchars;

    while (nleft > 0) {
      // get the length and type of the message
      memcpy(&length, ptr, sizeof(length));
      memcpy(&message_type, ptr + sizeof(length), sizeof(message_type));

      // switch on the message type and handle it
      switch (message_type) {
      case MESSAGE_PING:
	nchars = ulapi_socket_write(id, reinterpret_cast<char *>(&pingrep), sizeof(pingrep));
	if (nchars < 0) done = true;
	break;

      case MESSAGE_JOINT_TRAJ_PT:
	jtreq.read_joint_traj_pt_request(ptr);
	if (debug) {
	  printf("connection %d requested:\n", id);
	  jtreq.print_joint_traj_pt_request();
	}
	ulapi_mutex_take(&robot_mutex);
	for (int t = 0; t < JOINT_MAX; t++) {
	  if (! jtreq.get_pos(&f1, t)) break;
	  the_robot.set_robot_joint_pos(f1, t);
	}

	// FIXME -- testing with a delay, should thread this off
	ulapi_sleep(3);

	ulapi_mutex_give(&robot_mutex);
	jtrep.set_joint_traj_pt_reply(REPLY_SUCCESS);
	nchars = ulapi_socket_write(id, reinterpret_cast<char *>(&jtrep), sizeof(jtrep));
	if (debug) {
	  printf("replied to connection %d:\n", id);
	  jtrep.print_joint_traj_pt_reply();
	}
	if (nchars < 0) done = true;
	break;

      case MESSAGE_CART_TRAJ_PT:
	ctreq.read_cart_traj_pt_request(ptr);
	if (debug) {
	  printf("connection %d requested:\n", id);
	  ctreq.print_cart_traj_pt_request();
	}

	// FIXME -- ditto
	ulapi_sleep(3);

	ulapi_mutex_take(&robot_mutex);
	if (! ctreq.get_pos(&f1, &f2, &f3, &f4, &f5, &f6, &f7)) break;
	the_robot.set_robot_cart_pos(f1, f2, f3, f4, f5, f6, f7);
	ulapi_mutex_give(&robot_mutex);
	ctrep.set_seq_number(ctreq.get_seq_number());
	ctrep.set_cart_traj_pt_reply(REPLY_SUCCESS);
	nchars = ulapi_socket_write(id, reinterpret_cast<char *>(&ctrep), sizeof(ctrep));
	if (debug) {
	  printf("replied to connection %d:\n", id);
	  ctrep.print_cart_traj_pt_reply();
	}
	if (nchars < 0) done = true;
	break;

      default:
	// unknown message
	if (debug) printf("unknown message type: %d\n", message_type);
	break;
      } // switch (message type)
      nleft -= (sizeof(length) + length);
      ptr += (sizeof(length) + length);
    }	// while (nleft)
  }	// while (true)

  ulapi_socket_close(id);

  if (debug) printf("simple message connection handler %d done\n", id);

  free(thread);

  return;
}