[Playerstage-commit] code/player/server/drivers/shell readlog.cc, 1.41, 1.42 writelog.cc, 1.73, 1.74

[Radu Bogdan Rusu <veedee@us...>]

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
  ////////////////////////////////////////////////////////////////////////////

[Playerstage-commit] code/player/server/drivers/shell readlog.cc, 1.40, 1.41 writelog.cc, 1.72, 1.73

[Radu Bogdan Rusu <veedee@us...>]

Update of /cvsroot/playerstage/code/player/server/drivers/shell
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv7744/drivers/shell

Modified Files:
	readlog.cc writelog.cc 
Log Message:
Added IMU logging capabilities.


Index: writelog.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/shell/writelog.cc,v
retrieving revision 1.72
retrieving revision 1.73
diff -C2 -d -r1.72 -r1.73
*** writelog.cc	1 Jul 2006 12:31:38 -0000	1.72
--- writelog.cc	11 Sep 2006 20:14:13 -0000	1.73
***************
*** 76,79 ****
--- 76,80 ----
  - @ref interface_wifi
  - @ref interface_wsn
+ - @ref interface_imu
  
  The following interfaces are supported in principle but are currently
***************
*** 125,129 ****
  @endverbatim
  
! @author Andrew Howard
  
  */
--- 126,130 ----
  @endverbatim
  
! @author Andrew Howard, Radu Bogdan Rusu
  
  */
***************
*** 208,212 ****
  
    // Write WSN data to file
!   private: int WriteWSN(player_msghdr_t* hdr, void *data);
  #if 0
    // Write blobfinder data to file
--- 209,216 ----
  
    // Write WSN data to file
!   private: int WriteWSN (player_msghdr_t* hdr, void *data);
!   
!   // Write IMU data to file
!   private: int WriteIMU (player_msghdr_t* hdr, void *data);
  #if 0
    // Write blobfinder data to file
***************
*** 708,711 ****
--- 712,718 ----
        retval = this->WriteWSN(hdr, data);
        break;
+     case PLAYER_IMU_CODE:
+       retval = this->WriteIMU (hdr, data);
+       break;
  #if 0
      case PLAYER_BLOBFINDER_CODE:
***************
*** 1201,1204 ****
--- 1208,1314 ----
  }
  
+ /** @ingroup tutorial_datalog
+  * @defgroup player_driver_writelog_imu IMU format
+ 
+ @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
+ WriteLog::WriteIMU (player_msghdr_t* hdr, void *data)
+ {
+   // Check the type
+     switch(hdr->type)
+     {
+         case PLAYER_MSGTYPE_DATA:
+       // Check the subtype
+             switch(hdr->subtype)
+             {
+                 case PLAYER_IMU_DATA_STATE:
+ 		{
+ 		    player_imu_data_state_t* idata;
+                     idata = (player_imu_data_state_t*)data;
+                     fprintf (this->file,"%f %f %f %f %f %f",
+                             idata->pose.px, 
+                             idata->pose.py, 
+                             idata->pose.pz, 
+                             idata->pose.proll, 
+                             idata->pose.ppitch, 
+                             idata->pose.pyaw);
+                     return (0);
+ 		}
+ 
+                 case PLAYER_IMU_DATA_CALIB:
+ 		{
+ 		    player_imu_data_calib_t* idata;
+                     idata = (player_imu_data_calib_t*)data;
+                     fprintf (this->file,"%f %f %f %f %f %f %f %f %f",
+                             idata->accel_x, 
+                             idata->accel_y, 
+                             idata->accel_z, 
+                             idata->gyro_x, 
+                             idata->gyro_y, 
+ 			    idata->gyro_z,
+ 			    idata->magn_x,
+ 			    idata->magn_y,
+                             idata->magn_z);
+                     return (0);
+ 		}
+ 
+                 case PLAYER_IMU_DATA_QUAT:
+ 		{
+ 		    player_imu_data_quat_t* idata;
+                     idata = (player_imu_data_quat_t*)data;
+                     fprintf (this->file,"%f %f %f %f %f %f %f %f %f %f %f %f %f",
+                             idata->calib_data.accel_x, 
+                             idata->calib_data.accel_y, 
+                             idata->calib_data.accel_z, 
+                             idata->calib_data.gyro_x, 
+                             idata->calib_data.gyro_y, 
+ 			    idata->calib_data.gyro_z,
+ 			    idata->calib_data.magn_x,
+ 			    idata->calib_data.magn_y,
+                             idata->calib_data.magn_z,
+                             idata->q0, 
+                             idata->q1, 
+                             idata->q2, 
+                             idata->q3);
+                     return (0);
+ 		}
+ 		
+                 case PLAYER_IMU_DATA_EULER:
+ 		{
+ 		    player_imu_data_euler_t* idata;
+                     idata = (player_imu_data_euler_t*)data;
+                     fprintf (this->file,"%f %f %f %f %f %f %f %f %f %f %f %f",
+                             idata->calib_data.accel_x, 
+                             idata->calib_data.accel_y, 
+                             idata->calib_data.accel_z, 
+                             idata->calib_data.gyro_x, 
+                             idata->calib_data.gyro_y, 
+ 			    idata->calib_data.gyro_z,
+ 			    idata->calib_data.magn_x,
+ 			    idata->calib_data.magn_y,
+                             idata->calib_data.magn_z,
+                             idata->orientation.proll, 
+                             idata->orientation.ppitch, 
+                             idata->orientation.pyaw);
+                     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.40
retrieving revision 1.41
diff -C2 -d -r1.40 -r1.41
*** readlog.cc	12 Apr 2006 13:33:31 -0000	1.40
--- readlog.cc	11 Sep 2006 20:14:13 -0000	1.41
***************
*** 62,65 ****
--- 62,66 ----
  - @ref interface_wifi
  - @ref interface_wsn
+ - @ref interface_imu
  
  The following interfaces are supported in principle but are currently
***************
*** 119,123 ****
  @endverbatim
  
! @author Andrew Howard
  
  */
