Skip to content
This repository
Browse code

working on fixing player plugin

  • Loading branch information...
commit 1d44d2b5348575a7a712acfc30b317d4e0642f48 1 parent 15132c8
authored November 22, 2010
2  avonstage/avonstage.cc
@@ -179,7 +179,7 @@ int RangerData( Stg::Model* mod, av_msg_t* data )
179 179
 		t.pose[5] = s.pose.a;		
180 180
 		
181 181
 		const std::vector<Stg::meters_t>& ranges = s.ranges;
182  
-		const std::vector<float>& intensities = s.intensities;
  182
+		const std::vector<double>& intensities = s.intensities;
183 183
 
184 184
 		assert( ranges.size() == intensities.size() );
185 185
 
81  libstage/model.cc
@@ -339,55 +339,54 @@ void Model::GuiState::Load( Worldfile* wf, int wf_entity )
339 339
 // constructor
340 340
 Model::Model( World* world,
341 341
 							Model* parent,
342  
-							const std::string& type ) :
  342
+				  const std::string& type ) :
343 343
   Ancestor(), 	 
344 344
   alwayson(false),
345 345
   blockgroup(),
346 346
   blocks_dl(0),
347 347
   boundary(false),
348  
-	callbacks(__CB_TYPE_COUNT), // one slot in the vector for each type
  348
+  callbacks(__CB_TYPE_COUNT), // one slot in the vector for each type
349 349
   color( 1,0,0 ), // red
350 350
   data_fresh(false),
351 351
   disabled(false),
