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


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Mon Jun 10 2019 14:04:22