--- 120,124 ----
  @endverbatim
  
! @author Andrew Howard, Radu Bogdan Rusu
  
  */
***************
*** 200,203 ****
--- 201,209 ----
                                  void * data);
  
+   // Process IMU interface configuration requests
+   private: int ProcessIMUConfig(MessageQueue * resp_queue,
+                                 player_msghdr_t * hdr,
+                                 void * data);
+ 
    // Parse the header info
    private: int ParseHeader(int linenum, int token_count, char **tokens,
***************
*** 269,272 ****
--- 275,284 ----
                          int linenum,
                          int token_count, char **tokens, double time);
+ 
+   // 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
  
***************
*** 969,972 ****
--- 981,1018 ----
  
  int
+ ReadLog::ProcessIMUConfig(MessageQueue * resp_queue,
+                           player_msghdr_t * hdr,
+                           void * data)
+ {
+     switch(hdr->subtype)
+     {
+         case PLAYER_IMU_REQ_SET_DATATYPE:
+         {
+         // Find the right place from which to retrieve it
+             int j;
+             for(j=0;j<this->provide_count;j++)
+             {
+                 if(Device::MatchDeviceAddress(this->provide_ids[j], hdr->addr))
+                     break;
+             }
+             if(j>=this->provide_count)
+                 return(-1);
+ 
+             if(!this->provide_metadata[j])
+                 return(-1);
+ 
+             this->Publish(this->provide_ids[j], resp_queue,
+                           PLAYER_MSGTYPE_RESP_ACK, hdr->subtype,
+                           this->provide_metadata[j],
+                           sizeof(player_imu_datatype_config_t),
+                           NULL);
+             return(0);
+         }
+         default:
+             return(-1);
+     }
+ }
+ 
+ int
  ReadLog::ProcessMessage(MessageQueue * resp_queue,
                          player_msghdr_t * hdr,
***************
*** 995,998 ****
--- 1041,1049 ----
    }
    else if((hdr->type == PLAYER_MSGTYPE_REQ) &&
+            (hdr->addr.interf == PLAYER_IMU_CODE))
+   {
+       return(this->ProcessIMUConfig(resp_queue, hdr, data));
+   }
+   else if((hdr->type == PLAYER_MSGTYPE_REQ) &&
            (hdr->addr.interf == PLAYER_POSITION2D_CODE))
    {
***************
*** 1095,1098 ****
--- 1146,1152 ----
        return this->ParseWSN(id, type, subtype, linenum,
                              token_count, tokens, time);
+   else if (id.interf == PLAYER_IMU_CODE)
+       return this->ParseIMU (id, type, subtype, linenum,
+                             token_count, tokens, time);
  
  #if 0
***************
*** 1783,1786 ****
--- 1837,1961 ----
  }
  
