Menu
▾
▴
[Playerstage-commit] code/player/server/drivers/ranger lasertoranger.cc, 1.2, 1.3 sonartoranger.cc, 1.1, 1.2
|
From: G B. <gb...@us...> - 2007-06-22 20:25:46
|
Update of /cvsroot/playerstage/code/player/server/drivers/ranger In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv22150/server/drivers/ranger Modified Files: lasertoranger.cc sonartoranger.cc Log Message: Fixed some bugs in the ranger interface and drivers Index: sonartoranger.cc =================================================================== RCS file: /cvsroot/playerstage/code/player/server/drivers/ranger/sonartoranger.cc,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** sonartoranger.cc 20 May 2007 00:30:15 -0000 1.1 --- sonartoranger.cc 23 Jun 2007 03:25:46 -0000 1.2 *************** *** 286,289 **** --- 286,298 ---- return 0; } + // Config get request + else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_GET_CONFIG, device_addr)) + { + // No config for this device, so send back a pile of zeroes + player_ranger_config_t resp; + memset (&resp, 0, sizeof (resp)); + Publish (device_addr, ret_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_RANGER_REQ_GET_CONFIG, &resp, sizeof (resp), NULL); + return 0; + } Index: lasertoranger.cc =================================================================== RCS file: /cvsroot/playerstage/code/player/server/drivers/ranger/lasertoranger.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** lasertoranger.cc 17 Jun 2007 00:28:01 -0000 1.2 --- lasertoranger.cc 23 Jun 2007 03:25:46 -0000 1.3 *************** *** 110,114 **** bool HandleGeomRequest (player_laser_geom_t *geom); // Handle config values ! void HandleGetConfigResp (player_laser_config_t *data); player_laser_config_t laserConfig; // Stored laser config --- 110,114 ---- bool HandleGeomRequest (player_laser_geom_t *geom); // Handle config values ! void HandleConfigResp (player_ranger_config_t *dest, player_laser_config_t *data); player_laser_config_t laserConfig; // Stored laser config *************** *** 315,318 **** --- 315,331 ---- } + void LaserToRanger::HandleConfigResp (player_ranger_config_t *dest, player_laser_config_t *data) + { + // Save a copy of the laser config, we need it to set intensity and/or config later + memcpy (&laserConfig, data, sizeof (player_laser_config_t)); + // Copy it into the destination + dest->min_angle = data->min_angle; + dest->max_angle = data->max_angle; + dest->resolution = data->resolution; + dest->max_range = data->max_range; + dest->range_res = data->range_res; + dest->frequency = data->scanning_frequency; + } + int LaserToRanger::ProcessMessage (MessageQueue *respQueue, player_msghdr *hdr, void *data) { *************** *** 325,328 **** --- 338,343 ---- HANDLE_CAPABILITY_REQUEST (device_addr, respQueue, hdr, data, PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_INTNS); HANDLE_CAPABILITY_REQUEST (device_addr, respQueue, hdr, data, PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_GET_GEOM); + HANDLE_CAPABILITY_REQUEST (device_addr, respQueue, hdr, data, PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_SET_CONFIG); + HANDLE_CAPABILITY_REQUEST (device_addr, respQueue, hdr, data, PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_GET_CONFIG); // Messages from the ranger interface *************** *** 345,348 **** --- 360,364 ---- inputDevice->PutMsg (InQueue, PLAYER_MSGTYPE_REQ, PLAYER_LASER_REQ_SET_CONFIG, &laserConfig, sizeof (laserConfig), 0); lastReqType = PLAYER_RANGER_REQ_INTNS; + ret_queue = respQueue; return 0; } *************** *** 368,371 **** --- 384,388 ---- inputDevice->PutMsg (InQueue, PLAYER_MSGTYPE_REQ, PLAYER_LASER_REQ_SET_CONFIG, &laserConfig, sizeof (laserConfig), 0); lastReqType = PLAYER_RANGER_REQ_SET_CONFIG; + ret_queue = respQueue; return 0; } *************** *** 376,379 **** --- 393,397 ---- inputDevice->PutMsg (InQueue, PLAYER_MSGTYPE_REQ, PLAYER_LASER_REQ_GET_CONFIG, NULL, 0, NULL); lastReqType = PLAYER_RANGER_REQ_GET_CONFIG; + ret_queue = respQueue; return 0; } *************** *** 391,396 **** { // Set config may have been triggered by either a ranger set config request or a ranger set intensity request ! memcpy (&laserConfig, data, sizeof (player_laser_config_t)); ! Publish (device_addr, ret_queue, PLAYER_MSGTYPE_RESP_ACK, lastReqType, &laserConfig, sizeof (laserConfig), NULL); return 0; } --- 409,418 ---- { // Set config may have been triggered by either a ranger set config request or a ranger set intensity request ! player_ranger_config_t resp; ! HandleConfigResp (&resp, reinterpret_cast<player_laser_config_t*> (data)); ! if (lastReqType == PLAYER_RANGER_REQ_SET_CONFIG) ! Publish (device_addr, ret_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_RANGER_REQ_SET_CONFIG, &resp, sizeof (resp), NULL); ! else if (lastReqType == PLAYER_RANGER_REQ_INTNS) ! Publish (device_addr, ret_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_RANGER_REQ_INTNS, NULL, 0, NULL); return 0; } *************** *** 398,404 **** { // Only way to get a get config response from the laser is via a ranger get config request, or in startup ! memcpy (&laserConfig, data, sizeof (player_laser_config_t)); if (lastReqType == PLAYER_RANGER_REQ_GET_CONFIG && startupComplete) ! Publish (device_addr, ret_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_RANGER_REQ_GET_CONFIG, &laserConfig, sizeof (laserConfig), NULL); else if (!startupComplete) { --- 420,432 ---- { // Only way to get a get config response from the laser is via a ranger get config request, or in startup ! /* printf ("Got laser config from device:\nmin_angle = %f\tmax_angle = %f\tresolution = %f\tmax_range = %f\trange_res = %f\tintensity = %d\tscanning_frequency = %f\n", ! reinterpret_cast<player_laser_config_t*> (data)->min_angle, reinterpret_cast<player_laser_config_t*> (data)->max_angle, ! reinterpret_cast<player_laser_config_t*> (data)->resolution, reinterpret_cast<player_laser_config_t*> (data)->max_range, ! reinterpret_cast<player_laser_config_t*> (data)->range_res, reinterpret_cast<player_laser_config_t*> (data)->intensity, ! reinterpret_cast<player_laser_config_t*> (data)->scanning_frequency);*/ ! player_ranger_config_t resp; ! HandleConfigResp (&resp, reinterpret_cast<player_laser_config_t*> (data)); if (lastReqType == PLAYER_RANGER_REQ_GET_CONFIG && startupComplete) ! Publish (device_addr, ret_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_RANGER_REQ_GET_CONFIG, &resp, sizeof (resp), NULL); else if (!startupComplete) { *************** *** 415,418 **** --- 443,447 ---- else Publish (device_addr, ret_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_RANGER_REQ_GET_GEOM, NULL, 0, NULL); + return 0; } |