Update of /cvsroot/playerstage/code/player/server/drivers/shell
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv31174
Modified Files:
readlog.cc writelog.cc
Log Message:
Added support for PointCloud3d logging.
Index: writelog.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/shell/writelog.cc,v
retrieving revision 1.73
retrieving revision 1.74
diff -C2 -d -r1.73 -r1.74
*** writelog.cc 11 Sep 2006 20:14:13 -0000 1.73
--- writelog.cc 11 Sep 2006 22:13:17 -0000 1.74
***************
*** 77,80 ****
--- 77,81 ----
- @ref interface_wsn
- @ref interface_imu
+ - @ref interface_pointcloud3d
The following interfaces are supported in principle but are currently
***************
*** 213,216 ****
--- 214,220 ----
// Write IMU data to file
private: int WriteIMU (player_msghdr_t* hdr, void *data);
+
+ // Write PointCloud3D data to file
+ private: int WritePointCloud3d (player_msghdr_t* hdr, void *data);
#if 0
// Write blobfinder data to file
***************
*** 715,718 ****
--- 719,725 ----
retval = this->WriteIMU (hdr, data);
break;
+ case PLAYER_POINTCLOUD3D_CODE:
+ retval = this->WritePointCloud3d (hdr, data);
+ break;
#if 0
case PLAYER_BLOBFINDER_CODE:
***************
*** 1213,1222 ****
@brief IMU log format
! The format for each @ref interface_wsn message is:
! - magn_x (float): magnetic measurement on X-axis from a magnetometer
! - magn_y (float): magnetic measurement on Y-axis from a magnetometer
! - magn_z (float): magnetic measurement on Z-axis from a magnetometer
! - temperature (float): temperature measurement from a temperature sensor
! - battery (float): remaining battery voltage
*/
int
--- 1220,1265 ----
@brief IMU log format
! The format for each @ref interface_imu message is:
! - for PLAYER_IMU_DATA_STATE (player_imu_data_state_t):
! -> px (float): X pose
! -> py (float): Y pose
! -> pz (float): Z pose
! -> proll (float): roll angle
! -> ppitch (float): pitch angle
! -> pyaw (float): yaw angle
! - for PLAYER_IMU_DATA_CALIB (player_imu_data_calib_t):
! -> accel_x (float): acceleration value for X axis
! -> accel_y (float): acceleration value for Y axis
! -> accel_z (float): acceleration value for Z axis
! -> gyro_x (float): gyroscope value for X axis
! -> gyro_y (float): gyroscope value for Y axis
! -> gyro_z (float): gyroscope value for Z axis
! -> magn_x (float): magnetometer value for X axis
! -> magn_y (float): magnetometer value for Y axis
! -> magn_z (float): magnetometer value for Z axis
! - for PLAYER_IMU_DATA_QUAT (player_imu_data_quat_t):
! -> accel_x (float): acceleration value for X axis
! -> accel_y (float): acceleration value for Y axis
! -> accel_z (float): acceleration value for Z axis
! -> gyro_x (float): gyroscope value for X axis
! -> gyro_y (float): gyroscope value for Y axis
! -> gyro_z (float): gyroscope value for Z axis
! -> magn_x (float): magnetometer value for X axis
! -> magn_y (float): magnetometer value for Y axis
! -> magn_z (float): magnetometer value for Z axis
! -> q0, q1, q2, q3 (floats): quaternion values
! - for PLAYER_IMU_DATA_EULER (player_imu_data_euler_t):
! -> accel_x (float): acceleration value for X axis
! -> accel_y (float): acceleration value for Y axis
! -> accel_z (float): acceleration value for Z axis
! -> gyro_x (float): gyroscope value for X axis
! -> gyro_y (float): gyroscope value for Y axis
! -> gyro_z (float): gyroscope value for Z axis
! -> magn_x (float): magnetometer value for X axis
! -> magn_y (float): magnetometer value for Y axis
! -> magn_z (float): magnetometer value for Z axis
! -> proll (float): roll angle
! -> ppitch (float): pitch angle
! -> pyaw (float): yaw angle
*/
int
***************
*** 1311,1314 ****
--- 1354,1402 ----
}
+ /** @ingroup tutorial_datalog
+ * @defgroup player_driver_writelog_pointcloud3d pointcloud3d format
+
+ @brief PointCloud3D log format
+
+ The format for each @ref interface_pointcloud3d message is:
+ - points_count (int): the number of elements in the 3d point cloud
+ - list of elements; for each element:
+ - point.px (float): X [m]
+ - point.py (float): Y [m]
+ - point.pz (float): Z [m]
+ */
+ int
+ WriteLog::WritePointCloud3d (player_msghdr_t* hdr, void *data)
+ {
+ unsigned int i;
+ // Check the type
+ switch(hdr->type)
+ {
+ case PLAYER_MSGTYPE_DATA:
+ // Check the subtype
+ switch(hdr->subtype)
+ {
+ case PLAYER_POINTCLOUD3D_DATA_STATE:
+ {
+ player_pointcloud3d_data_t* pdata;
+ pdata = (player_pointcloud3d_data_t*)data;
+ fprintf (this->file, "%d ", pdata->points_count);
+ for (i = 0; i < pdata->points_count; i++)
+ fprintf (this->file,"%f %f %f ",
+ pdata->points[i].point.px,
+ pdata->points[i].point.py,
+ pdata->points[i].point.pz);
+ return (0);
+ }
+
+ default:
+ return (-1);
+ }
+
+ default:
+ return (-1);
+ }
+ }
+
#if 0
/** @ingroup tutorial_datalog
Index: readlog.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/shell/readlog.cc,v
retrieving revision 1.41
retrieving revision 1.42
diff -C2 -d -r1.41 -r1.42
*** readlog.cc 11 Sep 2006 20:14:13 -0000 1.41
--- readlog.cc 11 Sep 2006 22:13:17 -0000 1.42
***************
*** 63,66 ****
--- 63,67 ----
- @ref interface_wsn
- @ref interface_imu
+ - @ref interface_pointcloud3d
The following interfaces are supported in principle but are currently
***************
*** 277,284 ****
// Parse IMU data
! private: int ParseIMU(player_devaddr_t id,
! unsigned short type, unsigned short subtype,
! int linenum,
! int token_count, char **tokens, double time);
#if 0
--- 278,291 ----
// Parse IMU data
! private: int ParseIMU (player_devaddr_t id,
! unsigned short type, unsigned short subtype,
! int linenum,
! int token_count, char **tokens, double time);
!
! // Parse PointCloud3D data
! private: int ParsePointCloud3d (player_devaddr_t id,
! unsigned short type, unsigned short subtype,
! int linenum,
! int token_count, char **tokens, double time);
#if 0
***************
*** 1149,1152 ****
--- 1156,1162 ----
return this->ParseIMU (id, type, subtype, linenum,
token_count, tokens, time);
+ else if (id.interf == PLAYER_POINTCLOUD3D_CODE)
+ return this->ParsePointCloud3d (id, type, subtype, linenum,
+ token_count, tokens, time);
#if 0
***************
*** 1958,1961 ****
--- 1968,2019 ----
}
+ ////////////////////////////////////////////////////////////////////////////
+ // Parse PointCloud3d data
+ int ReadLog::ParsePointCloud3d (player_devaddr_t id,
+ unsigned short type, unsigned short subtype,
+ int linenum,
+ int token_count, char **tokens, double time)
+ {
+ unsigned int i;
+ switch(type)
+ {
+ case PLAYER_MSGTYPE_DATA:
+ switch(subtype)
+ {
+ case PLAYER_POINTCLOUD3D_DATA_STATE:
+ {
+ player_pointcloud3d_data_t data;
+ data.points_count = atoi (tokens[7]);
+ if (token_count < (int)(7+data.points_count))
+ {
+ PLAYER_ERROR2("invalid line at %s:%d",
this->filename, linenum);
+ return -1;
+ }
+ for (i = 0; i < data.points_count; i++)
+ {
+ player_pointcloud3d_element element;
+ player_point_3d_t point;
+ point.px = atof (tokens[8+i]);
+ point.py = atof (tokens[9+i]);
+ point.pz = atof (tokens[10+i]);
+ element.point = point;
+ data.points[i] = element;
+ }
+
+ this->Publish (id, NULL, type, subtype,
+ (void*)&data, sizeof(data), &time);
+ return (0);
+ }
+
+ default:
+ PLAYER_ERROR1 ("unknown PointCloud3d data subtype %d\n",
subtype);
+ return (-1);
+ }
+ default:
+ PLAYER_ERROR1 ("unknown PointCloud3d message type %d\n", type);
+ return (-1);
+ }
+ }
+
#if 0
////////////////////////////////////////////////////////////////////////////
-------------------------------------------------------------------------
Using Tomcat but need to do more? Need to support web services, security?
Get stuff done quickly with pre-integrated technology to make your job easier
Download IBM WebSphere Application Server v.1.0.1 based on Apache Geronimo
http://sel.as-us.falkag.net/sel?cmd=lnk&kid=120709&bid=263057&dat=121642
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