+ ////////////////////////////////////////////////////////////////////////////
+ // Parse IMU data
+ int ReadLog::ParseIMU (player_devaddr_t id, 
+                       unsigned short type, unsigned short subtype,
+                       int linenum,
+                       int token_count, char **tokens, double time)
+ {
+     switch(type)
+     {
+         case PLAYER_MSGTYPE_DATA:
+             switch(subtype)
+             {
+                 case PLAYER_IMU_DATA_STATE:
+                 {
+                     if (token_count < 13)
+                     {
+                         PLAYER_ERROR2("invalid line at %s:%d", this->filename, linenum);
+                         return -1;
+                     }
+ 		    player_imu_data_state_t data;
+ 		    
+ 		    data.pose.px = atof (tokens[7]);
+ 		    data.pose.py = atof (tokens[8]);
+ 		    data.pose.pz = atof (tokens[9]);
+ 		    data.pose.proll  = atof (tokens[10]);
+ 		    data.pose.ppitch = atof (tokens[11]);
+ 		    data.pose.pyaw   = atof (tokens[12]);
+ 		    
+                     this->Publish (id, NULL, type, subtype,
+                                   (void*)&data, sizeof(data), &time);
+                     return (0);
+                 }
+ 		
+ 		case PLAYER_IMU_DATA_CALIB:
+ 		{
+                     if (token_count < 16)
+                     {
+                         PLAYER_ERROR2("invalid line at %s:%d", this->filename, linenum);
+                         return -1;
+                     }
+ 		    player_imu_data_calib_t data;
+ 		    
+ 		    data.accel_x = atof (tokens[7]);
+ 		    data.accel_y = atof (tokens[8]);
+ 		    data.accel_z = atof (tokens[9]);
+ 		    data.gyro_x  = atof (tokens[10]);
+ 		    data.gyro_y  = atof (tokens[11]);
+ 		    data.gyro_z  = atof (tokens[12]);
+ 		    data.magn_x  = atof (tokens[13]);
+ 		    data.magn_y  = atof (tokens[14]);
+ 		    data.magn_z  = atof (tokens[15]);
+ 		    
+                     this->Publish (id, NULL, type, subtype,
+                                   (void*)&data, sizeof(data), &time);
+                     return (0);
+ 		}
+ 		
+ 		case PLAYER_IMU_DATA_QUAT:
+ 		{
+                     if (token_count < 20)
+                     {
+                         PLAYER_ERROR2("invalid line at %s:%d", this->filename, linenum);
+                         return -1;
+                     }
+ 		    player_imu_data_quat_t data;
+ 		    
+ 		    data.calib_data.accel_x = atof (tokens[7]);
+ 		    data.calib_data.accel_y = atof (tokens[8]);
+ 		    data.calib_data.accel_z = atof (tokens[9]);
+ 		    data.calib_data.gyro_x  = atof (tokens[10]);
+ 		    data.calib_data.gyro_y  = atof (tokens[11]);
+ 		    data.calib_data.gyro_z  = atof (tokens[12]);
+ 		    data.calib_data.magn_x  = atof (tokens[13]);
+ 		    data.calib_data.magn_y  = atof (tokens[14]);
+ 		    data.calib_data.magn_z  = atof (tokens[15]);
+ 		    data.q0      = atof (tokens[16]);
+ 		    data.q1      = atof (tokens[17]);
+ 		    data.q2      = atof (tokens[18]);
+ 		    data.q3      = atof (tokens[19]);
+ 		    
+                     this->Publish (id, NULL, type, subtype,
+                                   (void*)&data, sizeof(data), &time);
+                     return (0);
+ 		}
+ 		
+ 		case PLAYER_IMU_DATA_EULER:
+ 		{
+                     if (token_count < 19)
+                     {
+                         PLAYER_ERROR2("invalid line at %s:%d", this->filename, linenum);
+                         return -1;
+                     }
+ 		    player_imu_data_euler_t data;
+ 		    
+ 		    data.calib_data.accel_x = atof (tokens[7]);
+ 		    data.calib_data.accel_y = atof (tokens[8]);
+ 		    data.calib_data.accel_z = atof (tokens[9]);
+ 		    data.calib_data.gyro_x  = atof (tokens[10]);
+ 		    data.calib_data.gyro_y  = atof (tokens[11]);
+ 		    data.calib_data.gyro_z  = atof (tokens[12]);
+ 		    data.calib_data.magn_x  = atof (tokens[13]);
+ 		    data.calib_data.magn_y  = atof (tokens[14]);
+ 		    data.calib_data.magn_z  = atof (tokens[15]);
+ 		    data.orientation.proll  = atof (tokens[16]);
+ 		    data.orientation.ppitch = atof (tokens[17]);
+ 		    data.orientation.pyaw   = atof (tokens[18]);
+ 		    
+                     this->Publish (id, NULL, type, subtype,
+                                   (void*)&data, sizeof(data), &time);
+                     return (0);
+ 		}
+                 default:
+                     PLAYER_ERROR1 ("unknown IMU data subtype %d\n", subtype);
+                     return (-1);
+             }
+         default:
+             PLAYER_ERROR1 ("unknown IMU message type %d\n", type);
+             return (-1);
+     }
+ }
+ 
  #if 0
  ////////////////////////////////////////////////////////////////////////////

