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;
}
-------------------------------------------------------------------------
This SF.net email is sponsored by DB2 Express
Download DB2 Express C - the FREE version of DB2 express and take
control of your XML. No limits. Just data. Click to get it now.
http://sourceforge.net/powerbar/db2/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
