示例#1
0
// Process incoming data
void playerc_rfid_putmsg (playerc_rfid_t *device,
			  player_msghdr_t *header,
			  void *data)
{
  int i;

  if((header->type == PLAYER_MSGTYPE_DATA) &&
	(header->subtype == PLAYER_RFID_DATA_TAGS))
  {
    player_rfid_data_t* rfid_data = (player_rfid_data_t*)data;
    // clean up our existing tags
    if (device->tags) 
    {
      for (i = 0; i < device->tags_count; i++)
        free(device->tags[i].guid);
    }
    device->tags_count = rfid_data->tags_count;
    device->tags = realloc(device->tags,device->tags_count * sizeof(device->tags[0]));
    memset(device->tags,0,device->tags_count * sizeof(device->tags[0]));
    for (i = 0; i < device->tags_count; i++)
    {
      device->tags[i].type = rfid_data->tags[i].type;
      device->tags[i].guid_count = rfid_data->tags[i].guid_count;
      device->tags[i].guid = malloc(device->tags[i].guid_count);
      memcpy(device->tags[i].guid,rfid_data->tags[i].guid,device->tags[i].guid_count);
    }
  }
  else
	PLAYERC_WARN2("skipping rfid message with unknown type/subtype: %s/%d\n",
	    msgtype_to_str(header->type), header->subtype);
}
示例#2
0
// Process incoming data
void playerc_gps_putmsg(playerc_gps_t *device,
                               player_msghdr_t *header,
                               void *data, size_t len)
{
  if((header->type == PLAYER_MSGTYPE_DATA) &&
     (header->subtype == PLAYER_GPS_DATA_STATE))
  {
    player_gps_data_t * gps_data = (player_gps_data_t *) data;
    device->utc_time = gps_data->time_sec + ((double) gps_data->time_usec)/1e6;

    device->lat = gps_data->latitude / 1e7;
    device->lon = gps_data->longitude / 1e7;
    device->alt = gps_data->altitude / 1e3;

    device->utm_e = gps_data->utm_e / 100.0;
    device->utm_n = gps_data->utm_n / 100.0;

    device->hdop = gps_data->hdop / 10.0;
    device->vdop = gps_data->vdop / 10.0;

    device->err_horz = gps_data->err_horz;
    device->err_vert = gps_data->err_vert;

    device->quality = gps_data->quality;
    device->sat_count = gps_data->num_sats;
  }
  else
    PLAYERC_WARN2("skipping gps message with unknown type/subtype: %s/%d\n",
                 msgtype_to_str(header->type), header->subtype);
  return;
}
// Process incoming data
void
playerc_position3d_putmsg(playerc_position3d_t *device,
                          player_msghdr_t *header,
                          player_position3d_data_t *data, size_t len)
{
  if((header->type == PLAYER_MSGTYPE_DATA) &&
     (header->subtype == PLAYER_POSITION3D_DATA_STATE))
  {
    device->pos_x = data->pos.px;
    device->pos_y = data->pos.py;
    device->pos_z = data->pos.pz;

    device->pos_roll = data->pos.proll;
    device->pos_pitch = data->pos.ppitch;
    device->pos_yaw = data->pos.pyaw;

    device->vel_x = data->vel.px;
    device->vel_y = data->vel.py;
    device->vel_z = data->vel.pz;

    device->vel_roll = data->vel.proll;
    device->vel_pitch = data->vel.ppitch;
    device->vel_yaw = data->vel.pyaw;

    device->stall = data->stall;
  }
  else
    PLAYERC_WARN2("skipping position3d message with unknown type/subtype: %s/%d\n",
                  msgtype_to_str(header->type), header->subtype);
}
示例#4
0
// Process incoming data
void playerc_micronsonar_putmsg(playerc_micronsonar_t *device, player_msghdr_t *header,
                player_micronsonar_data_t *data, size_t len)
{
  if((header->type == PLAYER_MSGTYPE_DATA) &&
     (header->subtype == PLAYER_MICRONSONAR_DATA_STATE))
  {
    device->width        = data->width;
    device->height       = data->height;
    device->bpp          = data->bpp;
    device->format       = data->format;
    device->image_count  = data->image_count;
    device->image        = realloc(device->image, sizeof(device->image[0])*device->image_count);
    device->centreX      = data->centreX;
    device->centreY      = data->centreY;
    device->range        = data->range;
    device->numBins      = data->numBins;
    device->startAngle   = data->startAngle;
    device->endAngle     = data->endAngle;
    
    if (device->image)
      memcpy(device->image, data->image, device->image_count);
    else
      PLAYERC_ERR1("failed to allocate memory for image, needed %ld bytes\n", sizeof(device->image[0])*device->image_count);
  }
  else
    PLAYERC_WARN2("skipping micronsonar message with unknown type/subtype: %s/%d\n",
                 msgtype_to_str(header->type), header->subtype);
  return;
}
示例#5
0
// Process incoming data
void playerc_time_putmsg(playerc_time_t *device,
                               player_msghdr_t *header,
                               void *data, size_t len)
{
  PLAYERC_WARN2("skipping time message with unknown type/subtype: %s/%d\n",
		msgtype_to_str(header->type), header->subtype);
  return;
}
示例#6
0
// Process incoming data
void playerc_opaque_putmsg(playerc_opaque_t *device, player_msghdr_t *header,
                            player_opaque_data_t *data, size_t len)
{
  if((header->type == PLAYER_MSGTYPE_DATA) &&
     (header->subtype == PLAYER_OPAQUE_DATA_STATE))
  {
    device->data_count   = data->data_count;
    device->data = realloc(device->data, sizeof(*device->data)*device->data_count);

    memcpy(device->data, data->data, device->data_count);
  }
  else
    PLAYERC_WARN2("skipping opaque message with unknown type/subtype: %s/%d\n",
                 msgtype_to_str(header->type), header->subtype);
  return;
}
示例#7
0
// Process incoming data
void playerc_position1d_putmsg(playerc_position1d_t *device,
                               player_msghdr_t *header,
                               player_position1d_data_t *data, size_t len)
{
  if((header->type == PLAYER_MSGTYPE_DATA) &&
     (header->subtype == PLAYER_POSITION1D_DATA_STATE))
  {
    device->pos = data->pos;
    device->vel = data->vel;
    device->stall = data->stall;
    device->status = data->status;
  }
  else
    PLAYERC_WARN2("skipping position1d message with unknown type/subtype: %s/%d\n",
                 msgtype_to_str(header->type), header->subtype);
}
// Process incoming data
void playerc_pointcloud3d_putmsg (playerc_pointcloud3d_t *device,
			          player_msghdr_t *header,
		                  void *data)
{
    if((header->type == PLAYER_MSGTYPE_DATA) &&
       (header->subtype == PLAYER_POINTCLOUD3D_DATA_STATE))
    {
	player_pointcloud3d_data_t* pc3_data = (player_pointcloud3d_data_t*)data;
        device->points_count = pc3_data->points_count;
        device->points = realloc(device->points, device->points_count*sizeof(device->points[0]));
        memcpy (device->points, pc3_data->points,
        	sizeof (player_pointcloud3d_element_t)*device->points_count);
    }
    else
	PLAYERC_WARN2("skipping pointcloud3d message with unknown type/subtype: %s/%d\n",
	    msgtype_to_str(header->type), header->subtype);
}
示例#9
0
// Process incoming data
void playerc_motor_putmsg(playerc_motor_t *device,
                          player_msghdr_t *header,
                          player_motor_data_t *data, size_t len)
{
  if((header->type == PLAYER_MSGTYPE_DATA) &&
     (header->subtype == PLAYER_MOTOR_DATA_STATE))
  {
    device->pt = data->pos;
    device->vt = data->vel;

    device->limits = data->limits;
    device->stall = data->stall;
  }
  else
    PLAYERC_WARN2("skipping motor message with unknown type/subtype: %s/%d\n",
                 msgtype_to_str(header->type), header->subtype);
}
示例#10
0
// Process incoming data
void playerc_ranger_putmsg(playerc_ranger_t *device, player_msghdr_t *header,
                           void *data, size_t len)
{
  if(header->size == 0)
  {
    PLAYERC_ERR2("(putmsg) Ranger message size <= 0 in message %s/%d", msgtype_to_str(header->type), header->subtype);
    return;
  }

  if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_RANGER_DATA_RANGE))
  {
    playerc_ranger_copy_range_data(device, (player_ranger_data_range_t *) data);
    playerc_ranger_calculate_bearings(device);
    playerc_ranger_calculate_points(device);
  }
  else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_RANGER_DATA_RANGESTAMPED))
  {
    player_ranger_data_rangestamped_t *stampedData = (player_ranger_data_rangestamped_t*) data;
    playerc_ranger_copy_range_data(device, &stampedData->data);
    if (stampedData->have_geom)
      playerc_ranger_copy_geom(device, &stampedData->geom);
    if (stampedData->have_config)
      playerc_ranger_copy_config(device, &stampedData->config);
    playerc_ranger_calculate_bearings(device);
    playerc_ranger_calculate_points(device);
  }
  else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_RANGER_DATA_INTNS))
  {
    playerc_ranger_copy_intns_data(device, (player_ranger_data_intns_t *) data);
  }
  else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_RANGER_DATA_INTNSSTAMPED))
  {
    player_ranger_data_intnsstamped_t *stampedData = (player_ranger_data_intnsstamped_t*) data;
    playerc_ranger_copy_intns_data(device, &stampedData->data);
    if (stampedData->have_geom)
      playerc_ranger_copy_geom(device, &stampedData->geom);
    if (stampedData->have_config)
      playerc_ranger_copy_config(device, &stampedData->config);
  }
  else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_RANGER_DATA_GEOM))
  {
    playerc_ranger_copy_geom(device, (player_ranger_geom_t *) data);
  }
  else
    PLAYERC_WARN2("Skipping ranger message with unknown type/subtype: %s/%d\n", msgtype_to_str(header->type), header->subtype);
}
示例#11
0
void playerc_audio_putmsg(playerc_audio_t *device,
                          player_msghdr_t *header,
                          uint8_t *data, size_t len)
{
    if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_AUDIO_DATA_WAV_REC))
    {
        player_audio_wav_t * wdata = (player_audio_wav_t *) data;
        assert(header->size > 0);
        device->wav_data.data_count = wdata->data_count;
        if (device->wav_data.data != NULL)
            free (device->wav_data.data);
        if ((device->wav_data.data = (uint8_t*) malloc (wdata->data_count)) == NULL)
            PLAYERC_ERR("Failed to allocate space to store wave data locally");
        else
        {
            memcpy(device->wav_data.data, wdata->data, wdata->data_count * sizeof(device->wav_data.data[0]));
            device->wav_data.format = wdata->format;
        }
    }
    else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_AUDIO_DATA_SEQ))
    {
        player_audio_seq_t * sdata = (player_audio_seq_t *) data;
        assert(header->size > 0);
        device->seq_data.tones_count = sdata->tones_count;
        memcpy(device->seq_data.tones, sdata->tones, sdata->tones_count * sizeof(device->seq_data.tones[0]));
    }
    else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_AUDIO_DATA_MIXER_CHANNEL))
    {
        player_audio_mixer_channel_list_t * wdata = (player_audio_mixer_channel_list_t *) data;
        assert(header->size > 0);
        device->mixer_data.channels_count = wdata->channels_count;
        memcpy(device->mixer_data.channels, wdata->channels, wdata->channels_count * sizeof(device->mixer_data.channels[0]));
    }
    else if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_AUDIO_DATA_STATE))
    {
        player_audio_state_t *sdata = (player_audio_state_t *) data;
        assert(header->size > 0);
        device->state = sdata->state;
    }
    else
        PLAYERC_WARN2("skipping audio message with unknown type/subtype: %s/%d\n",
                      msgtype_to_str(header->type), header->subtype);
}
示例#12
0
// Process incoming data
void playerc_wsn_putmsg (playerc_wsn_t *device,
			 player_msghdr_t *header,
			 void *data)
{
//    int i, j;

    if((header->type == PLAYER_MSGTYPE_DATA) &&
       (header->subtype == PLAYER_WSN_DATA_STATE))
    {
	player_wsn_data_t* wsn_data = (player_wsn_data_t*)data;
	device->node_type      = wsn_data->node_type;
	device->node_id        = wsn_data->node_id;
	device->node_parent_id = wsn_data->node_parent_id;
	device->data_packet    = wsn_data->data_packet;
    }
    else
	PLAYERC_WARN2("skipping wsn message with unknown type/subtype: %s/%d\n",
	    msgtype_to_str(header->type), header->subtype);
}
示例#13
0
// Process incoming data
void playerc_joystick_putmsg(playerc_joystick_t *device, player_msghdr_t *header,
                            player_joystick_data_t *data, size_t len)
{
  uint32_t i;
  if((header->type == PLAYER_MSGTYPE_DATA) &&
     (header->subtype == PLAYER_JOYSTICK_DATA_STATE))
  {

    device->buttons       = data->buttons;
    device->axes_count       = data->axes_count;
    for (i = 0; i < data->axes_count ; i++)
        device->pos[i] = data->pos[i];


  }
  else
    PLAYERC_WARN2("skipping joystick message with unknown type/subtype: %s/%d\n",
                 msgtype_to_str(header->type), header->subtype);
  return;
}
示例#14
0
void playerc_blinkenlight_putmsg(playerc_blinkenlight_t *device,
				 player_msghdr_t *header,
				 void* data, size_t len)
{
  if((header->type == PLAYER_MSGTYPE_DATA) && 
     (header->subtype == PLAYER_BLINKENLIGHT_DATA_STATE))
    {
      player_blinkenlight_data_t* blink = (player_blinkenlight_data_t*)data;
      
      device->enabled = blink->enable;	    
      device->duty_cycle = blink->dutycycle;
      device->period = blink->period;
      device->red = blink->color.red;
      device->green = blink->color.green;
      device->blue = blink->color.blue;
    }
  else
    PLAYERC_WARN2("skipping blinkenlight message with unknown type/subtype: %s/%d\n",
                  msgtype_to_str(header->type), header->subtype);
}
示例#15
0
void playerc_actarray_putmsg(playerc_actarray_t *device,
                             player_msghdr_t *header,
                             player_actarray_data_t *data, size_t len)
{
  uint32_t i = 0;

  if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_ACTARRAY_DATA_STATE))
  {
    device->actuators_count = data->actuators_count;
    device->actuators_data = realloc(device->actuators_data,device->actuators_count*sizeof(device->actuators_data[0]));
    for (i = 0; i < device->actuators_count; i++)
    {
      device->actuators_data[i] = data->actuators[i];
    }
    device->motor_state = data->motor_state;
  }
  else
    PLAYERC_WARN2("skipping actarray message with unknown type/subtype: %s/%d\n",
                  msgtype_to_str(header->type), header->subtype);
}
示例#16
0
// Process incoming data
void playerc_health_putmsg (playerc_health_t *device,
			 player_msghdr_t *header,
			 player_health_data_t *data, size_t len)
{

    if((header->type == PLAYER_MSGTYPE_DATA) &&
       (header->subtype == PLAYER_HEALTH_DATA_STATE))
    {
	device->cpu_usage.idle    = data->cpu_usage.idle;
	device->cpu_usage.system    = data->cpu_usage.system;
	device->cpu_usage.user    = data->cpu_usage.user;
	device->mem.total	= data->mem.total;
	device->mem.used	= data->mem.used;
 	device->mem.free	= data->mem.free;
 	device->swap.total    = data->swap.total;
	device->swap.used    = data->swap.used;
    	device->swap.free    = data->swap.free;
    }
    else
	PLAYERC_WARN2("skipping health message with unknown type/subtype: %s/%d\n",
	    msgtype_to_str(header->type), header->subtype);
}
示例#17
0
void playerc_limb_putmsg(playerc_limb_t *device,
                         player_msghdr_t *header,
                         player_limb_data_t *data, size_t len)
{
    if((header->type == PLAYER_MSGTYPE_DATA) && (header->subtype == PLAYER_LIMB_DATA_STATE))
    {
        device->data.position.px = data->position.px;
        device->data.position.py = data->position.py;
        device->data.position.pz = data->position.pz;

        device->data.approach.px = data->approach.px;
        device->data.approach.py = data->approach.py;
        device->data.approach.pz = data->approach.pz;

        device->data.orientation.px = data->orientation.px;
        device->data.orientation.py = data->orientation.py;
        device->data.orientation.pz = data->orientation.pz;

        device->data.state = data->state;
    }
    else
        PLAYERC_WARN2("skipping limb message with unknown type/subtype: %s/%d\n",
                      msgtype_to_str(header->type), header->subtype);
}
示例#18
0
// Process incoming data
void playerc_laser_putmsg(playerc_laser_t *device,
                          player_msghdr_t *header,
                          void *data)
{
  uint32_t i;
  double r, b, db;

  if((header->type == PLAYER_MSGTYPE_DATA) &&
     (header->subtype == PLAYER_LASER_DATA_SCAN))
  {
    player_laser_data_t* scan_data = (player_laser_data_t*)data;

    b = scan_data->min_angle;
    db = scan_data->resolution;

    device->scan_start = b;
    device->scan_res = db;
    device->max_range = scan_data->max_range;
    device->min_left = device->max_range;
    device->min_right = device->max_range;

    device->scan_count = scan_data->ranges_count;
    playerc_laser_reallocate_scans(device);

    for (i = 0; i < scan_data->ranges_count; i++)
    {
      r = scan_data->ranges[i];
      //assert(r >= 0);
      device->ranges[i] = r;
      device->scan[i][0] = r;
      device->scan[i][1] = b;
      device->point[i].px = r * cos(b);
      device->point[i].py = r * sin(b);
      b += db;

      if((i <= scan_data->ranges_count/2) && (r < device->min_right))
        device->min_right = r;
      else if((i > scan_data->ranges_count/2) && (r < device->min_left))
        device->min_left = r;
    }
    for (i = 0; i < scan_data->intensity_count; i++)
      device->intensity[i] = scan_data->intensity[i];

    device->scan_id = scan_data->id;
  }
  else if((header->type == PLAYER_MSGTYPE_DATA) &&
          (header->subtype == PLAYER_LASER_DATA_SCANPOSE))
  {
    player_laser_data_scanpose_t* scan_data =
            (player_laser_data_scanpose_t*)data;

    b = scan_data->scan.min_angle;
    db = scan_data->scan.resolution;

    device->scan_start = b;
    device->scan_res = db;

    device->scan_count = scan_data->scan.ranges_count;
    playerc_laser_reallocate_scans(device);

    for (i = 0; i < scan_data->scan.ranges_count; i++)
    {
      r = scan_data->scan.ranges[i];
      assert(r >= 0);
      device->ranges[i] = r;
      device->scan[i][0] = r;
      device->scan[i][1] = b;
      device->point[i].px = r * cos(b);
      device->point[i].py = r * sin(b);
      b += db;
    }
    for (i = 0; i < scan_data->scan.intensity_count; i++)
      device->intensity[i] = scan_data->scan.intensity[i];

    device->scan_id = scan_data->scan.id;
    device->robot_pose[0] = scan_data->pose.px;
    device->robot_pose[1] = scan_data->pose.py;
    device->robot_pose[2] = scan_data->pose.pa;
  }
  else if((header->type == PLAYER_MSGTYPE_DATA) &&
          (header->subtype == PLAYER_LASER_DATA_SCANANGLE))
  {
	  player_laser_data_scanangle_t* scan_data = (player_laser_data_scanangle_t*)data;
	  
	  device->max_range = scan_data->max_range;
	  device->min_left = device->max_range;
	  device->min_right = device->max_range;	  
	  
	  device->scan_count = scan_data->ranges_count;
	  playerc_laser_reallocate_scans(device);
	  
	  for (i = 0; i < scan_data->ranges_count; i++)
	  {
		  r = scan_data->ranges[i];
		  assert(r >= 0);
		  b = scan_data->angles[i];
		  device->ranges[i] = r;
		  device->scan[i][0] = r;
		  device->scan[i][1] = b;
		  device->point[i].px = r * cos(b);
		  device->point[i].py = r * sin(b);
		  
		  if((i <= scan_data->ranges_count/2) && (r < device->min_right))
			  device->min_right = r;
		  else if((i > scan_data->ranges_count/2) && (r < device->min_left))
			  device->min_left = r;
	  }
	  for (i = 0; i < scan_data->intensity_count; i++)
		  device->intensity[i] = scan_data->intensity[i];
	  
	  device->scan_id = scan_data->id;
  }
	
  else
    PLAYERC_WARN2("skipping laser message with unknown type/subtype: %s/%d\n",
                 msgtype_to_str(header->type), header->subtype);
}
示例#19
0
// Disconnect from the server, with potential retry
int playerc_client_disconnect_retry(playerc_client_t *client)
{
  int retval;
  int i;
  int j;
  struct timespec sleeptime;

  sleeptime.tv_sec = (long) client->retry_time;
  sleeptime.tv_nsec = 0;

  /* Disconnect */
  if((retval = playerc_client_disconnect(client)) != 0)
    PLAYERC_WARN("playerc_client_disconnect() failed");

  for(j=0; (client->retry_limit < 0) || (j<client->retry_limit); j++)
  {
    PLAYERC_WARN1("Reconnecting, attempt %d", j);
    /* Reconnect */
    if((retval = playerc_client_connect(client)) != 0)
      PLAYERC_WARN("playerc_client_connect() failed");
    else
    {
      /* Clean out buffers */
      client->read_xdrdata_len = 0;

      /* TODO: re-establish replacement rules, delivery modes, etc. */

      /* Re-subscribe to devices */
      for(i=0;i<client->device_count;i++)
      {
        if(client->device[i]->subscribed)
        {
          // TODO: what should access be here?
          if((retval = playerc_device_subscribe(client->device[i],
                                                PLAYERC_OPEN_MODE)) != 0)
          {
            PLAYERC_WARN2("playerc_device_subscribe() failed for %d:%d",
                         client->device[i]->addr.interf,
                         client->device[i]->addr.index);

            // TODO: Subscription failed for one device; should we give up?
            if(playerc_client_disconnect(client) != 0)
              PLAYERC_WARN("playerc_client_disconnect() failed");
            break;
          }
        }
      }
      // Did we get all of them?
      if(i == client->device_count)
        break;
    }

    nanosleep(&sleeptime,NULL);
  }

  if((client->retry_limit < 0) || (j < client->retry_limit))
  {
    PLAYERC_WARN("successfully reconnected");
    return(0);
  }
  else
  {
    PLAYERC_WARN("failed to reconnect");
    return(-1);
  }
}