[Playerstage-commit] code/player/client_libs/libplayerc/test test_imu.c, NONE, 1.1 Makefile.am, 1.41, 1.42 test.c, 1.45, 1.46

[Radu Bogdan Rusu <veedee@us...>]

Update of /cvsroot/playerstage/code/player/client_libs/libplayerc/test
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv965/test

Modified Files:
	Makefile.am test.c 
Added Files:
	test_imu.c 
Log Message:
Added support for IMU.


--- NEW FILE: test_imu.c ---
/***************************************************************************
 * Desc: Tests for the IMU device
 * Author: Radu Bogdan Rusu
 * Date: 08th of September 2006
 **************************************************************************/

#include "test.h"
#include "playerc.h"

// Basic IMU test
int test_imu (playerc_client_t *client, int index)
{
  int t, i, j;
  void *rdevice;
  playerc_imu_t *device;

  printf ("device [imu] index [%d]\n", index);

  device = playerc_imu_create (client, index);

  TEST ("subscribing (read)");
  if (playerc_imu_subscribe (device, PLAYER_OPEN_MODE) == 0)
    PASS ();
  else
    FAIL ();

  for (t = 0; t < 10; t++)
  {
    TEST1 ("reading data (attempt %d)", t);

    do
      rdevice = playerc_client_read (client);
    while (rdevice == client);
    
    if (rdevice == device)
    {
      PASS ();
      printf ("roll  : [%f]\n"
	      "pitch : [%f]\n"
	      "yaw   : [%f]\n", 
	    device->pose.proll, device->pose.ppitch, device->pose.pyaw);
    }
    else
    {
      FAIL ();
      break;
    }
  }

  TEST ("resetting the orientation (global reset)...");
  if (playerc_imu_reset_orientation (device, 2) < 0)
    FAIL ();
  else
  {
    sleep (3);
    PASS ();
  }
  
  for (t = 0; t < 10; t++)
  {
    TEST1 ("reading data (attempt %d)", t);

    do
      rdevice = playerc_client_read (client);
    while (rdevice == client);
    
    if (rdevice == device)
    {
      PASS ();
      printf ("roll  : [%f]\n"
	      "pitch : [%f]\n"
	      "yaw   : [%f]\n", 
	    device->pose.proll, device->pose.ppitch, device->pose.pyaw);
    }
    else
    {
      FAIL ();
      break;
    }
  }
  
  TEST("unsubscribing");
  if (playerc_imu_unsubscribe (device) == 0)
    PASS ();
  else
    FAIL ();
  
  playerc_imu_destroy (device);
  
  return 0;
}

Index: test.c
===================================================================
RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/test/test.c,v
retrieving revision 1.45
retrieving revision 1.46
diff -C2 -d -r1.45 -r1.46
*** test.c	11 Apr 2006 15:40:31 -0000	1.45
--- test.c	11 Sep 2006 19:57:37 -0000	1.46
***************
*** 140,144 ****
          test_rfid(client, client->devinfos[i].addr.index);
          break;
! 	
  	// simulation device
        case PLAYER_SIMULATION_CODE:
--- 140,149 ----
          test_rfid(client, client->devinfos[i].addr.index);
          break;
! 
!         // imu device
!       case PLAYER_IMU_CODE:
!         test_imu (client, client->devinfos[i].addr.index);
! 	break;
! 		      	
  	// simulation device
        case PLAYER_SIMULATION_CODE:

Index: Makefile.am
===================================================================
RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/test/Makefile.am,v
retrieving revision 1.41
retrieving revision 1.42
diff -C2 -d -r1.41 -r1.42
*** Makefile.am	11 Apr 2006 15:40:31 -0000	1.41
--- Makefile.am	11 Sep 2006 19:57:37 -0000	1.42
***************
*** 29,33 ****
  	test_sonar.c \
  	test_rfid.c \
