p_fiducial.cc
Go to the documentation of this file.
1 /*
2  * Player - One Hell of a Robot Server
3  * Copyright (C) 2004, 2005 Richard Vaughan
4  *
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software
18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19  *
20  */
21 
22 /*
23  * Desc: A plugin driver for Player that gives access to Stage devices.
24  * Author: Richard Vaughan
25  * Date: 10 December 2004
26  * CVS: $Id$
27  */
28 
29 // DOCUMENTATION
30 
40 /* TODO
41  - PLAYER_FIDUCIAL_REQ_SET_FOV
42  - PLAYER_FIDUCIAL_REQ_GET_FOV
43 */
44 
45 // CODE
46 
47 #include "p_driver.h"
48 using namespace Stg;
49 
50 
51 InterfaceFiducial::InterfaceFiducial( player_devaddr_t addr,
52  StgDriver* driver,
53  ConfigFile* cf,
54  int section )
55  : InterfaceModel( addr, driver, cf, section, "fiducial" )
56 {
57 }
58 
59 void InterfaceFiducial::Publish( void )
60 {
61  player_fiducial_data_t pdata;
62  memset( &pdata, 0, sizeof(pdata) );
63 
64  std::vector<ModelFiducial::Fiducial>& fids =
65  ((ModelFiducial*)mod)->GetFiducials();
66 
67  pdata.fiducials_count = fids.size();
68 
69  if( pdata.fiducials_count > 0 )
70  {
71  pdata.fiducials = new player_fiducial_item_t[pdata.fiducials_count];
72 
73  for( unsigned int i=0; i<pdata.fiducials_count; i++ )
74  {
75  pdata.fiducials[i].id = fids[i].id;
76 
77  // 2D x,y only
78  double xpos = fids[i].range * cos(fids[i].bearing);
79  double ypos = fids[i].range * sin(fids[i].bearing);
80 
81  pdata.fiducials[i].pose.px = xpos;
82  pdata.fiducials[i].pose.py = ypos;
83  pdata.fiducials[i].pose.pz = 0.0;
84  pdata.fiducials[i].pose.proll = 0.0;
85  pdata.fiducials[i].pose.ppitch = 0.0;
86  pdata.fiducials[i].pose.pyaw = fids[i].geom.a;
87  }
88  }
89 
90  // publish this data
91  this->driver->Publish( this->addr,
92  PLAYER_MSGTYPE_DATA,
93  PLAYER_FIDUCIAL_DATA_SCAN,
94  &pdata, sizeof(pdata), NULL);
95 
96  if ( pdata.fiducials )
97  delete [] pdata.fiducials;
98 }
99 
100 int InterfaceFiducial::ProcessMessage(QueuePointer& resp_queue,
101  player_msghdr_t* hdr,
102  void* data )
103 {
104  //printf("got fiducial request\n");
105 
106  // Is it a request to get the geometry?
107  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
108  PLAYER_FIDUCIAL_REQ_GET_GEOM,
109  this->addr))
110  {
111  Geom geom = mod->GetGeom();
112  Pose pose = mod->GetPose();
113 
114  // fill in the geometry data formatted player-like
115  player_laser_geom_t pgeom;
116  pgeom.pose.px = pose.x;
117  pgeom.pose.py = pose.y;
118  pgeom.pose.pz = pose.z;
119  pgeom.pose.proll = 0.0;
120  pgeom.pose.ppitch = 0.0;
121  pgeom.pose.pyaw = pose.a;
122  pgeom.size.sl = geom.size.x;
123  pgeom.size.sw = geom.size.y;
124 
125  this->driver->Publish(this->addr, resp_queue,
126  PLAYER_MSGTYPE_RESP_ACK,
127  PLAYER_FIDUCIAL_REQ_GET_GEOM,
128  (void*)&pgeom, sizeof(pgeom), NULL);
129  return(0);
130  }
131  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
132  PLAYER_FIDUCIAL_REQ_SET_ID,
133  this->addr))
134  {
135  if( hdr->size == sizeof(player_fiducial_id_t) )
136  {
137  // copy the new ID
138  player_fiducial_id_t* incoming = (player_fiducial_id_t*)data;
139 
140  // Stage uses a simple int for IDs.
141  int id = incoming->id;
142 
143  mod->SetFiducialReturn( id );
144 
145  player_fiducial_id_t pid;
146  pid.id = id;
147 
148  // acknowledge, including the new ID
149  this->driver->Publish(this->addr, resp_queue,
150  PLAYER_MSGTYPE_RESP_ACK,
151  PLAYER_FIDUCIAL_REQ_SET_ID,
152  (void*)&pid, sizeof(pid) );
153  }
154  else
155  {
156  PRINT_ERR2("Incorrect packet size setting fiducial ID (%d/%d)",
157  (int)hdr->size, (int)sizeof(player_fiducial_id_t) );
158  return(-1); // error - NACK is sent automatically
159  }
160  }
161  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
162  PLAYER_FIDUCIAL_REQ_GET_ID,
163  this->addr))
164  {
165  // fill in the data formatted player-like
166 
167  player_fiducial_id_t pid;
168  pid.id = mod->GetFiducialReturn();
169 
170  // acknowledge, including the new ID
171  this->driver->Publish(this->addr, resp_queue,
172  PLAYER_MSGTYPE_RESP_ACK,
173  PLAYER_FIDUCIAL_REQ_GET_ID,
174  (void*)&pid, sizeof(pid) );
175  }
176  /* case PLAYER_FIDUCIAL_SET_FOV:
177 
178  if( len == sizeof(player_fiducial_fov_t) )
179  {
180  player_fiducial_fov_t* pfov = (player_fiducial_fov_t*)src;
181 
182  // convert from player to stage FOV packets
183  fiducial_config_t setcfg;
184  memset( &setcfg, 0, sizeof(setcfg) );
185  setcfg.min_range = (uint16_t)ntohs(pfov->min_range) / 1000.0;
186  setcfg.max_range_id = (uint16_t)ntohs(pfov->max_range) / 1000.0;
187  setcfg.max_range_anon = setcfg.max_range_id;
188  setcfg.fov = DTOR((uint16_t)ntohs(pfov->view_angle));
189 
190  //printf( "setting fiducial FOV to min %f max %f fov %f\n",
191  // setcfg.min_range, setcfg.max_range_anon, setcfg.fov );
192 
193  //model_set_config( this->mod, &setcfg, sizeof(setcfg));
194  model_set_property( this->mod, "fiducial_cfg",
195  &setcfg, sizeof(setcfg));
196  }
197  else
198  PRINT_ERR2("Incorrect packet size setting fiducial FOV (%d/%d)",
199  (int)len, (int)sizeof(player_fiducial_fov_t) );
200 
201  // deliberate no-break - SET_FOV needs the current FOV as a reply
202 
203  case PLAYER_FIDUCIAL_GET_FOV:
204  {
205  fiducial_config_t *cfg = (fiducial_config_t*)
206  model_get_property_fixed( this->mod, "fiducial_cfg", sizeof(fiducial_config_t));
207  assert(cfg);
208 
209  // fill in the geometry data formatted player-like
210  player_fiducial_fov_t pfov;
211  pfov.min_range = htons((uint16_t)(1000.0 * cfg->min_range));
212  pfov.max_range = htons((uint16_t)(1000.0 * cfg->max_range_anon));
213  pfov.view_angle = htons((uint16_t)RTOD(cfg->fov));
214 
215  if( this->driver->PutReply( this->id, client, PLAYER_MSGTYPE_RESP_ACK,
216  &pfov, sizeof(pfov), NULL ) != 0 )
217  DRIVER_ERROR("PutReply() failed for "
218  "PLAYER_FIDUCIAL_GET_FOV or PLAYER_FIDUCIAL_SET_FOV");
219  }
220  break;
221  */
222 
223  else
224  {
225  // Don't know how to handle this message.
226  PRINT_WARN2( "fiducial doesn't support msg with type/subtype %d/%d",
227  hdr->type, hdr->subtype);
228  return(-1);
229  }
230 
231  return 0;
232 }
233 
Stg
The Stage library uses its own namespace.
Definition: canvas.hh:8
Stg::Size::x
meters_t x
Definition: stage.hh:228
InterfaceFiducial::Publish
virtual void Publish(void)
Definition: p_fiducial.cc:59
PRINT_WARN2
#define PRINT_WARN2(m, a, b)
Definition: stage.hh:635
InterfaceFiducial::ProcessMessage
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr_t *hdr, void *data)
Definition: p_fiducial.cc:100
Stg::Geom::size
Size size
extent
Definition: stage.hh:395
Interface::driver
StgDriver * driver
Definition: p_driver.h:72
Stg::Pose::z
meters_t z
location in 3 axes
Definition: stage.hh:251
Stg::Pose::y
meters_t y
Definition: stage.hh:251
Interface::addr
player_devaddr_t addr
Definition: p_driver.h:66
p_driver.h
Stg::Size::y
meters_t y
Definition: stage.hh:228
Stg::Model::SetFiducialReturn
void SetFiducialReturn(int fid)
Definition: model.cc:1281
Stg::ModelFiducial
ModelFiducial class
Definition: stage.hh:2687
InterfaceModel::mod
Stg::Model * mod
Definition: p_driver.h:115
Stg::Model::GetFiducialReturn
int GetFiducialReturn() const
Definition: stage.hh:2356
PRINT_ERR2
#define PRINT_ERR2(m, a, b)
Definition: stage.hh:627
Stg::Model::GetPose
Pose GetPose() const
Definition: stage.hh:2382
Stg::Pose::a
radians_t a
rotation about the z axis.
Definition: stage.hh:252
InterfaceFiducial::InterfaceFiducial
InterfaceFiducial(player_devaddr_t addr, StgDriver *driver, ConfigFile *cf, int section)
Definition: p_fiducial.cc:51
Stg::Pose::x
meters_t x
Definition: stage.hh:251
Stg::Model::GetGeom
Geom GetGeom() const
Definition: stage.hh:2378

stage
Author(s): Richard Vaughan , Brian Gerkey , Reed Hedges , Andrew Howard , Toby Collett , Pooya Karimian , Jeremy Asher , Alex Couture-Beil , Geoff Biggs , Rich Mattes , Abbas Sadat
autogenerated on Mon Jun 10 2019 15:06:09