[Playerstage-commit] code/player/libplayercore driver.cc, 1.27, 1.28 interface_util.c, 1.2, 1.3 message.cc, 1.20, 1.21 player.h, 1.129, 1.130 property.cpp, 1.1, 1.2 property.h, 1.1, 1.2

[Toby C. <th...@us...>]

Update of /cvsroot/playerstage/code/player/libplayercore
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv22655/libplayercore

Modified Files:
	driver.cc interface_util.c message.cc player.h property.cpp 
	property.h 
Log Message:
added geoffs ranger interface


Index: player.h
===================================================================
RCS file: /cvsroot/playerstage/code/player/libplayercore/player.h,v
retrieving revision 1.129
retrieving revision 1.130
diff -C2 -d -r1.129 -r1.130
*** player.h	29 Apr 2007 07:18:43 -0000	1.129
--- player.h	20 May 2007 00:30:15 -0000	1.130
***************
*** 161,164 ****
--- 161,165 ----
  #define PLAYER_IMU_CODE            60  // Inertial Measurement Unit interface
  #define PLAYER_POINTCLOUD3D_CODE   61  // 3-D point cloud
+ #define PLAYER_RANGER_CODE         62  // Range sensor device
  /** @} */
  
***************
*** 221,224 ****
--- 222,226 ----
  #define PLAYER_WIFI_STRING            "wifi"
  #define PLAYER_WSN_STRING             "wsn"
+ #define PLAYER_RANGER_STRING          "ranger"
  
  /** @} */