! 	test_wsn.c
  
  #test_camera.c \
--- 29,34 ----
  	test_sonar.c \
  	test_rfid.c \
! 	test_wsn.c \
! 	test_imu.c
  
  #test_camera.c \

[Playerstage-commit] code/player/client_libs/libplayerc dev_imu.c, NONE, 1.1 Makefile.am, 1.93, 1.94 playerc.h, 1.206, 1.207 utils.c, 1.25, 1.26

[Radu Bogdan Rusu <veedee@us...>]

Update of /cvsroot/playerstage/code/player/client_libs/libplayerc
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv965

Modified Files:
	Makefile.am playerc.h utils.c 
Added Files:
	dev_imu.c 
Log Message:
Added support for IMU.


--- NEW FILE: dev_imu.c ---
/* 
 *  libplayerc : a Player client library
 *  Copyright (C) Andrew Howard 2002-2003
 *
 *  This program is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License
 *  as published by the Free Software Foundation; either version 2
 *  of the License, or (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 */
/***************************************************************************
 * Desc: IMU proxy
 * Author: Radu Bogdan Rusu
 * Date: 8th of September 2006
 **************************************************************************/

#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <netinet/in.h>

#include "playerc.h"
#include "error.h"

// Process incoming data
void playerc_imu_putmsg (playerc_imu_t *device, 
                         player_msghdr_t *header,
			 void *data);

// Create a new imu proxy
playerc_imu_t *playerc_imu_create(playerc_client_t *client, int index)
{
    playerc_imu_t *device;
    device = malloc(sizeof(playerc_imu_t));
    memset(device, 0, sizeof(playerc_imu_t));
    playerc_device_init(&device->info, client, PLAYER_IMU_CODE, index,
       (playerc_putmsg_fn_t) playerc_imu_putmsg);

    return device;
}


// Destroy a imu proxy
void playerc_imu_destroy(playerc_imu_t *device)
{
    playerc_device_term(&device->info);
    free(device);
}


// Subscribe to the imu device
int playerc_imu_subscribe(playerc_imu_t *device, int access)
{
    return playerc_device_subscribe(&device->info, access);
}


// Un-subscribe from the imu device
int playerc_imu_unsubscribe(playerc_imu_t *device)
{
    return playerc_device_unsubscribe(&device->info);
}

// Process incoming data
void playerc_imu_putmsg (playerc_imu_t *device, 
			 player_msghdr_t *header,
			 void *data)
{
//    int i, j;
	
    if (header->type == PLAYER_MSGTYPE_DATA) 
	switch (header->subtype)
	{
	    case PLAYER_IMU_DATA_STATE:
	    {
		player_imu_data_state_t* imu_data = (player_imu_data_state_t*)data;
		device->pose = imu_data->pose;
		break;
	    }
	    case PLAYER_IMU_DATA_CALIB:
	    {
		player_imu_data_calib_t* imu_data = (player_imu_data_calib_t*)data;
		device->calib_data.accel_x = imu_data->accel_x;
		device->calib_data.accel_y = imu_data->accel_y;
		device->calib_data.accel_z = imu_data->accel_z;
		device->calib_data.gyro_x = imu_data->gyro_x;
		device->calib_data.gyro_y = imu_data->gyro_y;
		device->calib_data.gyro_z = imu_data->gyro_z;
		device->calib_data.magn_x = imu_data->magn_x;
		device->calib_data.magn_y = imu_data->magn_y;
		device->calib_data.magn_z = imu_data->magn_z;
		break;
	    }
	    case PLAYER_IMU_DATA_QUAT:
	    {
		player_imu_data_quat_t* imu_data = (player_imu_data_quat_t*)data;
		device->calib_data = imu_data->calib_data;
		device->q0 = imu_data->q0;
		device->q1 = imu_data->q1;
		device->q2 = imu_data->q2;
		device->q3 = imu_data->q3;
		break;
	    }
	    case PLAYER_IMU_DATA_EULER:
	    {
		player_imu_data_euler_t* imu_data = (player_imu_data_euler_t*)data;
		device->calib_data  = imu_data->calib_data;
		device->pose.proll  = imu_data->orientation.proll;
		device->pose.ppitch = imu_data->orientation.ppitch;
		device->pose.pyaw   = imu_data->orientation.pyaw;
		break;
	    }
	    default:
	    {
		PLAYERC_WARN1 ("skipping imu message with unknown data subtype: %d\n",
		    header->subtype);
		break;
	    }
	}
    else
	PLAYERC_WARN2 ("skipping imu message with unknown type/subtype: %d/%d\n",
	    header->type, header->subtype);
}

