Skip to content

Commit 15ccb86

Browse files
rtvrtv
committed
tracked changes in Stage ranger API
1 parent 1863a7a commit 15ccb86

File tree

2 files changed

+138
-134
lines changed

2 files changed

+138
-134
lines changed

‎libstageplugin/p_driver.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -108,8 +108,8 @@ class InterfaceModel
108108

109109
virtual void Subscribe( void );
110110
virtual void Unsubscribe( void );
111-
virtual void Subscribe( QueuePointer &queue );
112-
virtual void Unsubscribe( QueuePointer &queue );
111+
//virtual void Subscribe( QueuePointer &queue );
112+
//virtual void Unsubscribe( QueuePointer &queue );
113113

114114
protected:
115115
Stg::Model* mod;

‎libstageplugin/p_ranger.cc‎

Lines changed: 136 additions & 132 deletions
Original file line numberDiff line numberDiff line change
@@ -29,11 +29,11 @@
2929
// DOCUMENTATION ------------------------------------------------------------
3030

3131
/** @addtogroup player
32-
@par Ranger interface
33-
- PLAYER_RANGER_DATA_RANGE
34-
- PLAYER_RANGER_DATA_INTENS
35-
- PLAYER_RANGER_REQ_GET_CONFIG
36-
- PLAYER_RANGER_REQ_GET_GEOM
32+
@par Ranger interface
33+
- PLAYER_RANGER_DATA_RANGE
34+
- PLAYER_RANGER_DATA_INTENS
35+
- PLAYER_RANGER_REQ_GET_CONFIG
36+
- PLAYER_RANGER_REQ_GET_GEOM
3737
*/
3838

3939
// CODE ----------------------------------------------------------------------
@@ -42,9 +42,9 @@
4242
using namespace Stg;
4343

