carmenwrapper.cpp
Go to the documentation of this file.
1 /*****************************************************************
2  *
3  * This file is part of the GMAPPING project
4  *
5  * GMAPPING Copyright (c) 2004 Giorgio Grisetti,
6  * Cyrill Stachniss, and Wolfram Burgard
7  *
8  * This software is licensed under the 3-Clause BSD License
9  * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss,
10  * and Wolfram Burgard.
11  *
12  * Further information on this license can be found at:
13  * https://opensource.org/licenses/BSD-3-Clause
14  *
15  * GMAPPING is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied
17  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
18  * PURPOSE.
19  *
20  *****************************************************************/
21 
22 
24 
25 using namespace GMapping;
26 using namespace std;
27 
28 //static vars for the carmenwrapper
30 deque<RangeReading> CarmenWrapper::m_rangeDeque;
31 pthread_mutex_t CarmenWrapper::m_mutex;
33 pthread_mutex_t CarmenWrapper::m_lock;
39 bool CarmenWrapper::stopped=true;
40 
41 
42 void CarmenWrapper::initializeIPC(const char* name) {
43  carmen_ipc_initialize(1,(char **)&name);
44 }
45 
46 
47 
48 
50  lock();
51  IPC_RETURN_TYPE err;
52 
53  /* register globalpos message */
54  err = IPC_defineMsg(CARMEN_LOCALIZE_GLOBALPOS_NAME, IPC_VARIABLE_LENGTH,
55  CARMEN_LOCALIZE_GLOBALPOS_FMT);
56  carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_GLOBALPOS_NAME);
57 
58  /* register robot particle message */
59  err = IPC_defineMsg(CARMEN_LOCALIZE_PARTICLE_NAME, IPC_VARIABLE_LENGTH,
60  CARMEN_LOCALIZE_PARTICLE_FMT);
61  carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_PARTICLE_NAME);
62 
63 /*
64  carmen_localize_subscribe_initialize_placename_message(NULL,
65  (carmen_handler_t)
66  carmen_localize_initialize_placename_handler,
67  CARMEN_SUBSCRIBE_LATEST);
68 
69  // register map request message
70  err = IPC_defineMsg(CARMEN_LOCALIZE_MAP_QUERY_NAME, IPC_VARIABLE_LENGTH,
71  CARMEN_LOCALIZE_MAP_QUERY_FMT);
72  carmen_test_ipc_exit(err, "Could not define",
73  CARMEN_LOCALIZE_MAP_QUERY_NAME);
74 
75  err = IPC_defineMsg(CARMEN_LOCALIZE_MAP_NAME, IPC_VARIABLE_LENGTH,
76  CARMEN_LOCALIZE_MAP_FMT);
77  carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_MAP_NAME);
78 
79  // subscribe to map request message
80  err = IPC_subscribe(CARMEN_LOCALIZE_MAP_QUERY_NAME, map_query_handler, NULL);
81  carmen_test_ipc(err, "Could not subscribe", CARMEN_LOCALIZE_MAP_QUERY_NAME);
82  IPC_setMsgQueueLength(CARMEN_LOCALIZE_MAP_QUERY_NAME, 1);
83 
84 
85  // register globalpos request message
86  err = IPC_defineMsg(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME,
87  IPC_VARIABLE_LENGTH,
88  CARMEN_DEFAULT_MESSAGE_FMT);
89  carmen_test_ipc_exit(err, "Could not define",
90  CARMEN_LOCALIZE_MAP_QUERY_NAME);
91 
92  // subscribe to globalpos request message
93  err = IPC_subscribe(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME,
94  globalpos_query_handler, NULL);
95  carmen_test_ipc(err, "Could not subscribe",
96  CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME);
97  IPC_setMsgQueueLength(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, 1);
98 */
99  unlock();
100  return 0;
101 }
102 
103 bool CarmenWrapper::start(const char* name){
104  if (m_threadRunning)
105  return false;
106  carmen_robot_subscribe_frontlaser_message(NULL, (carmen_handler_t)robot_frontlaser_handler, CARMEN_SUBSCRIBE_LATEST);
107  carmen_robot_subscribe_rearlaser_message(NULL, (carmen_handler_t)robot_rearlaser_handler, CARMEN_SUBSCRIBE_LATEST);
108  carmen_simulator_subscribe_truepos_message(NULL,(carmen_handler_t) simulator_truepos_handler, CARMEN_SUBSCRIBE_LATEST);
109 
110  IPC_RETURN_TYPE err;
111 
112  err = IPC_subscribe(CARMEN_NAVIGATOR_GO_NAME, navigator_go_handler, NULL);
113  carmen_test_ipc_exit(err, "Could not subscribe",
114  CARMEN_NAVIGATOR_GO_NAME);
115  IPC_setMsgQueueLength(CARMEN_NAVIGATOR_GO_NAME, 1);
116 
117  err = IPC_subscribe(CARMEN_NAVIGATOR_STOP_NAME, navigator_stop_handler, NULL);
118  carmen_test_ipc_exit(err, "Could not subscribe",
119  CARMEN_NAVIGATOR_STOP_NAME);
120  IPC_setMsgQueueLength(CARMEN_NAVIGATOR_STOP_NAME, 1);
121 
122 
123 
124  signal(SIGINT, shutdown_module);
125  pthread_mutex_init(&m_mutex, 0);
126  pthread_mutex_init(&m_lock, 0);
127  sem_init(&m_dequeSem, 0, 0);
128  m_threadRunning=true;
129  pthread_create (&m_readingThread,0,m_reading_function,0);
130  return true;
131 }
132 
134  //cerr <<"LOCK" << endl;
135  pthread_mutex_lock(&m_lock);
136 }
137 
139  //cerr <<"UNLOCK" << endl;
140  pthread_mutex_unlock(&m_lock);
141 }
142 
143 
145  pthread_mutex_lock(&m_mutex);
146  bool smok=m_frontLaser;
147  pthread_mutex_unlock(&m_mutex);
148  return smok;
149 }
150 
152  return m_sensorMap;
153 }
154 
156  return m_threadRunning;
157 }
158 
160  return stopped;
161 }
162 
164  int ql=0;
165  pthread_mutex_lock(&m_mutex);
166  ql=m_rangeDeque.size();
167  pthread_mutex_unlock(&m_mutex);
168  return ql;
169 }
170 
172  return m_truepos;
173 }
174 
176  bool present=false;
177  sem_wait(&m_dequeSem);
178  pthread_mutex_lock(&m_mutex);
179  if (!m_rangeDeque.empty()){
180 // cerr << __func__ << ": queue size=" <<m_rangeDeque.size() << endl;
181  reading=m_rangeDeque.front();
182  m_rangeDeque.pop_front();
183  present=true;
184  }
185  int sval;
186  sem_getvalue(&m_dequeSem,&sval);
187 // cerr << "fetch. elements= "<< m_rangeDeque.size() << " sval=" << sval <<endl;
188  pthread_mutex_unlock(&m_mutex);
189  return present;
190 }
191 
193  pthread_mutex_lock(&m_mutex);
194  m_rangeDeque.push_back(reading);
195  pthread_mutex_unlock(&m_mutex);
196  sem_post(&m_dequeSem);
197  int sval;
198  sem_getvalue(&m_dequeSem,&sval);
199 // cerr << "post. elements= "<< m_rangeDeque.size() << " sval=" << sval <<endl;
200 }
201 
202 
203 //RangeSensor::RangeSensor(std::string name, unsigned int beams_num, unsigned int res, const OrientedPoint& position, double span, double maxrange):
204 
205 void CarmenWrapper::robot_frontlaser_handler(carmen_robot_laser_message* frontlaser) {
206 /* if (! m_rangeSensor){
207  double res=0;
208  if (frontlaser->num_readings==180 || frontlaser->num_readings==181)
209  res=M_PI/180;
210  if (frontlaser->num_readings==360 || frontlaser->num_readings==361)
211  res=M_PI/360;
212  assert(res>0);
213  m_rangeSensor=new RangeSensor("FLASER",frontlaser->num_readings, res, OrientedPoint(0,0,0), 0, 89.9);
214  m_sensorMap.insert(make_pair(string("FLASER"), m_rangeSensor));
215 
216  cout << __func__
217  << ": FrontLaser configured."
218  << " Readings " << m_rangeSensor->beams().size()
219  << " Resolution " << res << endl;
220  }
221 
222  RangeReading reading(m_rangeSensor, frontlaser->timestamp);
223  reading.resize(m_rangeSensor->beams().size());
224  for (unsigned int i=0; i< (unsigned int)frontlaser->num_readings; i++){
225  reading[i]=(double)frontlaser->range[i];
226  }
227  reading.setPose(OrientedPoint(frontlaser->x, frontlaser->y, frontlaser->theta));
228 */
229  RangeReading reading=carmen2reading(*frontlaser);
230  addReading(reading);
231 }
232 
233 void CarmenWrapper::robot_rearlaser_handler(carmen_robot_laser_message* rearlaser) {
234 /* if (! m_rangeSensor){
235  double res=0;
236  if (frontlaser->num_readings==180 || frontlaser->num_readings==181)
237  res=M_PI/180;
238  if (frontlaser->num_readings==360 || frontlaser->num_readings==361)
239  res=M_PI/360;
240  assert(res>0);
241  m_rangeSensor=new RangeSensor("FLASER",frontlaser->num_readings, res, OrientedPoint(0,0,0), 0, 89.9);
242  m_sensorMap.insert(make_pair(string("FLASER"), m_rangeSensor));
243 
244  cout << __func__
245  << ": FrontLaser configured."
246  << " Readings " << m_rangeSensor->beams().size()
247  << " Resolution " << res << endl;
248  }
249 
250  RangeReading reading(m_rangeSensor, frontlaser->timestamp);
251  reading.resize(m_rangeSensor->beams().size());
252  for (unsigned int i=0; i< (unsigned int)frontlaser->num_readings; i++){
253  reading[i]=(double)frontlaser->range[i];
254  }
255  reading.setPose(OrientedPoint(frontlaser->x, frontlaser->y, frontlaser->theta));
256 */
257  RangeReading reading=carmen2reading(*rearlaser);
258  addReading(reading);
259 }
260 
261 
262 
263 
264 void CarmenWrapper:: navigator_go_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) {
265  carmen_navigator_go_message msg;
266  FORMATTER_PTR formatter;
267  IPC_RETURN_TYPE err;
268 
269  formatter = IPC_msgInstanceFormatter(msgRef);
270  err = IPC_unmarshallData(formatter, callData, &msg,
271  sizeof(carmen_navigator_go_message));
272  IPC_freeByteArray(callData);
273 
274  carmen_test_ipc_return
275  (err, "Could not unmarshall", IPC_msgInstanceName(msgRef));
276  cerr<<"go"<<endl;
277  stopped=false;
278 }
279 
280 
281 void CarmenWrapper:: navigator_stop_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) {
282  carmen_navigator_stop_message msg;
283  FORMATTER_PTR formatter;
284  IPC_RETURN_TYPE err;
285 
286  formatter = IPC_msgInstanceFormatter(msgRef);
287  err = IPC_unmarshallData(formatter, callData, &msg,
288  sizeof(carmen_navigator_stop_message));
289  IPC_freeByteArray(callData);
290 
291  carmen_test_ipc_return
292  (err, "Could not unmarshall", IPC_msgInstanceName(msgRef));
293  cerr<<"stop"<<endl;
294  stopped=true;
295 }
296 
297 
298 
299 void CarmenWrapper::simulator_truepos_handler(carmen_simulator_truepos_message* truepos){
300  m_truepos.x=truepos->truepose.x;
301  m_truepos.y=truepos->truepose.y;
302  m_truepos.theta=truepos->truepose.theta;
303 }
304 
305 RangeReading CarmenWrapper::carmen2reading(const carmen_robot_laser_message& msg){
306  //either front laser or rear laser
307  double dth=msg.laser_pose.theta-msg.robot_pose.theta;
308  dth=atan2(sin(dth), cos(dth));
309 
310  if (msg.laser_pose.theta==msg.robot_pose.theta && !m_frontLaser){
311  double res=0;
312  res = msg.config.angular_resolution;
313 // if (msg.num_readings==180 || msg.num_readings==181)
314 // res=M_PI/180;
315 // if (msg.num_readings==360 || msg.num_readings==361)
316 // res=M_PI/360;
317  assert(res>0);
318  string sensorName="FLASER";
319  OrientedPoint rpose(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta);
320  OrientedPoint lpose(msg.laser_pose.x, msg.laser_pose.y, msg.laser_pose.theta);
321  OrientedPoint dp=absoluteDifference(lpose, rpose);
322  m_frontLaser=new RangeSensor(sensorName,msg.num_readings, res, OrientedPoint(0,0,msg.laser_pose.theta-msg.robot_pose.theta), 0,
323  msg.config.maximum_range);
324  m_frontLaser->updateBeamsLookup();
325  m_sensorMap.insert(make_pair(sensorName, m_frontLaser));
326 
327  cout << __func__
328  << ": " << sensorName <<" configured."
329  << " Readings " << m_frontLaser->beams().size()
330  << " Resolution " << res << endl;
331  }
332  if (msg.laser_pose.theta!=msg.robot_pose.theta && !m_rearLaser){
333  double res=0;
334  res = msg.config.angular_resolution;
335 // if (msg.num_readings==180 || msg.num_readings==181)
336 // res=M_PI/180;
337 // if (msg.num_readings==360 || msg.num_readings==361)
338 // res=M_PI/360;
339  assert(res>0);
340  OrientedPoint rpose(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta);
341  OrientedPoint lpose(msg.laser_pose.x, msg.laser_pose.y, msg.laser_pose.theta);
342  OrientedPoint dp=absoluteDifference(lpose, rpose);
343  string sensorName="RLASER";
344  m_rearLaser=new RangeSensor(sensorName,msg.num_readings, res, OrientedPoint(0,0,msg.laser_pose.theta-msg.robot_pose.theta), 0,
345  msg.config.maximum_range);
346  m_rearLaser->updateBeamsLookup();
347  m_sensorMap.insert(make_pair(sensorName, m_rearLaser));
348 
349  cout << __func__
350  << ": " << sensorName <<" configured."
351  << " Readings " << m_rearLaser->beams().size()
352  << " Resolution " << res << endl;
353  }
354 
355  const RangeSensor * rs=(msg.laser_pose.theta==msg.robot_pose.theta)?m_frontLaser:m_rearLaser;
356  RangeReading reading(rs, msg.timestamp);
357  reading.resize(rs->beams().size());
358  for (unsigned int i=0; i< (unsigned int)msg.num_readings; i++){
359  reading[i]=(double)msg.range[i];
360  }
361  reading.setPose(OrientedPoint(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta));
362  return reading;
363 }
364 
365 void CarmenWrapper::publish_globalpos(carmen_localize_summary_p summary)
366 {
367  lock();
368  static carmen_localize_globalpos_message globalpos;
369  IPC_RETURN_TYPE err;
370 
371  globalpos.timestamp = carmen_get_time();
372  globalpos.host = carmen_get_host();
373  globalpos.globalpos = summary->mean;
374  globalpos.globalpos_std = summary->std;
375  globalpos.globalpos_xy_cov = summary->xy_cov;
376  globalpos.odometrypos = summary->odometry_pos;
377  globalpos.converged = summary->converged;
378  err = IPC_publishData(CARMEN_LOCALIZE_GLOBALPOS_NAME, &globalpos);
379  carmen_test_ipc_exit(err, "Could not publish",
380  CARMEN_LOCALIZE_GLOBALPOS_NAME);
381  unlock();
382 }
383 
384 /* publish a particle message */
385 
386 void CarmenWrapper::publish_particles(carmen_localize_particle_filter_p filter,
387  carmen_localize_summary_p summary)
388 {
389  lock();
390  static carmen_localize_particle_message pmsg;
391  IPC_RETURN_TYPE err;
392 
393  pmsg.timestamp = carmen_get_time();
394  pmsg.host = carmen_get_host();
395  pmsg.globalpos = summary->mean;
396  pmsg.globalpos_std = summary->mean;
397  pmsg.num_particles = filter->param->num_particles;
398  pmsg.particles = (carmen_localize_particle_ipc_p)filter->particles;
399  err = IPC_publishData(CARMEN_LOCALIZE_PARTICLE_NAME, &pmsg);
400  carmen_test_ipc_exit(err, "Could not publish",
401  CARMEN_LOCALIZE_PARTICLE_NAME);
402  fprintf(stderr, "P");
403  unlock();
404 }
405 
406 
407 
408 
409 
411  while (true) {
412  lock();
413  IPC_listen(100);
414  unlock();
415  usleep(20000);
416  }
417  return 0;
418 }
419 
421  if(sig == SIGINT) {
422  carmen_ipc_disconnect();
423 
424  fprintf(stderr, "\nDisconnecting (shutdown_module(%d) called).\n",sig);
425  exit(0);
426  }
427 }
428 /*
429 typedef struct {
430  int num_readings;
431  float *range;
432  char *tooclose;
433  double x, y, theta;//position of the laser on the robot
434  double odom_x, odom_y, odom_theta; //position of the center of the robot
435  double tv, rv;
436  double forward_safety_dist, side_safety_dist;
437  double turn_axis;
438  double timestamp;
439  char host[10];
440 } carmen_robot_laser_message;
441 */
442 
443 carmen_robot_laser_message CarmenWrapper::reading2carmen(const RangeReading& reading){
444  carmen_robot_laser_message frontlaser;
445  frontlaser.num_readings=reading.size();
446  frontlaser.range = new float[frontlaser.num_readings];
447  frontlaser.tooclose=0;
448  frontlaser.laser_pose.x=frontlaser.robot_pose.x=reading.getPose().x;
449  frontlaser.laser_pose.y=frontlaser.robot_pose.y=reading.getPose().y;
450  frontlaser.laser_pose.theta=frontlaser.robot_pose.theta=reading.getPose().theta;
451  frontlaser.tv=frontlaser.rv=0;
452  frontlaser.forward_safety_dist=frontlaser.side_safety_dist=0;
453  frontlaser.turn_axis=0;
454  frontlaser.timestamp=reading.getTime();
455  for (unsigned int i=0; i< reading.size(); i++){
456  frontlaser.range[i]=(float)reading[i];
457  }
458  return frontlaser;
459 }
460 
461 carmen_point_t CarmenWrapper::point2carmen (const OrientedPoint& p){
462  return (carmen_point_t){p.x,p.y,p.theta};
463 }
464 
465 OrientedPoint CarmenWrapper::carmen2point (const carmen_point_t& p){
466  return OrientedPoint(p.x, p.y, p.theta);
467 }
468 
469 
470 /*
471 int main (int argc, char** argv) {
472  CarmenWrapper::start(argc, argv);
473  while(1){
474  sleep(2);
475  RangeReading reading(0,0);
476  while(CarmenWrapper::getReading(reading)){
477  cout << "FLASER " << reading.size();
478  for (int i=0; i<reading.size(); i++)
479  cout << " " << reading[i];
480  cout << reading.getPose().x << " "
481  << reading.getPose().y << " "
482  << reading.getPose().theta << " 0 cazzo 0" << endl;
483  }
484  cout << endl;
485  }
486  return 1;
487 }
488 */
489 
static carmen_point_t point2carmen(const OrientedPoint &p)
static SensorMap m_sensorMap
Definition: carmenwrapper.h:87
const std::vector< Beam > & beams() const
Definition: rangesensor.h:24
static OrientedPoint carmen2point(const carmen_point_t &p)
static carmen_robot_laser_message reading2carmen(const RangeReading &reading)
bool err
Definition: gfs2stream.cpp:38
static pthread_mutex_t m_mutex
Definition: carmenwrapper.h:83
double getTime() const
Definition: sensoreading.h:12
static void robot_rearlaser_handler(carmen_robot_laser_message *frontlaser)
static OrientedPoint getTruePos()
static RangeSensor * m_frontLaser
Definition: carmenwrapper.h:88
static std::deque< RangeReading > m_rangeDeque
Definition: carmenwrapper.h:81
static void addReading(RangeReading &reading)
static void initializeIPC(const char *name)
static void navigator_stop_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *)
orientedpoint< T, A > absoluteDifference(const orientedpoint< T, A > &p1, const orientedpoint< T, A > &p2)
Definition: point.h:107
static void robot_frontlaser_handler(carmen_robot_laser_message *frontlaser)
static void * m_reading_function(void *)
static pthread_mutex_t m_lock
Definition: carmenwrapper.h:83
static bool sensorMapComputed()
static bool start(const char *name)
static void navigator_go_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *)
std::map< std::string, Sensor * > SensorMap
Definition: sensor.h:20
static bool m_threadRunning
Definition: carmenwrapper.h:86
static RangeSensor * m_rearLaser
Definition: carmenwrapper.h:88
void setPose(const OrientedPoint &pose)
Definition: rangereading.h:23
static void shutdown_module(int sig)
static void publish_globalpos(carmen_localize_summary_p summary)
static const SensorMap & sensorMap()
static void simulator_truepos_handler(carmen_simulator_truepos_message *truepos)
static void publish_particles(carmen_localize_particle_filter_p filter, carmen_localize_summary_p summary)
static int registerLocalizationMessages()
static RangeReading carmen2reading(const carmen_robot_laser_message &msg)
static bool getReading(RangeReading &reading)
static pthread_t m_readingThread
Definition: carmenwrapper.h:84
static OrientedPoint m_truepos
Definition: carmenwrapper.h:89
const OrientedPoint & getPose() const
Definition: rangereading.h:22
orientedpoint< double, double > OrientedPoint
Definition: point.h:203


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20