352  
-	cv_list(),
353  
-	flag_list(),
354  
-	geom(),
355  
-	has_default_block( true ),
356  
-	id( Model::count++ ),
357  
-	interval((usec_t)1e5), // 100msec
358  
-	interval_energy((usec_t)1e5), // 100msec
359  
-	interval_pose((usec_t)1e5), // 100msec
360  
-	last_update(0),
361  
-	log_state(false),
362  
-	map_resolution(0.1),
363  
-	mass(0),
364  
-	parent(parent),
365  
-	pose(),
366  
-	power_pack( NULL ),
367  
-	pps_charging(),
368  
-	rastervis(),
369  
-	rebuild_displaylist(true),
370  
-	say_string(),
371  
-	stack_children( true ),
372  
-	stall(false),	 
373  
-	subs(0),
374  
-	thread_safe( false ),
375  
-	trail(trail_length),
376  
-	trail_index(0),
377  
-	type(type),	
378  
-	event_queue_num( 0 ),
379  
-	used(false),
380  
-	velocity(),
381  
-	velocity_enable( false ),
382  
-	watts(0.0),
383  
-	watts_give(0.0),
384  
-	watts_take(0.0),	 
385  
-	wf(NULL),
386  
-	wf_entity(0),
387  
-	world(world),
388  
-	world_gui( dynamic_cast<WorldGui*>( world ) )
389  
-{
390  
-  //assert( modelsbyid );
  352
+  cv_list(),
  353
+  flag_list(),
  354
+  geom(),
  355
+  has_default_block( true ),
  356
+  id( Model::count++ ),
  357
+  interval((usec_t)1e5), // 100msec
  358
+  interval_energy((usec_t)1e5), // 100msec
  359
+  interval_pose((usec_t)1e5), // 100msec
  360
+  last_update(0),
  361
+  log_state(false),
  362
+  map_resolution(0.1),
  363
+  mass(0),
  364
+  parent(parent),
  365
+  pose(),
  366
+  power_pack( NULL ),
  367
+  pps_charging(),
  368
+  rastervis(),
  369
+  rebuild_displaylist(true),
  370
+  say_string(),
  371
+  stack_children( true ),
  372
+  stall(false),	 
  373
+  subs(0),
  374
+  thread_safe( false ),
  375
+  trail(trail_length),
  376
+  trail_index(0),
  377
+  type(type),	
  378
+  event_queue_num( 0 ),
  379
+  used(false),
  380
+  velocity(),
  381
+  velocity_enable( false ),
  382
+  watts(0.0),
  383
+  watts_give(0.0),
  384
+  watts_take(0.0),	 
  385
+  wf(NULL),
  386
+  wf_entity(0),
  387
+  world(world),
  388
+  world_gui( dynamic_cast<WorldGui*>( world ) )
  389
+{
391 390
   assert( world );
392 391
   
393 392
   PRINT_DEBUG3( "Constructing model world: %s parent: %s type: %s \n",
52  libstage/stage.hh
@@ -2793,7 +2793,7 @@ namespace Stg
2793 2793
 			static Option showTransducers;		
2794 2794
 			
2795 2795
 			Vis( World* world );		
2796  
-			virtual ~Vis( void ){}
  2796
+		  virtual ~Vis( void ){} 
2797 2797
 			virtual void Visualize( Model* mod, Camera* cam );
2798 2798
 		} vis;
2799 2799
 		
@@ -2808,7 +2808,7 @@ namespace Stg
2808 2808
 			Color col;
2809 2809
 			
2810 2810
 			std::vector<meters_t> ranges;
2811  
-			std::vector<float_t> intensities;
  2811
+			std::vector<double> intensities;
2812 2812
 			
2813 2813
 			Sensor() : pose( 0,0,0,0 ), 
2814 2814
 								 size( 0.02, 0.02, 0.02 ), // teeny transducer
@@ -2824,32 +2824,32 @@ namespace Stg
2824 2824
 			void Load( Worldfile* wf, int entity );
2825 2825
 		};
2826 2826
 
2827  
-		/** returns a const reference to a vector of range and reflectance samples */
2828  
-			const std::vector<Sensor>& GetSensors() const
2829  
-		{ return sensors; }
2830  
-		
2831  
-		/** returns a vector of range samples from the indicated sensor
2832  
-				(defaults to zero) */
2833  
-		const std::vector<meters_t>& GetRanges( unsigned int sensor=0) const 
2834  
-		{ 
2835  
-			if( sensor < sensors.size() )
2836  
-				return sensors[sensor].ranges;
2837  
-			
2838  
-			PRINT_ERR1( "invalid sensor index specified (%d)", sensor );
2839  
-			exit(-1);
2840  
-		}
  2827
+	 /** returns a const reference to a vector of range and reflectance samples */
  2828
+	 const std::vector<Sensor>& GetSensors() const
  2829
+	 { return sensors; }
  2830
+	 
  2831
+	 /** returns a vector of range samples from the indicated sensor
  2832
+		  (defaults to zero) */
  2833
+	 const std::vector<meters_t>& GetRanges( unsigned int sensor=0) const 
  2834
+	 { 
  2835
+		if( sensor < sensors.size() )
  2836
+		  return sensors[sensor].ranges;
2841 2837
 		
2842  
-		/** returns a vector of intensitye samples from the indicated sensor
2843  
-				(defaults to zero) */
2844  
-		const std::vector<float_t>& GetIntensities( unsigned int sensor=0) const 
2845  
-		{ 
2846  
-			if( sensor < sensors.size() )
2847  
-				return sensors[sensor].intensities;
2848  
-			
2849  
-			PRINT_ERR1( "invalid sensor index specified (%d)", sensor );
2850  
-			exit(-1);
2851  
-		}
  2838
+		PRINT_ERR1( "invalid sensor index specified (%d)", sensor );
  2839
+		exit(-1);
  2840
+	 }
  2841
+	 
  2842
+	 /** returns a vector of intensitye samples from the indicated sensor
  2843
+		  (defaults to zero) */
  2844
+	 const std::vector<double>& GetIntensities( unsigned int sensor=0) const 
  2845
+	 { 
  2846
+		if( sensor < sensors.size() )
  2847
+		  return sensors[sensor].intensities;
2852 2848
 		
  2849
+		PRINT_ERR1( "invalid sensor index specified (%d)", sensor );
  2850
+		exit(-1);
  2851
+	 }
  2852
+	 
2853 2853
 		void LoadSensor( Worldfile* wf, int entity );
2854 2854
 		
2855 2855
   private:
2  libstageplugin/p_driver.cc
@@ -106,7 +106,7 @@ driver
106 106
 driver
107 107
 (
108 108
   name "stage"
109  
-  provides ["position2d:0" "laser:0"]
  109
+  provides ["position2d:0" "ranger:0"]
110 110
   model "marvin"
111 111
 )
112 112
 
4  libstageplugin/p_driver.h
@@ -176,8 +176,8 @@ class InterfaceRanger : public InterfaceModel
176 176
   private:
177 177
     int scan_id;
178 178
  public:
179  
-  InterfaceLaser( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section );
180  
-  virtual ~InterfaceLaser( void ){ /* TODO: clean up*/ };
  179
+  InterfaceRanger( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section );
  180
+  virtual ~InterfaceRanger( void ){ /* TODO: clean up*/ };
181 181
   virtual int ProcessMessage(QueuePointer & resp_queue,
182 182
 			      player_msghdr_t* hdr,
183 183
 			      void* data);
308  libstageplugin/p_ranger.cc
@@ -53,49 +53,59 @@ InterfaceRanger::InterfaceRanger( player_devaddr_t addr,
53 53
 void InterfaceRanger::Publish( void )
54 54
 {
55 55
   ModelRanger* rgr = dynamic_cast<ModelRanger*>(this->mod);
56  
-	
57  
-  const std::vector<ModelRanger::Sensor::Sample>& samples = rgr->GetSamples();
58 56
   
59  
-  uint32_t sample_count = samples.size();
60  
-  // don't publish anything until we have some real data
61  
-  if( sample_count == 0 )
62  
-    return;
63  
-	
64  
-  // Write range data
65  
-	player_ranger_data_range_t prange;
66  
-  memset( &prange, 0, sizeof(prange) );  
67  
-	prange.ranges = &samples[0];
68  
-	prange.ranges_count = samples.size();
69  
-	
70  
-  this->driver->Publish(this->addr,
71  
-												PLAYER_MSGTYPE_DATA,
72  
-												PLAYER_RANGER_DATA_RANGE,
73  
-												(void*)&prange, sizeof(prange), NULL);
74  
-	
75  
-
76  
-  // Write intensity data
77  
-	player_ranger_data_intns_t pintens;
78  
-  memset( &pintensity, 0, sizeof(pintens) );	
79  
-	pntens.intensities = &intens[0];
80  
-	
81  
-	pintens.intensities_count = intens.size();
82  
-	this->driver->Publish(this->addr,
83  
-												PLAYER_MSGTYPE_DATA,
84  
-												PLAYER_RANGER_DATA_INTNS,
85  
-												(void*)&pintens, sizeof(pintens), NULL);
86  
-
87  
-	// todo: shouldn't this be published?
88  
-	
89  
-  const ModelRanger::Config& cfg = rgr->cfg;
90  
-  pdata.min_angle = -cfg.fov/2.0;
91  
-  pdata.max_angle = +cfg.fov/2.0;
92  
-  pdata.resolution = cfg.fov / cfg.sample_count;
93  
-  pdata.max_range = cfg.range_bounds.max;
94  
-  pdata.ranges_count = pdata.intensity_count = cfg.sample_count;
95  
-  pdata.id = this->scan_id++;
  57
+  { // limit scope
  58
+	 // Write range data
  59
+	 const std::vector<meters_t>& ranges = rgr->GetRanges();
  60
+	 uint32_t range_count = ranges.size();
  61
+	 
  62
+	 player_ranger_data_range_t prange;
  63
+	 memset( &prange, 0, sizeof(prange) );  
  64
+	 
  65
+	 if( range_count > 0 )
  66
+		{
  67
+		  prange.ranges = (meters_t*)&ranges[0];
  68
+		  prange.ranges_count = ranges.size();
  69
+		}
  70
+	 
  71
+	 this->driver->Publish(this->addr,
  72
+								  PLAYER_MSGTYPE_DATA,
  73
+								  PLAYER_RANGER_DATA_RANGE,
  74
+								  (void*)&prange, sizeof(prange), NULL);
  75
+  }
  76
+  
  77
+  { // limit scope
  78
+	 // Write intensity data
  79
+	 const std::vector<double>& intensities = rgr->GetIntensities();
  80
+	 uint32_t intensity_count = intensities.size();
  81
+	 
  82
+	 player_ranger_data_intns_t pintens;
  83
+	 memset( &pintens, 0, sizeof(pintens) );	
  84
+	 
  85
+	 if( intensity_count > 0 )
  86
+		{
  87
+		  pintens.intensities = (double_t*)&intensities[0];
  88
+		  pintens.intensities_count = intensities.size();
  89
+		}	 	 
  90
+  
  91
+	 this->driver->Publish(this->addr,
  92
+								  PLAYER_MSGTYPE_DATA,
  93
+								  PLAYER_RANGER_DATA_INTNS,
  94
+								  (void*)&pintens, sizeof(pintens), NULL);
  95
+  }
96 96
 
97  
-  pdata.ranges = new float[pdata.ranges_count];
98  
-  pdata.intensity = new uint8_t[pdata.ranges_count];
  97
+  // todo: shouldn't this be published?
  98
+  
  99
+//   const ModelRanger::Config& cfg = rgr->cfg;
  100
+//   pdata.min_angle = -cfg.fov/2.0;
  101
+//   pdata.max_angle = +cfg.fov/2.0;
  102
+//   pdata.resolution = cfg.fov / cfg.sample_count;
  103
+//   pdata.max_range = cfg.range_bounds.max;
  104
+//   pdata.ranges_count = pdata.intensity_count = cfg.sample_count;
  105
+//   pdata.id = this->scan_id++;
  106
+  
  107
+//   pdata.ranges = new float[pdata.ranges_count];
  108
+//   pdata.intensity = new uint8_t[pdata.ranges_count];
99 109
 }
100 110
 
101 111
 int InterfaceRanger::ProcessMessage(QueuePointer & resp_queue,
@@ -104,118 +114,120 @@ int InterfaceRanger::ProcessMessage(QueuePointer & resp_queue,
104 114
 {
105 115
   ModelRanger* mod = (ModelRanger*)this->mod;
106 116
 
107  
-  // Is it a request to set the ranger's config?
  117
+  //   // Is it a request to set the ranger's config?
108 118
   if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
109  
-                           PLAYER_RANGER_REQ_SET_CONFIG,
110  
-                           this->addr))
111  
-  {
112  
-
113  
-    player_ranger_config_t* plc = (player_ranger_config_t*)data;
114  
-
115  
-    if( hdr->size == sizeof(player_ranger_config_t) )
116  
-    {
117  
-      // TODO
118  
-      // int intensity = plc->intensity;
119  
-
120  
-      ModelRanger::Config cfg = mod->GetConfig();
121  
-
122  
-	  PRINT_DEBUG3( "ranger config was: resolution %d, fov %.6f, interval %d\n",
123  
-			  cfg.resolution, cfg.fov, cfg.interval );
124  
-
125  
-	  cfg.fov = plc->max_angle - plc->min_angle;
126  
-	  cfg.resolution = (uint32_t) ( cfg.fov / ( cfg.sample_count * plc->resolution ) );
127  
-	  if ( cfg.resolution < 1 )
128  
-		  cfg.resolution = 1;
129  
-	  cfg.interval = (usec_t) ( 1.0E6 / plc->scanning_frequency );
130  
-
131  
-	  PRINT_DEBUG3( "setting ranger config: resolution %d, fov %.6f, interval %d\n",
132  
-			  cfg.resolution, cfg.fov, cfg.interval );
133  
-
134  
-	  // Range resolution is currently locked to the world setting
135  
-	  //  and intensity values are always read.  The relevant settings
136  
-	  //  are ignored.
137  
-
138  
-      mod->SetConfig( cfg );
139  
-
140  
-      this->driver->Publish(this->addr, resp_queue,
141  
-			    PLAYER_MSGTYPE_RESP_ACK,
142  
-			    PLAYER_RANGER_REQ_SET_CONFIG);
143  
-      return(0);
144  
-    }
145  
-    else
146  
-    {
147  
-      PRINT_ERR2("config request len is invalid (%d != %d)",
148  
-		 (int)hdr->size, (int)sizeof(player_ranger_config_t));
149  
-
150  
-      return(-1);
151  
-    }
152  
-  }
  119
+									PLAYER_RANGER_REQ_SET_CONFIG,
  120
+									this->addr))
  121
+	 {
  122
+		puts( "WARNING: ranger cfg support is broken - fix coming soon" );
  123
+		
  124
+		player_ranger_config_t* plc = (player_ranger_config_t*)data;
  125
+		
  126
+		if( hdr->size == sizeof(player_ranger_config_t) )
  127
+		  {
  128
+			 //       // TODO
  129
+			 //       // int intensity = plc->intensity;
  130
+			 
  131
+			 //       ModelRanger::Config cfg = mod->GetConfig();
  132
+			 
  133
+			 // 	  PRINT_DEBUG3( "ranger config was: resolution %d, fov %.6f, interval %d\n",
  134
+			 // 			  cfg.resolution, cfg.fov, cfg.interval );
  135
+			 
  136
+			 // 	  cfg.fov = plc->max_angle - plc->min_angle;
  137
+			 // 	  cfg.resolution = (uint32_t) ( cfg.fov / ( cfg.sample_count * plc->resolution ) );
  138
+			 // 	  if ( cfg.resolution < 1 )
  139
+			 // 		  cfg.resolution = 1;
  140
+			 // 	  cfg.interval = (usec_t) ( 1.0E6 / plc->scanning_frequency );
  141
+			 
  142
+			 // 	  PRINT_DEBUG3( "setting ranger config: resolution %d, fov %.6f, interval %d\n",
  143
+			 // 			  cfg.resolution, cfg.fov, cfg.interval );
  144
+			 
  145
+			 // 	  // Range resolution is currently locked to the world setting
  146
+			 // 	  //  and intensity values are always read.  The relevant settings
  147
+			 // 	  //  are ignored.
  148
+			 
  149
+			 //       mod->SetConfig( cfg );
  150
+			 
  151
+			 //       this->driver->Publish(this->addr, resp_queue,
  152
+			 // 			    PLAYER_MSGTYPE_RESP_ACK,
  153
+			 // 			    PLAYER_RANGER_REQ_SET_CONFIG);
  154
+			 return(0);
  155
+		  }
  156
+		else
  157
+		  {
  158
+			 PRINT_ERR2("config request len is invalid (%d != %d)",
  159
+							(int)hdr->size, (int)sizeof(player_ranger_config_t));
  160
+			 return(-1);
  161
+		  }
  162
+	 }
153 163
   // Is it a request to get the ranger's config?
154 164
   else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
155  
-                                 PLAYER_RANGER_REQ_GET_CONFIG,
156  
-                                 this->addr))
157  
-  {
158  
-    if( hdr->size == 0 )
159  
-    {
160  
-      ModelRanger::Config cfg = mod->GetConfig();
161  
-
162  
-      player_ranger_config_t plc;
163  
-      memset(&plc,0,sizeof(plc));
164  
-      plc.min_angle = -cfg.fov/2.0;
165  
-      plc.max_angle = +cfg.fov/2.0;
166  
-      plc.resolution = cfg.fov / ( cfg.sample_count * cfg.resolution );
167  
-      plc.max_range = cfg.range_bounds.max;
168  
-	  plc.range_res = 1.0; // todo
169  
-      plc.intensity = 1; // todo
170  
-	  plc.scanning_frequency = 1.0E6 / cfg.interval;
171  
-
172  
-      this->driver->Publish(this->addr, resp_queue,
173  
-			    PLAYER_MSGTYPE_RESP_ACK,
174  
-			    PLAYER_RANGER_REQ_GET_CONFIG,
175  
-			    (void*)&plc, sizeof(plc), NULL);
176  
-      return(0);
177  
-    }
178  
-    else
179  
-    {
180  
-      PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size,0);
181  
-      return(-1);
182  
-    }
183  
-  }
184  
-  // Is it a request to get the ranger's geom?
185  
-  else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
186  
-                                 PLAYER_RANGER_REQ_GET_GEOM,
187  
-                                 this->addr))
188  
-  {
189  
-    if(hdr->size == 0)
190  
-    {
191  
-      Geom geom = this->mod->GetGeom();
192  
-
193  
-      Pose pose = this->mod->GetPose();
194  
-
195  
-      // fill in the geometry data formatted player-like
196  
-      player_ranger_geom_t pgeom;
197  
-      pgeom.pose.px = pose.x;
198  
-      pgeom.pose.py = pose.y;
199  
-      pgeom.pose.pyaw = pose.a;
200  
-      pgeom.size.sl = geom.size.x;
201  
-      pgeom.size.sw = geom.size.y;
202  
-
203  
-      this->driver->Publish(this->addr, resp_queue,
204  
-			    PLAYER_MSGTYPE_RESP_ACK,
205  
-			    PLAYER_RANGER_REQ_GET_GEOM,
206  
-			    (void*)&pgeom, sizeof(pgeom), NULL);
207  
-      return(0);
208  
-    }
209  
-    else
210  
-    {
211  
-      PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size,0);
212  
-      return(-1);
213  
-    }
214  
-  }
215  
-
  165
