Update of /cvsroot/playerstage/code/player/server/drivers/ranger
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv25524/server/drivers/ranger
Modified Files:
lasertoranger.cc toranger.cc toranger.h
Log Message:
Changes to ranger interface (moving some properties to messages).
Index: toranger.h
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/ranger/toranger.h,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** toranger.h 20 May 2007 00:30:15 -0000 1.1
--- toranger.h 17 Jun 2007 00:28:01 -0000 1.2
***************
*** 45,65 ****
protected:
- // Function called when a property has been changed so it can
be passed on to the input driver
- virtual bool PropertyChanged (void) = 0;
-
- // Property request processor
- int ProcessProperty (MessageQueue *respQueue, player_msghdr
*hdr, void *data);
-
// Ranger interface stuff - should be filled by ProcessMessage()
player_ranger_geom_t deviceGeom; //
Geometry of the device
- // Properties - should be set by GetDataFromSource()
- double minAngle;
// Minimum scan angle
- double maxAngle;
// Maximum scan angle
- double resolution;
// Scan resolution
- double maxRange;
// Scan range (m)
- double rangeRes;
// Range resolution (m)
- double frequency;
// Scanning frequency
-
// Input device
Device *inputDevice;
// Input device interface
--- 45,51 ----
Index: lasertoranger.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/ranger/lasertoranger.cc,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** lasertoranger.cc 20 May 2007 00:30:15 -0000 1.1
--- lasertoranger.cc 17 Jun 2007 00:28:01 -0000 1.2
***************
*** 101,106 ****
// Child message handler, for handling messages from the input
device
int ProcessMessage (MessageQueue *respQueue, player_msghdr
*hdr, void *data);
- // Function called when a property has been changed so it can
be passed on to the input driver
- bool PropertyChanged (void);
// Set power state
int SetPower (MessageQueue *respQueue, player_msghdr *hdr,
uint8_t state);
--- 101,104 ----
***************
*** 111,117 ****
// Convert geometry data
bool HandleGeomRequest (player_laser_geom_t *geom);
! uint8_t intensityState; // Intensity
data state
! bool lastReqWasPropSet; // True if the
last request sent to the laser was to set a property
};
--- 109,119 ----
// Convert geometry data
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
!
! uint8_t lastReqType; // Last request
type on the ranger interface
! bool startupComplete; // True once
this driver is ready to go
};
***************
*** 152,155 ****
--- 154,160 ----
return -1;
+ lastReqType = 0;
+ startupComplete = false;
+
// Subscribe to the laser.
if ((inputDevice = deviceTable->GetDevice (inputDeviceAddr)) == NULL)
***************
*** 165,169 ****
}
! // Request the config from the laser to fill in the properties
inputDevice->PutMsg (InQueue, PLAYER_MSGTYPE_REQ,
PLAYER_LASER_REQ_GET_CONFIG, NULL, 0, NULL);
--- 170,174 ----
}
! // Request the config from the laser to fill in the config values
inputDevice->PutMsg (InQueue, PLAYER_MSGTYPE_REQ,
PLAYER_LASER_REQ_GET_CONFIG, NULL, 0, NULL);
***************
*** 201,222 ****
////////////////////////////////////////////////////////////////////////////////
- bool LaserToRanger::PropertyChanged (void)
- {
- // Make a config request to the laser with the properties
- player_laser_config_t req;
-
- req.min_angle = minAngle;
- req.max_angle = maxAngle;
- req.resolution = resolution;
- req.max_range = maxRange;
- req.range_res = rangeRes;
- req.intensity = intensityState;
- req.scanning_frequency = frequency;
-
- inputDevice->PutMsg (InQueue, PLAYER_MSGTYPE_REQ,
PLAYER_LASER_REQ_SET_CONFIG, &req, sizeof (req), 0);
-
- return true;
- }
-
int LaserToRanger::ConvertData (player_msghdr *hdr, void *data)
{
--- 206,209 ----
***************
*** 255,262 ****
{
// Update the properties from the data
! minAngle = scanData->min_angle;
! maxAngle = scanData->max_angle;
! resolution = scanData->resolution;
! maxRange = scanData->max_range;
// Copy out the range data
--- 242,249 ----
{
// Update the properties from the data
! laserConfig.min_angle = scanData->min_angle;
! laserConfig.max_angle = scanData->max_angle;
! laserConfig.resolution = scanData->resolution;
! laserConfig.max_range = scanData->max_range;
// Copy out the range data
***************
*** 355,361 ****
{
// Tell the laser device to give intensity data
! intensityState =
reinterpret_cast<player_ranger_intns_config_t*> (data)->state;
! PropertyChanged ();
! lastReqWasPropSet = false;
return 0;
}
--- 342,348 ----
{
// Tell the laser device to give intensity data
! laserConfig.intensity =
reinterpret_cast<player_ranger_intns_config_t*> (data)->state;
! inputDevice->PutMsg (InQueue, PLAYER_MSGTYPE_REQ,
PLAYER_LASER_REQ_SET_CONFIG, &laserConfig, sizeof (laserConfig), 0);
! lastReqType = PLAYER_RANGER_REQ_INTNS;
return 0;
}
***************
*** 368,371 ****
--- 355,381 ----
return 0;
}
+ // Config set request
+ else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ,
PLAYER_RANGER_REQ_SET_CONFIG, device_addr))
+ {
+ // Translate and forward the request to the laser device
+ player_ranger_config_t *req =
reinterpret_cast<player_ranger_config_t*> (data);
+ laserConfig.min_angle = req->min_angle;
+ laserConfig.max_angle = req->max_angle;
+ laserConfig.resolution = req->resolution;
+ laserConfig.max_range = req->max_range;
+ laserConfig.range_res = req->range_res;
+ laserConfig.scanning_frequency = req->frequency;
+ inputDevice->PutMsg (InQueue, PLAYER_MSGTYPE_REQ,
PLAYER_LASER_REQ_SET_CONFIG, &laserConfig, sizeof (laserConfig), 0);
+ lastReqType = PLAYER_RANGER_REQ_SET_CONFIG;
+ return 0;
+ }
+ // Config get request
+ else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ,
PLAYER_RANGER_REQ_GET_CONFIG, device_addr))
+ {
+ // Forward the request onto the laser device, will handle the
response when it comes
+ inputDevice->PutMsg (InQueue, PLAYER_MSGTYPE_REQ,
PLAYER_LASER_REQ_GET_CONFIG, NULL, 0, NULL);
+ lastReqType = PLAYER_RANGER_REQ_GET_CONFIG;
+ return 0;
+ }
***************
*** 380,401 ****
else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_LASER_REQ_SET_CONFIG, inputDeviceAddr))
{
! // Config request (may have been triggered by a propset on the
ranger interface)
! if (lastReqWasPropSet)
! Publish (device_addr, ret_queue,
PLAYER_MSGTYPE_RESP_ACK, PLAYER_SET_DBLPROP_REQ, NULL, 0, NULL);
! else
! Publish (device_addr, ret_queue,
PLAYER_MSGTYPE_RESP_ACK, PLAYER_RANGER_REQ_INTNS, NULL, 0, NULL);
return 0;
}
else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_LASER_REQ_GET_CONFIG, inputDeviceAddr))
{
! // Copy the config values into the properties
! player_laser_config_t *req =
reinterpret_cast<player_laser_config_t*> (data);
! minAngle = req->min_angle;
! maxAngle = req->max_angle;
! resolution = req->resolution;
! maxRange = req->max_range;
! rangeRes = req->range_res;
! frequency = req->scanning_frequency;
! // Get config requests can only come from this driver, not
clients to this driver, so no need to send a message to clients
return 0;
}
--- 390,409 ----
else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_LASER_REQ_SET_CONFIG, inputDeviceAddr))
{
! // 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;
}
else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_LASER_REQ_GET_CONFIG, inputDeviceAddr))
{
! // 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)
! {
! PLAYER_MSG0 (1, "LaserToRanger startup complete");
! startupComplete = true;
! }
return 0;
}
***************
*** 418,426 ****
else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_NACK,
PLAYER_LASER_REQ_SET_CONFIG, inputDeviceAddr))
{
! // Config request (may have been triggered by a propset on the
ranger interface)
! if (lastReqWasPropSet)
! Publish (device_addr, ret_queue,
PLAYER_MSGTYPE_RESP_NACK, PLAYER_SET_DBLPROP_REQ, NULL, 0, NULL);
! else
! Publish (device_addr, ret_queue,
PLAYER_MSGTYPE_RESP_NACK, PLAYER_RANGER_REQ_INTNS, NULL, 0, NULL);
return 0;
}
--- 426,443 ----
else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_NACK,
PLAYER_LASER_REQ_SET_CONFIG, inputDeviceAddr))
{
! // Set config may have been triggered by either a ranger set
config request or a ranger set intensity request
! Publish (device_addr, ret_queue, PLAYER_MSGTYPE_RESP_NACK,
lastReqType, NULL, 0, NULL);
! return 0;
! }
! else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_RESP_NACK,
PLAYER_LASER_REQ_GET_CONFIG, inputDeviceAddr))
! {
! // Only way to get a get config response from the laser is via
a ranger get config request, or in startup
! if (lastReqType == PLAYER_RANGER_REQ_GET_CONFIG &&
startupComplete)
! Publish (device_addr, ret_queue,
PLAYER_MSGTYPE_RESP_NACK, PLAYER_RANGER_REQ_GET_CONFIG, NULL, 0, NULL);
! else if (!startupComplete)
! {
! PLAYER_MSG0 (1, "LaserToRanger startup failed to get
config from device");
! startupComplete = true;
! }
return 0;
}
Index: toranger.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/ranger/toranger.cc,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** toranger.cc 20 May 2007 00:30:15 -0000 1.1
--- toranger.cc 17 Jun 2007 00:28:01 -0000 1.2
***************
*** 57,67 ****
// Clean output
memset (&deviceGeom, 0, sizeof (deviceGeom));
- // Clean properties
- minAngle = 0.0f;
- maxAngle = 0.0f;
- resolution = 0.0f;
- maxRange = 0.0f;
- rangeRes = 0.0f;
- frequency = 0.0f;
return 0;
--- 57,60 ----
***************
*** 96,104 ****
HANDLE_CAPABILITY_REQUEST (device_addr, respQueue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_CAPABILTIES_REQ);
! // Override default handling of properties
! if (ProcessProperty (respQueue, hdr, data) == 0)
! return 0;
!
! // Pass other property get/set messages through to the input device
if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ,
PLAYER_GET_INTPROP_REQ, device_addr) ||
Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ,
PLAYER_SET_INTPROP_REQ, device_addr) ||
--- 89,93 ----
HANDLE_CAPABILITY_REQUEST (device_addr, respQueue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_CAPABILTIES_REQ);
! // Pass property get/set messages through to the input device
if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ,
PLAYER_GET_INTPROP_REQ, device_addr) ||
Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ,
PLAYER_SET_INTPROP_REQ, device_addr) ||
***************
*** 133,189 ****
return -1;
}
-
- // Property processing
- // This overrides the default handling of properties from the Driver class.
- // It only handles double properties, and only those we know about (the 5
member variables).
- // Anything else returns -1, so the Driver class property handling will catch
it.
- int ToRanger::ProcessProperty (MessageQueue *respQueue, player_msghdr *hdr,
void *data)
- {
- if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_GET_DBLPROP_REQ, device_addr))
- {
- player_dblprop_req_t *req =
reinterpret_cast<player_dblprop_req_t*> (data);
- if (strcmp (req->key, "min_angle") == 0)
- req->value = minAngle;
- else if (strcmp (req->key, "max_angle") == 0)
- req->value = maxAngle;
- else if (strcmp (req->key, "resolution") == 0)
- req->value = resolution;
- else if (strcmp (req->key, "max_range") == 0)
- req->value = maxRange;
- else if (strcmp (req->key, "range_res") == 0)
- req->value = rangeRes;
- else if (strcmp (req->key, "frequency") == 0)
- req->value = frequency;
- else
- return -1;
-
- printf ("Handling prop get request for property %s, returning
value %f\n", req->key, req->value);
- Publish (device_addr, respQueue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_GET_DBLPROP_REQ, reinterpret_cast<void*> (req),
sizeof(player_dblprop_req_t), NULL);
- return 0;
- }
- else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_SET_DBLPROP_REQ, device_addr))
- {
- player_dblprop_req_t *req =
reinterpret_cast<player_dblprop_req_t*> (data);
- if (strcmp (req->key, "min_angle") == 0)
- minAngle = req->value;
- else if (strcmp (req->key, "max_angle") == 0)
- maxAngle = req->value;
- else if (strcmp (req->key, "resolution") == 0)
- resolution = req->value;
- else if (strcmp (req->key, "max_range") == 0)
- maxRange = req->value;
- else if (strcmp (req->key, "range_res") == 0)
- rangeRes = req->value;
- else if (strcmp (req->key, "frequency") == 0)
- frequency = req->value;
- else
- return -1;
-
- // Notify the input device of the new property
- // Prop set request ACK will be sent when a reply is received
from the input device
- PropertyChanged ();
- return 0;
- }
-
- return -1;
- }
--- 122,123 ----
-------------------------------------------------------------------------
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