4444
InterfaceRanger::InterfaceRanger( player_devaddr_t addr,
45-
StgDriver* driver,
46-
ConfigFile* cf,
47-
int section )
45+
StgDriver* driver,
46+
ConfigFile* cf,
47+
int section )
4848
: InterfaceModel( addr, driver, cf, section, "ranger" )
4949
{
5050
this->scan_id = 0;
@@ -54,158 +54,162 @@ void InterfaceRanger::Publish( void )
5454
{
5555
ModelRanger* rgr = dynamic_cast<ModelRanger*>(this->mod);
5656

57-
// the Player interface dictates that if multiple sensor poses are
58-
// given, then we have exactly one range reading per sensor. To give
59-
// multiple ranges from the same origin, only one sensor is allowed.
57+
// the Player interface dictates that if multiple sensor poses are
58+
// given, then we have exactly one range reading per sensor. To give
59+
// multiple ranges from the same origin, only one sensor is allowed.
6060

61-
const std::vector<ModelRanger::Sensor>& sensors = rgr->GetSensors();
61+
// need to use the mutable version since we access the range data via a regular pointer
62+
std::vector<ModelRanger::Sensor>& sensors = rgr->GetSensorsMutable();
6263

63-
player_ranger_data_range_t prange;
64-
memset( &prange, 0, sizeof(prange) );
64+
player_ranger_data_range_t prange;
65+
memset( &prange, 0, sizeof(prange) );
6566

66-
player_ranger_data_intns_t pintens;
67-
memset( &pintens, 0, sizeof(pintens) );
68-
69-
std::vector<double> rv, iv;
70-
71-
if( sensors.size() == 1 ) // a laser scanner type, with one beam origin and many ranges
72-
{
73-
prange.ranges = rgr->GetRangesArr( 0, &prange.ranges_count );
74-
pintens.intensities = rgr->GetIntensitiesArr( 0, &pintens.intensities_count );
75-
}
76-
else
77-
{ // a sonar/IR type with one range per beam origin
78-
FOR_EACH( it, sensors )
79-
{
80-
if( it->ranges.size() )
81-
rv.push_back( it->ranges[0] );
67+
player_ranger_data_intns_t pintens;
68+
memset( &pintens, 0, sizeof(pintens) );
69+
70+
std::vector<double> rv, iv;
71+
72+
if( sensors.size() == 1 ) // a laser scanner type, with one beam origin and many ranges
73+
{
74+
prange.ranges = &sensors[0].ranges[0];
75+
prange.ranges_count = sensors[0].ranges.size();
76+
77+
pintens.intensities = &sensors[0].intensities[0];
78+
pintens.intensities_count = sensors[0].intensities.size();
79+
}
80+
else
81+
{ // a sonar/IR type with one range per beam origin
82+
FOR_EACH( it, sensors )
83+
{
84+
if( it->ranges.size() )
85+
rv.push_back( it->ranges[0] );
8286

83-
if( it->intensities.size() )
84-
iv.push_back( it->intensities[0] );
85-
}
87+
if( it->intensities.size() )
88+
iv.push_back( it->intensities[0] );
89+
}
8690

87-
prange.ranges_count = rv.size();
88-
prange.ranges = rv.size() ? &rv[0] : NULL;
91+
prange.ranges_count = rv.size();
92+
prange.ranges = rv.size() ? &rv[0] : NULL;
8993

90-
pintens.intensities_count = iv.size();
91-
pintens.intensities = iv.size() ? &iv[0] : NULL;
92-
}
94+
pintens.intensities_count = iv.size();
95+
pintens.intensities = iv.size() ? &iv[0] : NULL;
96+
}
9397

94-
if( prange.ranges_count )
95-
this->driver->Publish(this->addr,
96-
PLAYER_MSGTYPE_DATA,
97-
PLAYER_RANGER_DATA_RANGE,
98-
(void*)&prange, sizeof(prange), NULL);
98+
if( prange.ranges_count )
99+
this->driver->Publish(this->addr,
100+
PLAYER_MSGTYPE_DATA,
101+
PLAYER_RANGER_DATA_RANGE,
102+
(void*)&prange, sizeof(prange), NULL);
99103

100-
if( pintens.intensities_count )
101-
this->driver->Publish(this->addr,
102-
PLAYER_MSGTYPE_DATA,
103-
PLAYER_RANGER_DATA_INTNS,
104-
(void*)&pintens, sizeof(pintens), NULL);
104+
if( pintens.intensities_count )
105+
this->driver->Publish(this->addr,
106+
PLAYER_MSGTYPE_DATA,
107+
PLAYER_RANGER_DATA_INTNS,
108+
(void*)&pintens, sizeof(pintens), NULL);
105109
}
106110

107111

108112
int InterfaceRanger::ProcessMessage(QueuePointer & resp_queue,
109-
player_msghdr_t* hdr,
110-
void* data)
113+
player_msghdr_t* hdr,
114+
void* data)
111115
{
112116
ModelRanger* mod = (ModelRanger*)this->mod;
113117

114118
// Is it a request to get the ranger's config?
115-
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
116-
PLAYER_RANGER_REQ_GET_CONFIG,
117-
this->addr))
118-
{
119-
if( hdr->size == 0 )
120-
{
121-
// the Player ranger config is a little weaker than Stage's
122-
// natice device, so all we can do is warn about this.
123-
PRINT_WARN( "stageplugin ranger config describes only the first sensor of the ranger." );
119+
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
120+
PLAYER_RANGER_REQ_GET_CONFIG,
121+
this->addr))
122+
{
123+
if( hdr->size == 0 )
124+
{
125+
// the Player ranger config is a little weaker than Stage's
126+
// natice device, so all we can do is warn about this.
127+
PRINT_WARN( "stageplugin ranger config describes only the first sensor of the ranger." );
124128

125-
player_ranger_config_t prc;
126-
bzero(&prc,sizeof(prc));
129+
player_ranger_config_t prc;
130+
bzero(&prc,sizeof(prc));
127131

128-
const ModelRanger::Sensor& s = mod->GetSensors()[0];
132+
const ModelRanger::Sensor& s = mod->GetSensors()[0];
129133

130-
prc.min_angle = -s.fov/2.0;
131-
prc.max_angle = +s.fov/2.0;
132-
prc.angular_res = s.fov / (double)s.sample_count;
133-
prc.max_range = s.range.max;
134-
prc.min_range = s.range.min;
135-
prc.range_res = 1.0 / mod->GetWorld()->Resolution();
136-
prc.frequency = 1.0E6 / mod->GetInterval();
134+
prc.min_angle = -s.fov/2.0;
135+
prc.max_angle = +s.fov/2.0;
136+
prc.angular_res = s.fov / (double)s.sample_count;
137+
prc.max_range = s.range.max;
138+
prc.min_range = s.range.min;
139+
prc.range_res = 1.0 / mod->GetWorld()->Resolution();
140+
prc.frequency = 1.0E6 / mod->GetInterval();
137141

138-
this->driver->Publish(this->addr, resp_queue,
139-
PLAYER_MSGTYPE_RESP_ACK,
140-
PLAYER_RANGER_REQ_GET_CONFIG,
141-
(void*)&prc, sizeof(prc), NULL);
142-
return(0);
143-
}
144-
else
145-
{
146-
PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size,0);
147-
return(-1);
148-
}
149-
}
150-
else // Is it a request to get the ranger's geom?
151-
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
152-
PLAYER_RANGER_REQ_GET_GEOM,
153-
this->addr))
154-
{
155-
if(hdr->size == 0)
156-
{
157-
Geom geom = mod->GetGeom();
158-
Pose pose = mod->GetPose();
142+
this->driver->Publish(this->addr, resp_queue,
143+
PLAYER_MSGTYPE_RESP_ACK,
144+
PLAYER_RANGER_REQ_GET_CONFIG,
145+
(void*)&prc, sizeof(prc), NULL);
146+
return(0);
147+
}
148+
else
149+
{
150+
PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size,0);
151+
return(-1);
152+
}
153+
}
154+
else // Is it a request to get the ranger's geom?
155+
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
156+
PLAYER_RANGER_REQ_GET_GEOM,
157+
this->addr))
158+
{
159+
if(hdr->size == 0)
160+
{
161+
Geom geom = mod->GetGeom();
162+
Pose pose = mod->GetPose();
159163

160-
const std::vector<ModelRanger::Sensor>& sensors = mod->GetSensors();
164+
const std::vector<ModelRanger::Sensor>& sensors = mod->GetSensors();
161165

162-
// fill in the geometry data formatted player-like
163-
player_ranger_geom_t pgeom;
164-
bzero( &pgeom, sizeof(pgeom));
165-
pgeom.pose.px = pose.x;
166-
pgeom.pose.py = pose.y;
167-
pgeom.pose.pyaw = pose.a;
168-
pgeom.size.sl = geom.size.x;
169-
pgeom.size.sw = geom.size.y;
166+
// fill in the geometry data formatted player-like
167+
player_ranger_geom_t pgeom;
168+
bzero( &pgeom, sizeof(pgeom));
169+
pgeom.pose.px = pose.x;
170+
pgeom.pose.py = pose.y;
171+
pgeom.pose.pyaw = pose.a;
172+
pgeom.size.sl = geom.size.x;
173+
pgeom.size.sw = geom.size.y;
170174

171-
pgeom.element_poses_count = pgeom.element_sizes_count = sensors.size();
175+
pgeom.element_poses_count = pgeom.element_sizes_count = sensors.size();
172176

173-
player_pose3d_t poses[ sensors.size() ];
174-
player_bbox3d_t sizes[ sensors.size() ];
177+
player_pose3d_t poses[ sensors.size() ];
178+
player_bbox3d_t sizes[ sensors.size() ];
175179

176-
for( size_t s=0; s<pgeom.element_poses_count; s++ )
177-
{
178-
poses[s].px = sensors[s].pose.x;
179-
poses[s].py = sensors[s].pose.y;
180-
poses[s].pz = sensors[s].pose.z;
181-
poses[s].proll = 0.0;
182-
poses[s].ppitch = 0.0;
183-
poses[s].pyaw = sensors[s].pose.a;
184-
185-
sizes[s].sw = sensors[s].size.x;
186-
sizes[s].sl = sensors[s].size.y;
187-
sizes[s].sh = sensors[s].size.z;
188-
}
189-
190-
pgeom.element_poses = poses;
191-
pgeom.element_sizes = sizes;
192-
193-
this->driver->Publish(this->addr, resp_queue,
194-
PLAYER_MSGTYPE_RESP_ACK,
195-
PLAYER_RANGER_REQ_GET_GEOM,
196-
(void*)&pgeom, sizeof(pgeom), NULL);
197-
return(0);
198-
}
199-
else
200-
{
201-
PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size,0);
202-
return(-1);
203-
}
204-
}
180+
for( size_t s=0; s<pgeom.element_poses_count; s++ )
181+
{
182+
poses[s].px = sensors[s].pose.x;
183+
poses[s].py = sensors[s].pose.y;
184+
poses[s].pz = sensors[s].pose.z;
185+
poses[s].proll = 0.0;
186+
poses[s].ppitch = 0.0;
187+
poses[s].pyaw = sensors[s].pose.a;
188+
189+
sizes[s].sw = sensors[s].size.x;
190+
sizes[s].sl = sensors[s].size.y;
191+
sizes[s].sh = sensors[s].size.z;
192+
}
193+
194+
pgeom.element_poses = poses;
195+
pgeom.element_sizes = sizes;
196+
197+
this->driver->Publish(this->addr, resp_queue,
198+
PLAYER_MSGTYPE_RESP_ACK,
199+
PLAYER_RANGER_REQ_GET_GEOM,
200+
(void*)&pgeom, sizeof(pgeom), NULL);
201+
return(0);
202+
}
203+
else
204+
{
205+
PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size,0);
206+
return(-1);
207+
}
208+
}
205209

206210
// Don't know how to handle this message.
207211
PRINT_WARN2( "stage ranger doesn't support message %d:%d.",
208-
hdr->type, hdr->subtype);
212+
hdr->type, hdr->subtype);
209213
return(-1);
210214
}
211215

0 commit comments

Comments
 (0)