+											PLAYER_RANGER_REQ_GET_CONFIG,
  166
+											this->addr))
  167
+	 {
  168
+		 puts( "WARNING: ranger GET CFG support is broken - fix coming soon" );
  169
+
  170
+     if( hdr->size == 0 )
  171
+     {
  172
+		 
  173
+		 //       ModelRanger::Config cfg = mod->GetConfig();
  174
+		 
  175
+		 //       player_ranger_config_t plc;
  176
+		 //       memset(&plc,0,sizeof(plc));
  177
+//       plc.min_angle = -cfg.fov/2.0;
  178
+//       plc.max_angle = +cfg.fov/2.0;
  179
+//       plc.resolution = cfg.fov / ( cfg.sample_count * cfg.resolution );
  180
+//       plc.max_range = cfg.range_bounds.max;
  181
+// 	  plc.range_res = 1.0; // todo
  182
+//       plc.intensity = 1; // todo
  183
+// 	  plc.scanning_frequency = 1.0E6 / cfg.interval;
  184
+
  185
+//       this->driver->Publish(this->addr, resp_queue,
  186
+// 			    PLAYER_MSGTYPE_RESP_ACK,
  187
+// 			    PLAYER_RANGER_REQ_GET_CONFIG,
  188
+// 			    (void*)&plc, sizeof(plc), NULL);
  189
+       return(0);
  190
+     }
  191
+     else
  192
+     {
  193
+       PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size,0);
  194
+       return(-1);
  195
+     }
  196
