43 carmen_ipc_initialize(1,(
char **)&name);
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);
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);
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);
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);
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);
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);
135 pthread_mutex_lock(&m_lock);
140 pthread_mutex_unlock(&m_lock);
145 pthread_mutex_lock(&m_mutex);
146 bool smok=m_frontLaser;
147 pthread_mutex_unlock(&m_mutex);
156 return m_threadRunning;
165 pthread_mutex_lock(&m_mutex);
166 ql=m_rangeDeque.size();
167 pthread_mutex_unlock(&m_mutex);
177 sem_wait(&m_dequeSem);
178 pthread_mutex_lock(&m_mutex);
179 if (!m_rangeDeque.empty()){
181 reading=m_rangeDeque.front();
182 m_rangeDeque.pop_front();
186 sem_getvalue(&m_dequeSem,&sval);
188 pthread_mutex_unlock(&m_mutex);
193 pthread_mutex_lock(&m_mutex);
194 m_rangeDeque.push_back(reading);
195 pthread_mutex_unlock(&m_mutex);
196 sem_post(&m_dequeSem);
198 sem_getvalue(&m_dequeSem,&sval);
265 carmen_navigator_go_message msg;
266 FORMATTER_PTR formatter;
269 formatter = IPC_msgInstanceFormatter(msgRef);
270 err = IPC_unmarshallData(formatter, callData, &msg,
271 sizeof(carmen_navigator_go_message));
272 IPC_freeByteArray(callData);
274 carmen_test_ipc_return
275 (
err,
"Could not unmarshall", IPC_msgInstanceName(msgRef));
282 carmen_navigator_stop_message msg;
283 FORMATTER_PTR formatter;
286 formatter = IPC_msgInstanceFormatter(msgRef);
287 err = IPC_unmarshallData(formatter, callData, &msg,
288 sizeof(carmen_navigator_stop_message));
289 IPC_freeByteArray(callData);
291 carmen_test_ipc_return
292 (
err,
"Could not unmarshall", IPC_msgInstanceName(msgRef));
300 m_truepos.x=truepos->truepose.x;
301 m_truepos.y=truepos->truepose.y;
302 m_truepos.theta=truepos->truepose.theta;
307 double dth=msg.laser_pose.theta-msg.robot_pose.theta;
308 dth=atan2(sin(dth), cos(dth));
310 if (msg.laser_pose.theta==msg.robot_pose.theta && !m_frontLaser){
312 res = msg.config.angular_resolution;
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);
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));
328 <<
": " << sensorName <<
" configured."
329 <<
" Readings " << m_frontLaser->beams().size()
330 <<
" Resolution " << res << endl;
332 if (msg.laser_pose.theta!=msg.robot_pose.theta && !m_rearLaser){
334 res = msg.config.angular_resolution;
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);
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));
350 <<
": " << sensorName <<
" configured."
351 <<
" Readings " << m_rearLaser->beams().size()
352 <<
" Resolution " << res << endl;
355 const RangeSensor * rs=(msg.laser_pose.theta==msg.robot_pose.theta)?m_frontLaser:m_rearLaser;
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];
368 static carmen_localize_globalpos_message globalpos;
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);
387 carmen_localize_summary_p summary)
390 static carmen_localize_particle_message pmsg;
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");
422 carmen_ipc_disconnect();
424 fprintf(stderr,
"\nDisconnecting (shutdown_module(%d) called).\n",sig);
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];
462 return (carmen_point_t){p.
x,p.
y,p.
theta};