Update of /cvsroot/playerstage/code/player/client_libs/libplayerc In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv12526/client_libs/libplayerc Modified Files: dev_bumper.c dev_motor.c client.c dev_imu.c dev_planner.c dev_position3d.c dev_limb.c dev_audio.c playerc.h dev_blackboard.c device.c dev_position2d.c dev_vectormap.c dev_log.c dev_position1d.c dev_opaque.c dev_gripper.c dev_sonar.c dev_localize.c dev_ir.c dev_wsn.c dev_laser.c dev_ptz.c dev_simulation.c dev_ranger.c dev_map.c dev_fiducial.c dev_actarray.c Removed Files: dev_truth.c Log Message: Changed playerc_client_request to make the need to cleanup returned structures obvious. Index: dev_ptz.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_ptz.c,v retrieving revision 1.16 retrieving revision 1.17 diff -C2 -d -r1.16 -r1.17 *** dev_ptz.c 17 Jan 2007 02:25:56 -0000 1.16 --- dev_ptz.c 20 Sep 2007 23:15:47 -0000 1.17 *************** *** 125,138 **** int playerc_ptz_query_status(playerc_ptz_t *device) { ! player_ptz_req_status_t cmd; if(playerc_client_request(device->info.client, &device->info, PLAYER_PTZ_REQ_STATUS, NULL, ! &cmd, sizeof(player_ptz_req_status_t)) < 0) return -1; ! device->status = cmd.status; return 0; } --- 125,139 ---- int playerc_ptz_query_status(playerc_ptz_t *device) { ! player_ptz_req_status_t *cmd; if(playerc_client_request(device->info.client, &device->info, PLAYER_PTZ_REQ_STATUS, NULL, ! (void**)&cmd) < 0) return -1; ! device->status = cmd->status; ! player_ptz_req_status_t_free(cmd); return 0; } *************** *** 165,168 **** &device->info, PLAYER_PTZ_REQ_CONTROL_MODE, ! &config, NULL, 0)); } --- 166,169 ---- &device->info, PLAYER_PTZ_REQ_CONTROL_MODE, ! &config, NULL)); } Index: dev_map.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_map.c,v retrieving revision 1.10 retrieving revision 1.11 diff -C2 -d -r1.10 -r1.11 *** dev_map.c 17 Sep 2007 02:18:51 -0000 1.10 --- dev_map.c 20 Sep 2007 23:15:47 -0000 1.11 *************** *** 99,105 **** int playerc_map_get_map(playerc_map_t* device) { ! player_map_info_t info_req; ! player_map_data_t* data_req; ! size_t repsize; int i,j; int oi,oj; --- 99,105 ---- int playerc_map_get_map(playerc_map_t* device) { ! player_map_info_t *info_req; ! player_map_data_t data_req, *data_resp; ! int i,j; int oi,oj; *************** *** 117,121 **** &device->info, PLAYER_MAP_REQ_GET_INFO, ! NULL, &info_req, sizeof(info_req)) < 0) { PLAYERC_ERR("failed to get map info"); --- 117,121 ---- &device->info, PLAYER_MAP_REQ_GET_INFO, ! NULL, (void**)&info_req) < 0) { PLAYERC_ERR("failed to get map info"); *************** *** 123,132 **** } ! device->resolution = info_req.scale; ! device->width = info_req.width; ! device->height = info_req.height; ! device->origin[0] = info_req.origin.px; ! device->origin[1] = info_req.origin.py; ! // Allocate space for the whole map if(device->cells) --- 123,134 ---- } ! device->resolution = info_req->scale; ! device->width = info_req->width; ! device->height = info_req->height; ! device->origin[0] = info_req->origin.px; ! device->origin[1] = info_req->origin.py; ! player_map_info_t_free(info_req); ! info_req=NULL; ! // Allocate space for the whole map if(device->cells) *************** *** 138,148 **** // now, get the map, in tiles - // Allocate space for one received tile. Note that we need to allocate - // space to hold the maximum possible tile, because the received tile - // will be unpacked into a structure of that size. - repsize = sizeof(player_map_data_t); - data_req = (player_map_data_t*)malloc(repsize); - assert(data_req); - #if HAVE_ZLIB_H // Allocate a buffer into which we'll decompress the map data --- 140,143 ---- *************** *** 161,176 **** sj = MIN(sy, device->height - oj); ! memset(data_req,0,repsize); ! data_req->col = oi; ! data_req->row = oj; ! data_req->width = si; ! data_req->height = sj; if(playerc_client_request(device->info.client, &device->info, PLAYER_MAP_REQ_GET_DATA, ! data_req, data_req, repsize) < 0) { PLAYERC_ERR("failed to get map data"); - free(data_req); free(device->cells); #if HAVE_ZLIB_H --- 156,170 ---- sj = MIN(sy, device->height - oj); ! memset(&data_req,0,sizeof(data_req)); ! data_req.col = oi; ! data_req.row = oj; ! data_req.width = si; ! data_req.height = sj; if(playerc_client_request(device->info.client, &device->info, PLAYER_MAP_REQ_GET_DATA, ! (void*)&data_req, (void**)&data_resp) < 0) { PLAYERC_ERR("failed to get map data"); free(device->cells); #if HAVE_ZLIB_H *************** *** 183,190 **** unzipped_data_len = PLAYER_MAP_MAX_TILE_SIZE; if(uncompress((Bytef*)unzipped_data, &unzipped_data_len, ! (uint8_t*)data_req->data, data_req->data_count) != Z_OK) { PLAYERC_ERR("failed to decompress map data"); ! free(data_req); free(device->cells); free(unzipped_data); --- 177,184 ---- unzipped_data_len = PLAYER_MAP_MAX_TILE_SIZE; if(uncompress((Bytef*)unzipped_data, &unzipped_data_len, ! (uint8_t*)data_resp->data, data_resp->data_count) != Z_OK) { PLAYERC_ERR("failed to decompress map data"); ! player_map_data_t_free(data_resp); free(device->cells); free(unzipped_data); *************** *** 202,206 **** *cell = unzipped_data[j*si + i]; #else ! *cell = data_req->data[j*si + i]; #endif } --- 196,200 ---- *cell = unzipped_data[j*si + i]; #else ! *cell = data_resp->data[j*si + i]; #endif } *************** *** 218,222 **** free(unzipped_data); #endif ! free(data_req); return(0); --- 212,216 ---- free(unzipped_data); #endif ! player_map_data_t_free(data_resp); return(0); *************** *** 226,246 **** playerc_map_get_vector(playerc_map_t* device) { - size_t repsize; player_map_data_vector_t* vmap; - // Allocate space for one received tile. Note that we need to allocate - // space to hold the maximum possible tile, because the received tile - // will be unpacked into a structure of that size. - repsize = sizeof(player_map_data_vector_t); - vmap = (player_map_data_vector_t*)malloc(repsize); - assert(vmap); - if(playerc_client_request(device->info.client, &device->info, PLAYER_MAP_REQ_GET_VECTOR, ! NULL, vmap, repsize) < 0) { PLAYERC_ERR("failed to get map vector data"); - free(vmap); return(-1); } --- 220,231 ---- playerc_map_get_vector(playerc_map_t* device) { player_map_data_vector_t* vmap; if(playerc_client_request(device->info.client, &device->info, PLAYER_MAP_REQ_GET_VECTOR, ! NULL, (void**)&vmap) < 0) { PLAYERC_ERR("failed to get map vector data"); return(-1); } *************** *** 262,266 **** vmap->segments, device->num_segments*sizeof(player_segment_t)); ! free(vmap); return(0); } --- 247,251 ---- vmap->segments, device->num_segments*sizeof(player_segment_t)); ! player_map_data_vector_t_free(vmap); return(0); } Index: dev_sonar.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_sonar.c,v retrieving revision 1.15 retrieving revision 1.16 diff -C2 -d -r1.15 -r1.16 *** dev_sonar.c 8 Oct 2005 02:00:18 -0000 1.15 --- dev_sonar.c 20 Sep 2007 23:15:47 -0000 1.16 *************** *** 140,156 **** int i; ! player_sonar_geom_t config; if(playerc_client_request(device->info.client, &device->info, PLAYER_SONAR_REQ_GET_GEOM, ! NULL, &config, sizeof(config)) < 0) return -1; ! device->pose_count = config.poses_count; for (i = 0; i < device->pose_count; i++) { ! device->poses[i] = config.poses[i]; } ! return 0; } --- 140,157 ---- int i; ! player_sonar_geom_t *config; if(playerc_client_request(device->info.client, &device->info, PLAYER_SONAR_REQ_GET_GEOM, ! NULL, (void**)&config) < 0) return -1; ! device->pose_count = config->poses_count; for (i = 0; i < device->pose_count; i++) { ! device->poses[i] = config->poses[i]; } ! player_sonar_geom_t_free(config); ! return 0; } Index: dev_wsn.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_wsn.c,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** dev_wsn.c 20 Aug 2007 06:37:26 -0000 1.4 --- dev_wsn.c 20 Sep 2007 23:15:47 -0000 1.5 *************** *** 125,129 **** &device->info, PLAYER_WSN_REQ_POWER, ! &config, NULL, 0)); } --- 125,129 ---- &device->info, PLAYER_WSN_REQ_POWER, ! &config, NULL)); } *************** *** 139,143 **** &device->info, PLAYER_WSN_REQ_DATATYPE, ! &config, NULL, 0)); } --- 139,143 ---- &device->info, PLAYER_WSN_REQ_DATATYPE, ! &config, NULL)); } *************** *** 156,159 **** &device->info, PLAYER_WSN_REQ_DATAFREQ, ! &config, NULL, 0)); } --- 156,159 ---- &device->info, PLAYER_WSN_REQ_DATAFREQ, ! &config, NULL)); } Index: dev_opaque.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_opaque.c,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** dev_opaque.c 14 Mar 2007 16:41:51 -0000 1.3 --- dev_opaque.c 20 Sep 2007 23:15:47 -0000 1.4 *************** *** 124,134 **** // Send a generic request ! int playerc_opaque_req(playerc_opaque_t *device, player_opaque_data_t *request, player_opaque_data_t *reply) { - /*int playerc_client_request(playerc_client_t *client, - struct _playerc_device_t *device, uint8_t reqtype, - void *req_data, void *rep_data, int rep_len);*/ return playerc_client_request(device->info.client, &device->info, PLAYER_OPAQUE_REQ, ! (void*)request, (void*)reply, sizeof(player_opaque_data_t)); } --- 124,131 ---- // Send a generic request ! int playerc_opaque_req(playerc_opaque_t *device, player_opaque_data_t *request, player_opaque_data_t **reply) { return playerc_client_request(device->info.client, &device->info, PLAYER_OPAQUE_REQ, ! (void*)request, (void**)reply); } Index: dev_ranger.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_ranger.c,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** dev_ranger.c 17 Jun 2007 00:28:01 -0000 1.2 --- dev_ranger.c 20 Sep 2007 23:15:47 -0000 1.3 *************** *** 214,224 **** int playerc_ranger_get_geom(playerc_ranger_t *device) { ! player_ranger_geom_t geom; if(playerc_client_request(device->info.client, &device->info, PLAYER_RANGER_REQ_GET_GEOM, ! NULL, &geom, sizeof(geom)) < 0) return -1; ! playerc_ranger_copy_geom(device, &geom); return 0; } --- 214,225 ---- int playerc_ranger_get_geom(playerc_ranger_t *device) { ! player_ranger_geom_t *geom; if(playerc_client_request(device->info.client, &device->info, PLAYER_RANGER_REQ_GET_GEOM, ! NULL, (void**)&geom) < 0) return -1; ! playerc_ranger_copy_geom(device, geom); ! player_ranger_geom_t_free(geom); return 0; } *************** *** 233,237 **** if(playerc_client_request(device->info.client, &device->info, PLAYER_RANGER_REQ_POWER, ! &req, NULL, 0) < 0) return -1; --- 234,238 ---- if(playerc_client_request(device->info.client, &device->info, PLAYER_RANGER_REQ_POWER, ! &req, NULL) < 0) return -1; *************** *** 247,251 **** if(playerc_client_request(device->info.client, &device->info, PLAYER_RANGER_REQ_INTNS, ! &req, NULL, 0) < 0) return -1; --- 248,252 ---- if(playerc_client_request(device->info.client, &device->info, PLAYER_RANGER_REQ_INTNS, ! &req, NULL) < 0) return -1; *************** *** 259,263 **** double frequency) { ! player_ranger_config_t config; config.min_angle = min_angle; --- 260,264 ---- double frequency) { ! player_ranger_config_t config, *resp; config.min_angle = min_angle; *************** *** 270,283 **** if(playerc_client_request(device->info.client, &device->info, PLAYER_RANGER_REQ_SET_CONFIG, ! (void*)&config, &config, sizeof(config)) < 0) return -1; ! device->min_angle = config.min_angle; ! device->max_angle = config.max_angle; ! device->resolution = config.resolution; ! device->max_range = config.max_range; ! device->range_res = config.range_res; ! device->frequency = config.frequency; ! return 0; } --- 271,284 ---- if(playerc_client_request(device->info.client, &device->info, PLAYER_RANGER_REQ_SET_CONFIG, ! (void*)&config, (void**)&resp) < 0) return -1; ! device->min_angle = resp->min_angle; ! device->max_angle = resp->max_angle; ! device->resolution = resp->resolution; ! device->max_range = resp->max_range; ! device->range_res = resp->range_res; ! device->frequency = resp->frequency; ! player_ranger_config_t_free(resp); return 0; } *************** *** 289,306 **** double *frequency) { ! player_ranger_config_t config; if(playerc_client_request(device->info.client, &device->info, PLAYER_RANGER_REQ_GET_CONFIG, ! NULL, &config, sizeof(config)) < 0) return(-1); ! device->min_angle = config.min_angle; ! device->max_angle = config.max_angle; ! device->resolution = config.resolution; ! device->max_range = config.max_range; ! device->range_res = config.range_res; ! device->frequency = config.frequency; ! if (min_angle != NULL) *min_angle = device->min_angle; --- 290,307 ---- double *frequency) { ! player_ranger_config_t *config; if(playerc_client_request(device->info.client, &device->info, PLAYER_RANGER_REQ_GET_CONFIG, ! NULL, (void**)&config) < 0) return(-1); ! device->min_angle = config->min_angle; ! device->max_angle = config->max_angle; ! device->resolution = config->resolution; ! device->max_range = config->max_range; ! device->range_res = config->range_res; ! device->frequency = config->frequency; ! player_ranger_config_t_free(config); if (min_angle != NULL) *min_angle = device->min_angle; Index: dev_planner.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_planner.c,v retrieving revision 1.5 retrieving revision 1.6 diff -C2 -d -r1.5 -r1.6 *** dev_planner.c 30 Aug 2005 00:08:15 -0000 1.5 --- dev_planner.c 20 Sep 2007 23:15:47 -0000 1.6 *************** *** 145,163 **** { int i; ! player_planner_waypoints_req_t config; if(playerc_client_request(device->info.client, &device->info, PLAYER_PLANNER_REQ_GET_WAYPOINTS, ! NULL, &config, sizeof(config)) < 0) return -1; ! device->waypoint_count = config.waypoints_count; for(i=0;i<device->waypoint_count;i++) { ! device->waypoints[i][0] = config.waypoints[i].px; ! device->waypoints[i][1] = config.waypoints[i].py; ! device->waypoints[i][2] = config.waypoints[i].pa; } return 0; } --- 145,164 ---- { int i; ! player_planner_waypoints_req_t *config; if(playerc_client_request(device->info.client, &device->info, PLAYER_PLANNER_REQ_GET_WAYPOINTS, ! NULL, (void**)&config) < 0) return -1; ! device->waypoint_count = config->waypoints_count; for(i=0;i<device->waypoint_count;i++) { ! device->waypoints[i][0] = config->waypoints[i].px; ! device->waypoints[i][1] = config->waypoints[i].py; ! device->waypoints[i][2] = config->waypoints[i].pa; } + player_planner_waypoints_req_t_free(config); return 0; } *************** *** 172,176 **** if(playerc_client_request(device->info.client, &device->info, PLAYER_PLANNER_REQ_ENABLE, ! &config, NULL, 0) < 0) return(-1); else --- 173,177 ---- if(playerc_client_request(device->info.client, &device->info, PLAYER_PLANNER_REQ_ENABLE, ! &config, NULL) < 0) return(-1); else Index: dev_gripper.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_gripper.c,v retrieving revision 1.8 retrieving revision 1.9 diff -C2 -d -r1.8 -r1.9 *** dev_gripper.c 20 Aug 2007 06:37:26 -0000 1.8 --- dev_gripper.c 20 Sep 2007 23:15:47 -0000 1.9 *************** *** 156,171 **** int playerc_gripper_get_geom (playerc_gripper_t *device) { ! player_gripper_geom_t config; if(playerc_client_request(device->info.client,&device->info, PLAYER_GRIPPER_REQ_GET_GEOM, ! NULL, &config, sizeof (config)) < 0) return -1; ! device->pose = config.pose; ! device->outer_size = config.outer_size; ! device->inner_size = config.inner_size; ! device->num_beams = config.num_beams; ! device->capacity = config.capacity; ! return 0; } --- 156,171 ---- int playerc_gripper_get_geom (playerc_gripper_t *device) { ! player_gripper_geom_t *config; if(playerc_client_request(device->info.client,&device->info, PLAYER_GRIPPER_REQ_GET_GEOM, ! NULL, (void**)&config) < 0) return -1; ! device->pose = config->pose; ! device->outer_size = config->outer_size; ! device->inner_size = config->inner_size; ! device->num_beams = config->num_beams; ! device->capacity = config->capacity; ! player_gripper_geom_t_free(config); return 0; } Index: dev_laser.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_laser.c,v retrieving revision 1.42 retrieving revision 1.43 diff -C2 -d -r1.42 -r1.43 *** dev_laser.c 9 Jul 2007 17:17:58 -0000 1.42 --- dev_laser.c 20 Sep 2007 23:15:47 -0000 1.43 *************** *** 190,194 **** double scanning_frequency) { ! player_laser_config_t config; config.min_angle = min_angle; --- 190,194 ---- double scanning_frequency) { ! player_laser_config_t config, *resp; config.min_angle = min_angle; *************** *** 201,213 **** if(playerc_client_request(device->info.client, &device->info, PLAYER_LASER_REQ_SET_CONFIG, ! (void*)&config, &config, sizeof(config)) < 0) return -1; // copy them locally ! device->scan_start = config.min_angle; ! device->scan_res = config.resolution; ! device->range_res = config.range_res; ! device->intensity_on = config.intensity; ! device->scanning_frequency = config.scanning_frequency; return 0; --- 201,214 ---- if(playerc_client_request(device->info.client, &device->info, PLAYER_LASER_REQ_SET_CONFIG, ! (void*)&config, (void**)&resp) < 0) return -1; // copy them locally ! device->scan_start = resp->min_angle; ! device->scan_res = resp->resolution; ! device->range_res = resp->range_res; ! device->intensity_on = resp->intensity; ! device->scanning_frequency = resp->scanning_frequency; ! player_laser_config_t_free(resp); return 0; *************** *** 224,243 **** double *scanning_frequency) { ! player_laser_config_t config; if(playerc_client_request(device->info.client, &device->info, PLAYER_LASER_REQ_GET_CONFIG, ! NULL, &config, sizeof(config)) < 0) return(-1); ! *min_angle = device->scan_start = config.min_angle; ! *max_angle = config.max_angle; ! *resolution = config.resolution; device->scan_res = *resolution; ! *intensity = device->intensity_on = config.intensity; ! *range_res = config.range_res; ! *scanning_frequency = config.scanning_frequency; device->range_res = *range_res; ! device->max_range = config.max_range; return 0; } --- 225,245 ---- double *scanning_frequency) { ! player_laser_config_t *config; if(playerc_client_request(device->info.client, &device->info, PLAYER_LASER_REQ_GET_CONFIG, ! NULL, (void**)&config) < 0) return(-1); ! *min_angle = device->scan_start = config->min_angle; ! *max_angle = config->max_angle; ! *resolution = config->resolution; device->scan_res = *resolution; ! *intensity = device->intensity_on = config->intensity; ! *range_res = config->range_res; ! *scanning_frequency = config->scanning_frequency; device->range_res = *range_res; ! device->max_range = config->max_range; ! player_laser_config_t_free(config); return 0; } *************** *** 248,264 **** playerc_laser_get_geom(playerc_laser_t *device) { ! player_laser_geom_t config; if(playerc_client_request(device->info.client, &device->info,PLAYER_LASER_REQ_GET_GEOM, ! NULL, &config, sizeof(config)) < 0) return -1; ! device->pose[0] = config.pose.px; ! device->pose[1] = config.pose.py; ! device->pose[2] = config.pose.pyaw; ! device->size[0] = config.size.sl; ! device->size[1] = config.size.sw; ! return 0; } --- 250,267 ---- playerc_laser_get_geom(playerc_laser_t *device) { ! player_laser_geom_t *config; if(playerc_client_request(device->info.client, &device->info,PLAYER_LASER_REQ_GET_GEOM, ! NULL, (void**)&config) < 0) return -1; ! device->pose[0] = config->pose.px; ! device->pose[1] = config->pose.py; ! device->pose[2] = config->pose.pyaw; ! device->size[0] = config->size.sl; ! device->size[1] = config->size.sw; ! player_laser_geom_t_free(config); ! return 0; } *************** *** 269,280 **** playerc_laser_get_id (playerc_laser_t *device) { ! player_laser_get_id_config_t config; if (playerc_client_request(device->info.client, &device->info,PLAYER_LASER_REQ_GET_ID, ! NULL, &config, sizeof(config)) < 0) return -1; ! device->laser_id = config.serial_number; return 0; --- 272,284 ---- playerc_laser_get_id (playerc_laser_t *device) { ! player_laser_get_id_config_t *config; if (playerc_client_request(device->info.client, &device->info,PLAYER_LASER_REQ_GET_ID, ! NULL, (void**)&config) < 0) return -1; ! device->laser_id = config->serial_number; ! player_laser_get_id_config_t_free(config); return 0; Index: playerc.h =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/playerc.h,v retrieving revision 1.232 retrieving revision 1.233 diff -C2 -d -r1.232 -r1.233 *** playerc.h 20 Sep 2007 02:50:47 -0000 1.232 --- playerc.h 20 Sep 2007 23:15:47 -0000 1.233 *************** *** 669,672 **** --- 669,677 ---- /** @brief Issue a request to the server and await a reply (blocking). @internal + The rep_data pointer is filled with a pointer to the response data recieved. It is + the callers responisbility to free this memory with the approriate player _free method. + + If an error is returned then no data will have been stored in rep_data. + @returns Returns -1 on error and -2 on NACK. *************** *** 674,678 **** int playerc_client_request(playerc_client_t *client, struct _playerc_device_t *device, uint8_t reqtype, ! void *req_data, void *rep_data, int rep_len); /** @brief Wait for response from server (blocking). --- 679,683 ---- int playerc_client_request(playerc_client_t *client, struct _playerc_device_t *device, uint8_t reqtype, ! void *req_data, void **rep_data); /** @brief Wait for response from server (blocking). *************** *** 2274,2279 **** int playerc_opaque_cmd(playerc_opaque_t *device, player_opaque_data_t *data); ! /** @brief Send a generic request */ ! int playerc_opaque_req(playerc_opaque_t *device, player_opaque_data_t *request, player_opaque_data_t *reply); /** @} */ --- 2279,2289 ---- int playerc_opaque_cmd(playerc_opaque_t *device, player_opaque_data_t *data); ! /** @brief Send a generic request ! * ! * If a non null value is passed for reply memory for the response will be allocated ! * and its pointer stored in reply. The caller is responsible for freeing this memory ! * ! * If an error is returned no memory will have been allocated*/ ! int playerc_opaque_req(playerc_opaque_t *device, player_opaque_data_t *request, player_opaque_data_t **reply); /** @} */ Index: dev_simulation.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_simulation.c,v retrieving revision 1.14 retrieving revision 1.15 diff -C2 -d -r1.14 -r1.15 *** dev_simulation.c 20 Apr 2007 00:59:47 -0000 1.14 --- dev_simulation.c 20 Sep 2007 23:15:47 -0000 1.15 *************** *** 118,122 **** return playerc_client_request(device->info.client, &device->info, PLAYER_SIMULATION_REQ_SET_POSE2D, ! &cmd, NULL, 0); } --- 118,122 ---- return playerc_client_request(device->info.client, &device->info, PLAYER_SIMULATION_REQ_SET_POSE2D, ! &cmd, NULL); } *************** *** 125,129 **** double *x, double *y, double *a) { ! player_simulation_pose2d_req_t cfg; memset(&cfg, 0, sizeof(cfg)); --- 125,129 ---- double *x, double *y, double *a) { ! player_simulation_pose2d_req_t cfg, *resp; memset(&cfg, 0, sizeof(cfg)); *************** *** 133,141 **** if (playerc_client_request(device->info.client, &device->info, PLAYER_SIMULATION_REQ_GET_POSE2D, ! &cfg, &cfg, sizeof(cfg)) < 0) return (-1); ! *x = cfg.pose.px; ! *y = cfg.pose.py; ! *a = cfg.pose.pa; return 0; } --- 133,142 ---- if (playerc_client_request(device->info.client, &device->info, PLAYER_SIMULATION_REQ_GET_POSE2D, ! &cfg, (void**)&resp) < 0) return (-1); ! *x = resp->pose.px; ! *y = resp->pose.py; ! *a = resp->pose.pa; ! player_simulation_pose2d_req_t_free(resp); return 0; } *************** *** 160,164 **** return playerc_client_request(device->info.client, &device->info, PLAYER_SIMULATION_REQ_SET_POSE3D, ! &cmd, NULL, 0); } --- 161,165 ---- return playerc_client_request(device->info.client, &device->info, PLAYER_SIMULATION_REQ_SET_POSE3D, ! &cmd, NULL); } *************** *** 167,171 **** double *x, double *y, double *z, double *roll, double *pitch, double *yaw, double *time) { ! player_simulation_pose3d_req_t cfg; memset(&cfg, 0, sizeof(cfg)); --- 168,172 ---- double *x, double *y, double *z, double *roll, double *pitch, double *yaw, double *time) { ! player_simulation_pose3d_req_t cfg, *resp; memset(&cfg, 0, sizeof(cfg)); *************** *** 175,187 **** if (playerc_client_request(device->info.client, &device->info, PLAYER_SIMULATION_REQ_GET_POSE3D, ! &cfg, &cfg, sizeof(cfg)) < 0) return (-1); ! *x = cfg.pose.px; ! *y = cfg.pose.py; ! *z = cfg.pose.pz; ! *pitch = cfg.pose.ppitch; ! *roll = cfg.pose.proll; ! *yaw = cfg.pose.pyaw; ! *time = cfg.simtime; return 0; } --- 176,189 ---- if (playerc_client_request(device->info.client, &device->info, PLAYER_SIMULATION_REQ_GET_POSE3D, ! &cfg, (void**)&resp) < 0) return (-1); ! *x = resp->pose.px; ! *y = resp->pose.py; ! *z = resp->pose.pz; ! *pitch = resp->pose.ppitch; ! *roll = resp->pose.proll; ! *yaw = resp->pose.pyaw; ! *time = resp->simtime; ! player_simulation_pose3d_req_t_free(resp); return 0; } *************** *** 217,221 **** return playerc_client_request(device->info.client, &device->info, PLAYER_SIMULATION_REQ_SET_PROPERTY, ! &req, NULL, 0); } --- 219,223 ---- return playerc_client_request(device->info.client, &device->info, PLAYER_SIMULATION_REQ_SET_PROPERTY, ! &req, NULL); } *************** *** 227,231 **** size_t value_len ) { ! player_simulation_property_req_t req; memset(&req, 0, sizeof(req)); --- 229,233 ---- size_t value_len ) { ! player_simulation_property_req_t req, *resp; memset(&req, 0, sizeof(req)); *************** *** 249,257 **** if( playerc_client_request(device->info.client, &device->info, PLAYER_SIMULATION_REQ_GET_PROPERTY, ! &req, &req, sizeof(req)) < 0) return -1; ! memcpy(value, req.value, value_len); ! return 0; } --- 251,259 ---- if( playerc_client_request(device->info.client, &device->info, PLAYER_SIMULATION_REQ_GET_PROPERTY, ! &req, (void**)&resp) < 0) return -1; ! memcpy(value, resp->value, value_len); ! player_simulation_property_req_t_free(resp); return 0; } Index: dev_position2d.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_position2d.c,v retrieving revision 1.10 retrieving revision 1.11 diff -C2 -d -r1.10 -r1.11 *** dev_position2d.c 9 Jul 2007 17:17:58 -0000 1.10 --- dev_position2d.c 20 Sep 2007 23:15:47 -0000 1.11 *************** *** 131,135 **** &device->info, PLAYER_POSITION2D_REQ_MOTOR_POWER, ! &config, NULL, 0)); } --- 131,135 ---- &device->info, PLAYER_POSITION2D_REQ_MOTOR_POWER, ! &config, NULL)); } *************** *** 143,147 **** return(playerc_client_request(device->info.client, &device->info, PLAYER_POSITION2D_REQ_VELOCITY_MODE, ! &config, NULL, 0)); } --- 143,147 ---- return(playerc_client_request(device->info.client, &device->info, PLAYER_POSITION2D_REQ_VELOCITY_MODE, ! &config, NULL)); } *************** *** 151,167 **** playerc_position2d_get_geom(playerc_position2d_t *device) { ! player_position2d_geom_t geom; if(playerc_client_request(device->info.client, &device->info, PLAYER_POSITION2D_REQ_GET_GEOM, ! NULL, (void*)&geom, sizeof(geom)) < 0) return(-1); ! device->pose[0] = geom.pose.px; ! device->pose[1] = geom.pose.py; ! device->pose[2] = geom.pose.pyaw; ! device->size[0] = geom.size.sl; ! device->size[1] = geom.size.sw; ! return(0); } --- 151,167 ---- playerc_position2d_get_geom(playerc_position2d_t *device) { ! player_position2d_geom_t *geom; if(playerc_client_request(device->info.client, &device->info, PLAYER_POSITION2D_REQ_GET_GEOM, ! NULL, (void**)&geom) < 0) return(-1); ! device->pose[0] = geom->pose.px; ! device->pose[1] = geom->pose.py; ! device->pose[2] = geom->pose.pyaw; ! device->size[0] = geom->size.sl; ! device->size[1] = geom->size.sw; ! player_position2d_geom_t_free(geom); return(0); } *************** *** 251,255 **** &device->info, PLAYER_POSITION2D_REQ_SET_ODOM, ! &req, NULL, 0)); } --- 251,255 ---- &device->info, PLAYER_POSITION2D_REQ_SET_ODOM, ! &req, NULL)); } Index: dev_ir.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_ir.c,v retrieving revision 1.7 retrieving revision 1.8 diff -C2 -d -r1.7 -r1.8 *** dev_ir.c 20 Aug 2007 06:37:26 -0000 1.7 --- dev_ir.c 20 Sep 2007 23:15:47 -0000 1.8 *************** *** 116,121 **** int playerc_ir_get_geom(playerc_ir_t *device) { ! return playerc_client_request(device->info.client, &device->info,PLAYER_IR_REQ_POSE, ! 0, &device->poses, sizeof(device->poses)); } --- 116,128 ---- int playerc_ir_get_geom(playerc_ir_t *device) { ! player_ir_pose_t *geom; ! int ret; ! ret = playerc_client_request(device->info.client, &device->info,PLAYER_IR_REQ_POSE, NULL, (void**)&geom); ! if (ret < 0) ! return ret; ! player_ir_pose_t_copy(&device->poses, geom); ! player_ir_pose_t_free(geom); ! return 0; ! } Index: dev_localize.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_localize.c,v retrieving revision 1.16 retrieving revision 1.17 diff -C2 -d -r1.16 -r1.17 *** dev_localize.c 26 Jan 2006 13:31:11 -0000 1.16 --- dev_localize.c 20 Sep 2007 23:15:47 -0000 1.17 *************** *** 138,142 **** &device->info, PLAYER_LOCALIZE_REQ_SET_POSE, ! &req, NULL, 0) < 0) { printf("%s\n", playerc_error_str()); --- 138,142 ---- &device->info, PLAYER_LOCALIZE_REQ_SET_POSE, ! &req, NULL) < 0) { printf("%s\n", playerc_error_str()); *************** *** 152,172 **** { int i; ! player_localize_get_particles_t req; if(playerc_client_request(device->info.client, &device->info, PLAYER_LOCALIZE_REQ_GET_PARTICLES, ! NULL, ! &req, sizeof(player_localize_get_particles_t)) < 0) return -1; ! device->mean[0] = req.mean.px; ! device->mean[1] = req.mean.py; ! device->mean[2] = req.mean.pa; ! device->variance = req.variance; ! device->num_particles = req.particles_count; for(i=0;i<device->num_particles;i++) --- 152,171 ---- { int i; ! player_localize_get_particles_t *req; if(playerc_client_request(device->info.client, &device->info, PLAYER_LOCALIZE_REQ_GET_PARTICLES, ! NULL, (void**) &req) < 0) return -1; ! device->mean[0] = req->mean.px; ! device->mean[1] = req->mean.py; ! device->mean[2] = req->mean.pa; ! device->variance = req->variance; ! device->num_particles = req->particles_count; for(i=0;i<device->num_particles;i++) *************** *** 179,188 **** } ! device->particles[i].pose[0] = req.particles[i].pose.px; ! device->particles[i].pose[1] = req.particles[i].pose.py; ! device->particles[i].pose[2] = req.particles[i].pose.pa; ! device->particles[i].weight = req.particles[i].alpha; } ! return 0; } --- 178,187 ---- } ! device->particles[i].pose[0] = req->particles[i].pose.px; ! device->particles[i].pose[1] = req->particles[i].pose.py; ! device->particles[i].pose[2] = req->particles[i].pose.pa; ! device->particles[i].weight = req->particles[i].alpha; } ! player_localize_get_particles_t_free(req); return 0; } Index: dev_log.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_log.c,v retrieving revision 1.7 retrieving revision 1.8 diff -C2 -d -r1.7 -r1.8 *** dev_log.c 7 Sep 2005 18:41:12 -0000 1.7 --- dev_log.c 20 Sep 2007 23:15:47 -0000 1.8 *************** *** 90,106 **** int playerc_log_get_state(playerc_log_t* device) { ! player_log_get_state_t req; if(playerc_client_request(device->info.client, &device->info, PLAYER_LOG_REQ_GET_STATE, ! NULL, &req, sizeof(req)) < 0) { PLAYERC_ERR("failed to get logging/playback state"); return(-1); } ! device->type = req.type; ! device->state = req.state; ! return(0); } --- 90,106 ---- int playerc_log_get_state(playerc_log_t* device) { ! player_log_get_state_t *req; if(playerc_client_request(device->info.client, &device->info, PLAYER_LOG_REQ_GET_STATE, ! NULL, (void**)&req) < 0) { PLAYERC_ERR("failed to get logging/playback state"); return(-1); } ! device->type = req->type; ! device->state = req->state; ! player_log_get_state_t_free(req); return(0); } *************** *** 115,119 **** if(playerc_client_request(device->info.client, &device->info,PLAYER_LOG_REQ_SET_WRITE_STATE, ! &req, &req, sizeof(req)) < 0) { PLAYERC_ERR("failed to start/stop data logging"); --- 115,119 ---- if(playerc_client_request(device->info.client, &device->info,PLAYER_LOG_REQ_SET_WRITE_STATE, ! &req, NULL) < 0) { PLAYERC_ERR("failed to start/stop data logging"); *************** *** 132,136 **** if(playerc_client_request(device->info.client, &device->info, PLAYER_LOG_REQ_SET_READ_STATE, ! &req, NULL, 0) < 0) { PLAYERC_ERR("failed to start/stop data playback"); --- 132,136 ---- if(playerc_client_request(device->info.client, &device->info, PLAYER_LOG_REQ_SET_READ_STATE, ! &req, NULL) < 0) { PLAYERC_ERR("failed to start/stop data playback"); *************** *** 145,149 **** if(playerc_client_request(device->info.client, &device->info, PLAYER_LOG_REQ_SET_READ_REWIND, ! NULL, NULL, 0) < 0) { PLAYERC_ERR("failed to rewind data playback"); --- 145,149 ---- if(playerc_client_request(device->info.client, &device->info, PLAYER_LOG_REQ_SET_READ_REWIND, ! NULL, NULL) < 0) { PLAYERC_ERR("failed to rewind data playback"); *************** *** 169,173 **** if(playerc_client_request(device->info.client, &device->info, PLAYER_LOG_REQ_SET_FILENAME, ! &req, NULL, 0) < 0) { PLAYERC_ERR("failed to set logfile name"); --- 169,173 ---- if(playerc_client_request(device->info.client, &device->info, PLAYER_LOG_REQ_SET_FILENAME, ! &req, NULL) < 0) { PLAYERC_ERR("failed to set logfile name"); Index: dev_fiducial.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_fiducial.c,v retrieving revision 1.16 retrieving revision 1.17 diff -C2 -d -r1.16 -r1.17 *** dev_fiducial.c 20 Aug 2007 06:37:26 -0000 1.16 --- dev_fiducial.c 20 Sep 2007 23:15:47 -0000 1.17 *************** *** 137,153 **** PLAYERC_WARN( "playerc_fiducial_putgeom is not yet implemented" ); - /* if (len != sizeof(player_fiducial_geom_t)) */ - /* { */ - /* PLAYERC_ERR2("reply has unexpected length (%d != %d)", len, sizeof(player_fiducial_geom_t)); */ - /* return; */ - /* } */ - - /* device->pose[0] = ((int16_t) ntohs(data->pose[0])) / 1000.0; */ - /* device->pose[1] = ((int16_t) ntohs(data->pose[1])) / 1000.0; */ - /* device->pose[2] = ((int16_t) ntohs(data->pose[2])) * M_PI / 180; */ - /* device->size[0] = ((int16_t) ntohs(data->size[0])) / 1000.0; */ - /* device->size[1] = ((int16_t) ntohs(data->size[1])) / 1000.0; */ - /* device->fiducial_size[0] = ((int16_t) ntohs(data->fiducial_size[0])) / 1000.0; */ - /* device->fiducial_size[1] = ((int16_t) ntohs(data->fiducial_size[1])) / 1000.0; */ } --- 137,140 ---- *************** *** 157,184 **** { int len; ! player_fiducial_geom_t config; ! ! // config.subtype = PLAYER_FIDUCIAL_REQ_GET_GEOM; len = playerc_client_request(device->info.client, &device->info, PLAYER_FIDUCIAL_REQ_GET_GEOM, ! NULL, &config, sizeof(config)); if (len < 0) return -1; ! // ? ! //while(device->info.freshgeom == 0) ! // playerc_client_read(device->info.client); ! ! device->fiducial_geom = config; ! /* device->pose[1] = config.pose.py; ! device->pose[2] = config.pose.pa; ! ! device->size[0] = config.size.sl; ! device->size[1] = config.size.sw; ! ! device->fiducial_size[0] = config.fiducial_size.sl; ! device->fiducial_size[1] = config.fiducial_size.sw;*/ return 0; --- 144,158 ---- { int len; ! player_fiducial_geom_t *config; len = playerc_client_request(device->info.client, &device->info, PLAYER_FIDUCIAL_REQ_GET_GEOM, ! NULL, (void**)&config); if (len < 0) return -1; ! player_fiducial_geom_t_copy(&device->fiducial_geom, config); ! player_fiducial_geom_t_free(config); return 0; Index: dev_limb.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_limb.c,v retrieving revision 1.6 retrieving revision 1.7 diff -C2 -d -r1.6 -r1.7 *** dev_limb.c 20 Aug 2007 06:37:26 -0000 1.6 --- dev_limb.c 20 Sep 2007 23:15:47 -0000 1.7 *************** *** 109,123 **** int playerc_limb_get_geom(playerc_limb_t *device) { ! player_limb_geom_req_t geom; if(playerc_client_request(device->info.client, &device->info, PLAYER_LIMB_REQ_GEOM, ! NULL, (void*)&geom, sizeof(geom)) < 0) return -1; ! device->geom.basePos.px = geom.basePos.px; ! device->geom.basePos.py = geom.basePos.py; ! device->geom.basePos.pz = geom.basePos.pz; ! return 0; } --- 109,123 ---- int playerc_limb_get_geom(playerc_limb_t *device) { ! player_limb_geom_req_t *geom; if(playerc_client_request(device->info.client, &device->info, PLAYER_LIMB_REQ_GEOM, ! NULL, (void**)&geom) < 0) return -1; ! device->geom.basePos.px = geom->basePos.px; ! device->geom.basePos.py = geom->basePos.py; ! device->geom.basePos.pz = geom->basePos.pz; ! player_limb_geom_req_t_free(geom); return 0; } *************** *** 202,206 **** return playerc_client_request(device->info.client, &device->info, PLAYER_LIMB_REQ_POWER, ! &config, NULL, 0); } --- 202,206 ---- return playerc_client_request(device->info.client, &device->info, PLAYER_LIMB_REQ_POWER, ! &config, NULL); } *************** *** 214,218 **** return playerc_client_request(device->info.client, &device->info, PLAYER_LIMB_REQ_BRAKES, ! &config, NULL, 0); } --- 214,218 ---- return playerc_client_request(device->info.client, &device->info, PLAYER_LIMB_REQ_BRAKES, ! &config, NULL); } *************** *** 226,229 **** return playerc_client_request(device->info.client, &device->info, PLAYER_LIMB_REQ_SPEED, ! &config, NULL, 0); } --- 226,229 ---- return playerc_client_request(device->info.client, &device->info, PLAYER_LIMB_REQ_SPEED, ! &config, NULL); } Index: client.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/client.c,v retrieving revision 1.79 retrieving revision 1.80 diff -C2 -d -r1.79 -r1.80 *** client.c 20 Aug 2007 06:37:25 -0000 1.79 --- client.c 20 Sep 2007 23:15:47 -0000 1.80 *************** *** 481,485 **** req.replace = replace; ! if (playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_ADD_REPLACE_RULE, &req, NULL, 0) < 0) return -1; --- 481,485 ---- req.replace = replace; ! if (playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_ADD_REPLACE_RULE, &req, NULL) < 0) return -1; *************** *** 495,499 **** req.mode = mode; ! if (playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DATAMODE, &req, NULL, 0) < 0) return -1; --- 495,499 ---- req.mode = mode; ! if (playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DATAMODE, &req, NULL) < 0) return -1; *************** *** 512,516 **** // req.subtype = htons(PLAYER_PLAYER_DATA_REQ); ! return(playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DATA, &req, NULL, 0)); } --- 512,516 ---- // req.subtype = htons(PLAYER_PLAYER_DATA_REQ); ! return(playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DATA, &req, NULL)); } *************** *** 652,656 **** playerc_device_t *deviceinfo, uint8_t subtype, ! void *req_data, void *rep_data, int rep_len) { double t; --- 652,656 ---- playerc_device_t *deviceinfo, uint8_t subtype, ! void *req_data, void **rep_data) { double t; *************** *** 714,724 **** return -1; } ! else if(rep_header.size > rep_len) { ! PLAYERC_ERR2("insufficient space to store the reply (%u > %u)", ! rep_header.size, rep_len); ! return -1; } - memcpy(rep_data, client->data, rep_header.size); return(0); } --- 714,723 ---- return -1; } ! memcpy(rep_data, client->data, rep_header.size); ! if (rep_data) { ! *rep_data = client->data; ! client->data = NULL; } return(0); } *************** *** 781,790 **** { int i; ! player_device_devlist_t config; memset(&config,0,sizeof(config)); if(playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DEVLIST, ! &config, &config, sizeof(config)) < 0) { PLAYERC_ERR("failed to get response"); --- 780,789 ---- { int i; ! player_device_devlist_t config, *rep_config; memset(&config,0,sizeof(config)); if(playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DEVLIST, ! &config, (void**)&rep_config) < 0) { PLAYERC_ERR("failed to get response"); *************** *** 792,799 **** } ! for (i = 0; i < config.devices_count; i++) ! client->devinfos[i].addr = config.devices[i]; ! client->devinfo_count = config.devices_count; // Now get the driver info return playerc_client_get_driverinfo(client); --- 791,800 ---- } ! for (i = 0; i < rep_config->devices_count; i++) ! client->devinfos[i].addr = rep_config->devices[i]; ! client->devinfo_count = rep_config->devices_count; + player_device_devlist_t_free(rep_config); + // Now get the driver info return playerc_client_get_driverinfo(client); *************** *** 806,810 **** { int i; ! player_device_driverinfo_t req; for (i = 0; i < client->devinfo_count; i++) --- 807,811 ---- { int i; ! player_device_driverinfo_t req, *resp; for (i = 0; i < client->devinfo_count; i++) *************** *** 814,818 **** if(playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DRIVERINFO, ! &req, &req, sizeof(req)) < 0) { PLAYERC_ERR("failed to get response"); --- 815,819 ---- if(playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DRIVERINFO, ! &req, (void**)&resp) < 0) { PLAYERC_ERR("failed to get response"); *************** *** 820,826 **** } ! strncpy(client->devinfos[i].drivername, req.driver_name, ! req.driver_name_count); ! client->devinfos[i].drivername[req.driver_name_count] = '\0'; } --- 821,829 ---- } ! strncpy(client->devinfos[i].drivername, resp->driver_name, ! resp->driver_name_count); ! client->devinfos[i].drivername[resp->driver_name_count] = '\0'; ! ! player_device_driverinfo_t_free(resp); } *************** *** 833,837 **** int access, char *drivername, size_t len) { ! player_device_req_t req; req.addr.host = 0; --- 836,840 ---- int access, char *drivername, size_t len) { ! player_device_req_t req, *resp; req.addr.host = 0; *************** *** 843,847 **** if (playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DEV, ! (void*)&req, (void*)&req, sizeof(req)) < 0) { PLAYERC_ERR("failed to get response"); --- 846,850 ---- if (playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DEV, ! (void*)&req, (void**)&resp) < 0) { PLAYERC_ERR("failed to get response"); *************** *** 856,860 **** // Copy the driver name ! strncpy(drivername, req.driver_name, len); return 0; --- 859,864 ---- // Copy the driver name ! strncpy(drivername, resp->driver_name, len); ! player_device_req_t_free(resp); return 0; *************** *** 865,870 **** int playerc_client_unsubscribe(playerc_client_t *client, int code, int index) { ! player_device_req_t req; ! req.addr.host = 0; req.addr.robot = 0; --- 869,875 ---- int playerc_client_unsubscribe(playerc_client_t *client, int code, int index) { ! player_device_req_t req, *resp; ! int ret; ! req.addr.host = 0; req.addr.robot = 0; *************** *** 875,888 **** if (playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DEV, ! (void*)&req, (void*)&req, sizeof(req)) < 0) return -1; ! if (req.access != PLAYER_CLOSE_MODE) { ! PLAYERC_ERR2("requested [%d] access, but got [%d] access", PLAYER_CLOSE_MODE, req.access); ! return -1; } ! return 0; } --- 880,898 ---- if (playerc_client_request(client, NULL, PLAYER_PLAYER_REQ_DEV, ! (void*)&req, (void**)&resp) < 0) return -1; ! if (resp->access != PLAYER_CLOSE_MODE) { ! PLAYERC_ERR2("requested [%d] access, but got [%d] access", PLAYER_CLOSE_MODE, resp->access); ! ret = -1; ! } ! else ! { ! ret = 0; } ! player_device_req_t_free(resp); ! return ret; } Index: dev_blackboard.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_blackboard.c,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** dev_blackboard.c 20 Sep 2007 02:50:47 -0000 1.1 --- dev_blackboard.c 20 Sep 2007 23:15:47 -0000 1.2 *************** *** 98,102 **** req.key = strdup(key); req.key_count = strlen(key) + 1; - size_t entry_size = sizeof(req) + req.key_count; if (playerc_client_request( --- 98,101 ---- *************** *** 104,111 **** &device->info, PLAYER_BLACKBOARD_REQ_SUBSCRIBE_TO_KEY, ! &req, ! entry_out, ! entry_size) < 0) { PLAYERC_ERR("failed to subscribe to blackboard key"); return -1; --- 103,109 ---- &device->info, PLAYER_BLACKBOARD_REQ_SUBSCRIBE_TO_KEY, ! &req, NULL) < 0) { + free(req.key); PLAYERC_ERR("failed to subscribe to blackboard key"); return -1; *************** *** 123,127 **** req.key = strdup(key); req.key_count = strlen(key) + 1; - size_t entry_size = sizeof(req) + req.key_count; if (playerc_client_request( --- 121,124 ---- *************** *** 130,136 **** PLAYER_BLACKBOARD_REQ_UNSUBSCRIBE_FROM_KEY, &req, ! NULL, ! entry_size) < 0) { PLAYERC_ERR("failed to unsubscribe to blackboard key"); return -1; --- 127,133 ---- PLAYER_BLACKBOARD_REQ_UNSUBSCRIBE_FROM_KEY, &req, ! NULL) < 0) { + free(req.key); PLAYERC_ERR("failed to unsubscribe to blackboard key"); return -1; *************** *** 145,158 **** int playerc_blackboard_set_entry(playerc_blackboard_t *device, player_blackboard_entry_t* entry) { - size_t entry_size = sizeof(entry) + entry->key_count + entry->data_count; - player_blackboard_entry_t *reply; - if (playerc_client_request( device->info.client, &device->info, PLAYER_BLACKBOARD_REQ_SET_ENTRY, ! entry, ! &reply, ! entry_size) < 0) { PLAYERC_ERR("failed to set blackboard key"); --- 142,150 ---- int playerc_blackboard_set_entry(playerc_blackboard_t *device, player_blackboard_entry_t* entry) { if (playerc_client_request( device->info.client, &device->info, PLAYER_BLACKBOARD_REQ_SET_ENTRY, ! entry, NULL) < 0) { PLAYERC_ERR("failed to set blackboard key"); Index: dev_motor.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_motor.c,v retrieving revision 1.5 retrieving revision 1.6 diff -C2 -d -r1.5 -r1.6 *** dev_motor.c 14 Mar 2007 16:41:51 -0000 1.5 --- dev_motor.c 20 Sep 2007 23:15:47 -0000 1.6 *************** *** 127,131 **** &device->info, PLAYER_MOTOR_REQ_POWER, ! &config, NULL, 0)); } --- 127,131 ---- &device->info, PLAYER_MOTOR_REQ_POWER, ! &config, NULL)); } *************** *** 139,143 **** return(playerc_client_request(device->info.client, &device->info, PLAYER_MOTOR_REQ_VELOCITY_MODE, ! &config, NULL, 0)); } --- 139,143 ---- return(playerc_client_request(device->info.client, &device->info, PLAYER_MOTOR_REQ_VELOCITY_MODE, ! &config, NULL)); } *************** *** 187,191 **** &device->info, PLAYER_MOTOR_REQ_SET_ODOM, ! &req, NULL, 0)); } --- 187,191 ---- &device->info, PLAYER_MOTOR_REQ_SET_ODOM, ! &req, NULL)); } Index: dev_bumper.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_bumper.c,v retrieving revision 1.6 retrieving revision 1.7 diff -C2 -d -r1.6 -r1.7 *** dev_bumper.c 20 Aug 2007 06:37:26 -0000 1.6 --- dev_bumper.c 20 Sep 2007 23:15:47 -0000 1.7 *************** *** 131,147 **** { int i; ! player_bumper_geom_t config; // config.subtype = PLAYER_BUMPER_REQ_GET_GEOM; if (playerc_client_request(device->info.client, &device->info, PLAYER_BUMPER_REQ_GET_GEOM, ! NULL, &config, sizeof(config)) < 0) return -1; ! device->pose_count = config.bumper_def_count; for (i = 0; i < device->pose_count; i++) { ! device->poses[i] = config.bumper_def[i]; } ! return 0; } --- 131,148 ---- { int i; ! player_bumper_geom_t *config; // config.subtype = PLAYER_BUMPER_REQ_GET_GEOM; if (playerc_client_request(device->info.client, &device->info, PLAYER_BUMPER_REQ_GET_GEOM, ! NULL, (void**)&config) < 0) return -1; ! device->pose_count = config->bumper_def_count; for (i = 0; i < device->pose_count; i++) { ! device->poses[i] = config->bumper_def[i]; } ! ! player_bumper_geom_t_free(config); return 0; } Index: dev_position1d.c =================================================================== RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/dev_position1d.c,v retrieving revision 1.5 retrieving revision 1.6 diff -C2 -d -r1.5 -r1.6 *** dev_position1d.c 9 Jul 2007 17:17:58 -0000 1.5 --- dev_position1d.c 20 Sep 2007 23:15:47 -0000 1.6 *************** *** 126,130 **** &device->info, PLAYER_POSITION1D_REQ_MOTOR_POWER, ! &config, NULL, 0)); } --- 126,130 ---- &device->info, PLAYER_POSITION1D_REQ_MOTOR_POWER, ! &config, NULL)); } *************** *** 134,150 **** playerc_position1d_get_geom(playerc_position1d_t *device) { ! player_position1d_geom_t geom; if(playerc_client_request(device->info.client, &device->info, ... [truncated message content] |