***************
*** 4549,4552 ****
--- 4551,4697 ----
  // /////////////////////////////////////////////////////////////////////////////
  /** @ingroup interfaces
+  * @defgroup interface_ranger ranger
+  * @brief A range sensor
+ 
+ Receive data from a range sensor, such as a laser scannar, sonar array or IR
+ array.
+ 
+ @section properties Recommended Properties
+ 
+ The following properties are recommended to be provided by drivers supporting
+ this interface, depending on device type.
+ 
+ @subsection laserprops Laser scanner devices
+ 
+ (double) min_angle:  Start angle of scans [rad].
+ (double) max_angle:  End angle of scans [rad].
+ (double) resolution: Scan resolution [rad].
+ (double) max_range:  Maximum range [m].
+ (double) range_res:  Range resolution [m].
+ (double) frequency:  Scanning frequency [Hz].
+ 
+ @subsection sonarprops Sonar array devices
+ 
+ (double) max_range:  Maximum range [m].
+ (double) range_res:  Range resolution [m].
+ 
+ @subsection irprops IR array devices
+ 
+ (double) max_range:  Maximum range [m].
+ (double) range_res:  Range resolution [m].
+ 
+ */
+ 
+ /** @ingroup interface_ranger
+  * @{ */
+ 
+ /** Data subtype: range scan */
+ #define PLAYER_RANGER_DATA_RANGE        1
+ /** Data subtype: pose-stamped range scan */
+ #define PLAYER_RANGER_DATA_RANGEPOSE    2
+ /** Data subtype: intensity scan */
+ #define PLAYER_RANGER_DATA_INTNS        3
+ /** Data subtype: pose-stamped intensity scan */
+ #define PLAYER_RANGER_DATA_INTNSPOSE    4
+ /** Data subtype: sensor geometry */
+ #define PLAYER_RANGER_DATA_GEOM         5
+ 
+ /** Request/reply subtype: get geometry */
+ #define PLAYER_RANGER_REQ_GET_GEOM      1
+ /** Request/reply subtype: power config */
+ #define PLAYER_RANGER_REQ_POWER         2
+ /** Request/reply subtype: intensity data config */
+ #define PLAYER_RANGER_REQ_INTNS         3
+ 
+ /** @brief Data and Request/reply: Get geometry. (@ref PLAYER_RANGER_REQ_GET_GEOM)
+ 
+ The ranger device position, orientation and size. */
+ typedef struct player_ranger_geom
+ {
+   /** Device centre pose in robot CS [m, m, m, rad, rad, rad]. */
+   player_pose3d_t pose;
+   /** Size of the device [m, m, m]. */
+   player_bbox3d_t size;
+   /** Number of individual range sensors that make up the device. */
+   uint32_t sensor_poses_count;
+   /** Pose of each individual range sensor that makes up the device (in device CS). */
+   player_pose3d_t *sensor_poses;
+   /** Number of individual range sensors that make up the device. */
+   uint32_t sensor_sizes_count;
+   /** Size of each individual range sensor that makes up the device. */
+   player_bbox3d_t *sensor_sizes;
+ } player_ranger_geom_t;
+ 
+ /** @brief Data: range scan (@ref PLAYER_RANGER_DATA_RANGE)
+ 
+ The basic ranger scan data packet, containing a set of range readings. */
+ typedef struct player_ranger_data_range
+ {
+   /** Number of range readings. */
+   uint32_t ranges_count;
+   /** Range readings [m]. */
+   double *ranges;
+ } player_ranger_data_range_t;
+ 
+ /** @brief Data: post-stamped range scan (@ref PLAYER_RANGER_DATA_RANGEPOSE)
+ 
+ A range scan with the (possibly estimated) pose of the device when the scan was
+ acquired. */
+ typedef struct player_ranger_data_rangepose
+ {
+   /** The scan data. */
+   player_ranger_data_range_t data;
+   /** The geometry of the device at the time the scan was acquired. */
+   player_ranger_geom_t geom;
+ } player_ranger_data_rangepose_t;
+ 
+ /** @brief Data: intensity scan (@ref PLAYER_RANGER_DATA_INTNS)
+ 
+ A set of intensity readings. */
+ typedef struct player_ranger_data_intns
+ {
+   /** Number of intensity readings. */
+   uint32_t intensities_count;
+   /** Intensity readings. */
+   double *intensities;
+ } player_ranger_data_intns_t;
+ 
+ /** @brief Data: post-stamped intensity scan (@ref PLAYER_RANGER_DATA_INTNSPOSE)
+ 
+ An intensity scan with the (possibly estimated) pose of the device when the scan
+ was acquired. */
+ typedef struct player_ranger_data_intnspose
+ {
+   /** The scan data. */
+   player_ranger_data_intns_t data;
+   /** The geometry of the device at the time the scan was acquired. */
+   player_ranger_geom_t geom;
+ } player_ranger_data_intnspose_t;
+ 
+ /** @brief Request/reply: Turn power on/off (@ref PLAYER_RANGER_REQ_POWER)
+ 
+ If the device supports it, use this message to turn the power on or off. */
+ typedef struct player_ranger_power_config
+ {
+   /** TRUE to turn device on, FALSE to turn device off. */
+   uint8_t state;
+ } player_ranger_power_config_t;
+ 
+ /** @brief Request/reply: Turn intensity data on/off for devices that provide it
+ (@ref PLAYER_RANGER_REQ_INTNS)
+ 
+ If the device is capable of providing intensity information (such as laser
+ reflection intensity or IR voltage), this will enable the transmission of the
+ data in the @ref PLAYER_RANGER_DATA_INTNS data message. */
+ typedef struct player_ranger_intns_config
+ {
+   /** TRUE to turn data on, FALSE to turn data off. */
+   uint8_t state;
+ } player_ranger_intns_config_t;
+ 
+ /** @} */
+ 
+ // /////////////////////////////////////////////////////////////////////////////
+ /** @ingroup interfaces
   * @defgroup interface_simulation simulation
   * @brief A robot simulator

Index: interface_util.c
===================================================================
RCS file: /cvsroot/playerstage/code/player/libplayercore/interface_util.c,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** interface_util.c	14 Mar 2007 16:41:51 -0000	1.2
--- interface_util.c	20 May 2007 00:30:15 -0000	1.3
***************
*** 95,98 ****
--- 95,99 ----
    {PLAYER_IMU_CODE, PLAYER_IMU_STRING},
    {PLAYER_POINTCLOUD3D_CODE, PLAYER_POINTCLOUD3D_STRING},
+   {PLAYER_RANGER_CODE, PLAYER_RANGER_STRING},
    {0,NULL}
  };

Index: property.h
===================================================================
RCS file: /cvsroot/playerstage/code/player/libplayercore/property.h,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** property.h	29 Apr 2007 07:18:43 -0000	1.1
--- property.h	20 May 2007 00:30:15 -0000	1.2
***************
*** 31,35 ****
  	public:
  		Property (void);
! 		Property (const char *newKey);
  		virtual ~Property (void);
  
--- 31,35 ----
  	public:
  		Property (void);
! 		Property (const char *newKey, bool readOnly);
  		virtual ~Property (void);
  
***************
*** 45,48 ****
--- 45,49 ----
  	protected:
  		char *key;			// Key for this property
+ 		bool readonly;		// true if this property is read-only
  };
  
***************
*** 54,61 ****
  {
  	public:
! 		IntProperty (const char *newKey, int newValue);
  
  		int GetValue (void) const			{ return value; }
! 		void SetValue (int newValue)		{ value = newValue; }
  		void GetValueToMessage (void *data) const;
  		void SetValueFromMessage (const void *data);
--- 55,62 ----
  {
  	public:
! 		IntProperty (const char *newKey, int newValue, bool readOnly);
  
  		int GetValue (void) const			{ return value; }
! 		void SetValue (int newValue);
  		void GetValueToMessage (void *data) const;
  		void SetValueFromMessage (const void *data);
***************
*** 67,71 ****
  		operator int (void)				{ return value; }
  		const IntProperty& operator= (const IntProperty &rhs);
! 		int operator= (int rhs)				{ value = rhs; return value; }
  
  	private:
--- 68,72 ----
  		operator int (void)				{ return value; }
  		const IntProperty& operator= (const IntProperty &rhs);
! 		int operator= (int rhs);
  
  	private:
***************
*** 80,87 ****
  {
  	public:
! 		DoubleProperty (const char *newKey, double newValue);
  
  		double GetValue (void) const		{ return value; }
! 		void SetValue (double newValue)		{ value = newValue; }
  		void GetValueToMessage (void *data) const;
  		void SetValueFromMessage (const void *data);
--- 81,88 ----
  {
  	public:
! 		DoubleProperty (const char *newKey, double newValue, bool readOnly);
  
  		double GetValue (void) const		{ return value; }
! 		void SetValue (double newValue);
  		void GetValueToMessage (void *data) const;
  		void SetValueFromMessage (const void *data);
***************
*** 93,97 ****
  		operator double (void)				{ return value; }
  		const DoubleProperty& operator= (const DoubleProperty &rhs);
! 		double operator= (double rhs)		{ value = rhs; return value; }
  
  	private:
--- 94,98 ----
  		operator double (void)				{ return value; }
  		const DoubleProperty& operator= (const DoubleProperty &rhs);
! 		double operator= (double rhs);
  
  	private:
***************
*** 105,109 ****
  {
  	public:
! 		StringProperty (const char *newKey, const char *newValue);
  		~StringProperty (void);
  
--- 106,110 ----
  {
  	public:
! 		StringProperty (const char *newKey, const char *newValue, bool readOnly);
  		~StringProperty (void);
  

Index: message.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/libplayercore/message.cc,v
retrieving revision 1.20
retrieving revision 1.21
diff -C2 -d -r1.20 -r1.21
*** message.cc	30 Apr 2007 21:45:24 -0000	1.20
--- message.cc	20 May 2007 00:30:15 -0000	1.21
***************
*** 34,37 ****
--- 34,38 ----
  #include <libplayercore/player.h>
  #include <libplayercore/error.h>
+ #include <libplayercore/interface_util.h>
  #include <libplayerxdr/playerxdr.h>
  
***************
*** 67,71 ****
        {
          // Possible error
!         PLAYER_WARN3 ("copied zero bytes in deep copy of message %d: %d, %d", Header.addr.interf, Header.type, Header.subtype);
        }
      }
--- 68,72 ----
        {
          // Possible error
!         PLAYER_WARN3 ("copied zero bytes in deep copy of message %s: %s, %d", interf_to_str (Header.addr.interf), msgtype_to_str (Header.type), Header.subtype);
        }
      }

Index: driver.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/libplayercore/driver.cc,v
retrieving revision 1.27
retrieving revision 1.28
diff -C2 -d -r1.27 -r1.28
*** driver.cc	29 Apr 2007 07:18:43 -0000	1.27
--- driver.cc	20 May 2007 00:30:15 -0000	1.28
***************
*** 340,350 ****
      }
  
!     // First check if it's an internal message
!     if (ProcessInternalMessages(msg->Queue, hdr, data) == 0)
!       continue;
! 
      int ret = this->ProcessMessage(msg->Queue, hdr, data);
      if(ret < 0)
      {
        PLAYER_WARN7("Unhandled message for driver "
                     "device=%d:%d:%s:%d type=%s subtype=%d len=%d\n",
--- 340,351 ----
      }
  
!     // Try the driver's process function first
!     // Drivers can override internal message handlers this way
      int ret = this->ProcessMessage(msg->Queue, hdr, data);
      if(ret < 0)
      {
+       // Check if it's an internal message, if that doesn't handle it, give a warning
+       if (ProcessInternalMessages(msg->Queue, hdr, data) != 0)
+       {
        PLAYER_WARN7("Unhandled message for driver "
                     "device=%d:%d:%s:%d type=%s subtype=%d len=%d\n",
***************
*** 358,361 ****
--- 359,363 ----
                        hdr->subtype, NULL, 0, NULL);
      }
+     }
      delete msg;
      pthread_testcancel();

Index: property.cpp
===================================================================
RCS file: /cvsroot/playerstage/code/player/libplayercore/property.cpp,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** property.cpp	29 Apr 2007 07:18:43 -0000	1.1
--- property.cpp	20 May 2007 00:30:15 -0000	1.2
***************
*** 35,39 ****
  }
  
! Property::Property (const char *newKey)
  {
  	if ((key = strdup (newKey)) == NULL)
--- 35,39 ----
  }
  
! Property::Property (const char *newKey, bool readOnly)
  {
  	if ((key = strdup (newKey)) == NULL)
***************
*** 42,45 ****
--- 42,46 ----
  		key = NULL;
  	}
+ 
  }
  
***************
*** 65,73 ****
  ////////////////////////////////////////////////////////////////////////////////
  
! IntProperty::IntProperty (const char *newKey, int newValue)
! 	: Property (newKey), value (newValue)
  {
  }
  
  void IntProperty::GetValueToMessage (void *data) const
  {
--- 66,85 ----
  ////////////////////////////////////////////////////////////////////////////////
  
! IntProperty::IntProperty (const char *newKey, int newValue, bool readOnly)
! 	: Property (newKey, readOnly), value (newValue)
  {
  }
  
+ void IntProperty::SetValue (int newValue)
+ {
+ 	if (readonly)
+ 	{
+ 		PLAYER_WARN2 ("Property %s is read only, cannot change value 50 %d", key, newValue);
+ 		return;
+ 	}
+ 
+ 	value = newValue;
+ }
+ 
  void IntProperty::GetValueToMessage (void *data) const
  {
***************
*** 77,80 ****
--- 89,98 ----
  void IntProperty::SetValueFromMessage (const void *data)
  {
+ 	if (readonly)
+ 	{
+ 		PLAYER_WARN2 ("Property %s is read only, cannot change value 50 %d", key, reinterpret_cast<const player_intprop_req_t*> (data)->value);
+ 		return;
+ 	}
+ 
  	value = reinterpret_cast<const player_intprop_req_t*> (data)->value;
  }
***************
*** 90,105 ****
  const IntProperty& IntProperty::operator= (const IntProperty &rhs)
  {
  	value = rhs.GetValue ();
  	return *this;
  }
  
  ////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////
  
! DoubleProperty::DoubleProperty (const char *newKey, double newValue)
! 	: Property (newKey), value (newValue)
  {
  }
  
  void DoubleProperty::GetValueToMessage (void *data) const
  {
--- 108,152 ----
  const IntProperty& IntProperty::operator= (const IntProperty &rhs)
  {
+ 	if (readonly)
+ 	{
+ 		PLAYER_WARN2 ("Property %s is read only, cannot change value 50 %d", key, rhs.GetValue ());
+ 		return *this;
+ 	}
+ 
  	value = rhs.GetValue ();
  	return *this;
  }
  
+ int IntProperty::operator= (int rhs)
+ {
+ 	if (readonly)
+ 	{
+ 		PLAYER_WARN2 ("Property %s is read only, cannot change value 50 %d", key, rhs);
+ 		return value;
+ 	}
+ 
+ 	value = rhs;
+ 	return value;
+ }
+ 
  ////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////
  
! DoubleProperty::DoubleProperty (const char *newKey, double newValue, bool readOnly)
! 	: Property (newKey, readOnly), value (newValue)
  {
  }
  
+ void DoubleProperty::SetValue (double newValue)
+ {
+ 	if (readonly)
+ 	{
+ 		PLAYER_WARN2 ("Property %s is read only, cannot change value 50 %f", key, newValue);
+ 		return;
+ 	}
+ 
+ 	value = newValue;
+ }
+ 
  void DoubleProperty::GetValueToMessage (void *data) const
  {
***************
*** 109,112 ****
--- 156,165 ----
  void DoubleProperty::SetValueFromMessage (const void *data)
  {
+ 	if (readonly)
+ 	{
+ 		PLAYER_WARN2 ("Property %s is read only, cannot change value 50 %f", key, reinterpret_cast<const player_dblprop_req_t*> (data)->value);
+ 		return;
+ 	}
+ 
  	value = reinterpret_cast<const player_dblprop_req_t*> (data)->value;
  }
***************
*** 122,134 ****
  const DoubleProperty& DoubleProperty::operator= (const DoubleProperty &rhs)
  {
  	value = rhs.GetValue ();
  	return *this;
  }
  
  ////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////
  
! StringProperty::StringProperty (const char *newKey, const char *newValue)
! 	: Property (newKey)
  {
  	if (newValue != NULL)
--- 175,205 ----
  const DoubleProperty& DoubleProperty::operator= (const DoubleProperty &rhs)
  {
+ 	if (readonly)
+ 	{
+ 		PLAYER_WARN2 ("Property %s is read only, cannot change value 50 %f", key, rhs.GetValue ());
+ 		return *this;
+ 	}
+ 
  	value = rhs.GetValue ();
  	return *this;
  }
  
+ double DoubleProperty::operator= (double rhs)
+ {
+ 	if (readonly)
+ 	{
+ 		PLAYER_WARN2 ("Property %s is read only, cannot change value 50 %f", key, rhs);
+ 		return value;
+ 	}
+ 
+ 	value = rhs;
+ 	return value;
+ }
+ 
  ////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////
  
! StringProperty::StringProperty (const char *newKey, const char *newValue, bool readOnly)
! 	: Property (newKey, readOnly)
  {
  	if (newValue != NULL)
***************
*** 152,155 ****
--- 223,232 ----
  void StringProperty::SetValue (const char *newValue)
  {
+ 	if (readonly)
+ 	{
+ 		PLAYER_WARN2 ("Property %s is read only, cannot change value 50 %s", key, newValue);
+ 		return;
+ 	}
+ 
  	if (value != NULL)
  		free (value);
***************
*** 184,187 ****
--- 261,270 ----
  void StringProperty::SetValueFromMessage (const void *data)
  {
+ 	if (readonly)
+ 	{
+ 		PLAYER_WARN2 ("Property %s is read only, cannot change value 50 %s", key, reinterpret_cast<const player_strprop_req_t*> (data)->value);
+ 		return;
+ 	}
+ 
  	const player_strprop_req_t *req = reinterpret_cast<const player_strprop_req_t*> (data);
  	if (value != NULL)
***************
*** 215,218 ****
--- 298,307 ----
  const StringProperty& StringProperty::operator= (const StringProperty &rhs)
  {
+ 	if (readonly)
+ 	{
+ 		PLAYER_WARN2 ("Property %s is read only, cannot change value 50 %s", key, rhs.GetValue ());
+ 		return *this;
+ 	}
+ 
  	if (value != NULL)
  		free (value);
***************
*** 230,233 ****
--- 319,328 ----
  const char* StringProperty::operator= (const char* rhs)
  {
+ 	if (readonly)
+ 	{
+ 		PLAYER_WARN2 ("Property %s is read only, cannot change value 50 %s", key, rhs);
+ 		return value;
+ 	}
+ 
  	if (value != NULL)
  		free (value);