// Change the data type to one of the predefined data structures.
int
playerc_imu_datatype (playerc_imu_t *device, int value)
{
  player_imu_datatype_config_t config;

  config.value = value;

  return (playerc_client_request(device->info.client,
                                 &device->info,
                                 PLAYER_IMU_REQ_SET_DATATYPE,
                                 &config, NULL, 0));
}

// Reset orientation
int
playerc_imu_reset_orientation (playerc_imu_t *device, int value)
{
  player_imu_reset_orientation_config_t config;

  config.value = value;

  return (playerc_client_request(device->info.client,
                                 &device->info,
                                 PLAYER_IMU_REQ_RESET_ORIENTATION,
                                 &config, NULL, 0));
}

Index: utils.c
===================================================================
RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/utils.c,v
retrieving revision 1.25
retrieving revision 1.26
diff -C2 -d -r1.25 -r1.26
*** utils.c	31 May 2006 14:04:51 -0000	1.25
--- utils.c	11 Sep 2006 19:57:37 -0000	1.26
***************
*** 113,116 ****
--- 113,118 ----
      case PLAYER_SIMULATION_CODE:
        return PLAYER_SIMULATION_STRING;
+     case PLAYER_IMU_CODE:
+       return PLAYER_IMU_STRING;
  
      default:
***************
*** 182,185 ****
--- 184,189 ----
    if (strcmp(name, PLAYER_SIMULATION_STRING) == 0)
      return PLAYER_SIMULATION_CODE;
+   if (strcmp(name, PLAYER_IMU_STRING) == 0)
+     return PLAYER_IMU_CODE;
    return -1;
  }

Index: Makefile.am
===================================================================
RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/Makefile.am,v
retrieving revision 1.93
retrieving revision 1.94
diff -C2 -d -r1.93 -r1.94
*** Makefile.am	6 Sep 2006 16:54:51 -0000	1.93
--- Makefile.am	11 Sep 2006 19:57:37 -0000	1.94
***************
*** 37,40 ****
--- 37,41 ----
                          dev_gripper.c \
                          dev_health.c \
+ 			dev_imu.c \
                          dev_ir.c \
                          dev_laser.c \

Index: playerc.h
===================================================================
RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/playerc.h,v
retrieving revision 1.206
retrieving revision 1.207
diff -C2 -d -r1.206 -r1.207
*** playerc.h	6 Sep 2006 16:54:51 -0000	1.206
--- playerc.h	11 Sep 2006 19:57:37 -0000	1.207
***************
*** 2921,2924 ****
--- 2921,2970 ----
  /**************************************************************************/
  /** @ingroup playerc_proxies
+     @defgroup playerc_proxy_imu imu
+ 
+ The imu proxy provides an interface to an Inertial Measurement Unit.
+ 
+ @{
+ */
+ 
+ /** @brief IMU proxy state data. */
+ typedef struct
+ {
+     /** Device info; must be at the start of all device structures.         */
+     playerc_device_t info;
+ 
+     /** The complete pose of the IMU in 3D coordinates + angles             */
+     player_pose3d_t pose;
+ 
+     /** Calibrated IMU data (accel, gyro, magnetometer)                     */
+     player_imu_data_calib_t calib_data;
+ 
+     /** Orientation data as quaternions                                     */
+     float q0, q1, q2, q3;
+ } playerc_imu_t;
+ 
+ /** @brief Create a imu proxy. */
+ playerc_imu_t *playerc_imu_create (playerc_client_t *client, int index);
+ 
+ /** @brief Destroy a imu proxy. */
+ void playerc_imu_destroy (playerc_imu_t *device);
+ 
+ /** @brief Subscribe to the imu device. */
+ int playerc_imu_subscribe (playerc_imu_t *device, int access);
+ 
+ /** @brief Un-subscribe from the imu device. */
+ int playerc_imu_unsubscribe (playerc_imu_t *device);
+ 
+ /** Change the data type to one of the predefined data structures. */
+ int playerc_imu_datatype (playerc_imu_t *device, int value);
+ 
+ /**  Reset orientation. */
+ int playerc_imu_reset_orientation (playerc_imu_t *device, int value);
+ 
+ /** @} */
+ /***************************************************************************/
+ 
+ /**************************************************************************/
+ /** @ingroup playerc_proxies
      @defgroup playerc_proxy_wsn wsn