+   }
  197
+	else // Is it a request to get the ranger's geom?
  198
+	  if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
  199
+										 PLAYER_RANGER_REQ_GET_GEOM,
  200
+										 this->addr))
  201
+		 {
  202
+		if(hdr->size == 0)
  203
+		  {
  204
+			 Geom geom = this->mod->GetGeom();
  205
+			 Pose pose = this->mod->GetPose();
  206
+			 
  207
+			 // fill in the geometry data formatted player-like
  208
+			 player_ranger_geom_t pgeom;
  209
+			 pgeom.pose.px = pose.x;
  210
+			 pgeom.pose.py = pose.y;
  211
+			 pgeom.pose.pyaw = pose.a;
  212
+			 pgeom.size.sl = geom.size.x;
  213
+			 pgeom.size.sw = geom.size.y;
  214
+			 
  215
+			 this->driver->Publish(this->addr, resp_queue,
  216
+										  PLAYER_MSGTYPE_RESP_ACK,
  217
+										  PLAYER_RANGER_REQ_GET_GEOM,
  218
+										  (void*)&pgeom, sizeof(pgeom), NULL);
  219
+			 return(0);
  220
+		  }
  221
+		else
  222
+		  {
  223
+			 PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size,0);
  224
+			 return(-1);
  225
+		  }
  226
+	 }
  227
+  
216 228
   // Don't know how to handle this message.
217 229
   PRINT_WARN2( "stage ranger doesn't support message %d:%d.",
218  
-	       hdr->type, hdr->subtype);
  230
+					hdr->type, hdr->subtype);
219 231
   return(-1);
220 232
 }
221 233
 

0 notes on commit 1d44d2b

Please sign in to comment.
Something went wrong with that request. Please try again.