# Libraries linked to all programs in this Makefile
lib_LOADLIBES = robot mcl robomath roboorte robottype pthread rt m \
orte pathplan sharp map fsm rbtree motion robodim \
- actlib ulut shape_detect
+ actlib ulut shape_detect lidar
# Automatic generation of event definition files
include-pass_HOOKS = roboevent.c roboevent.h
#include <robodim.h>
#include <map.h>
#include <robomath.h>
-#include <hokuyo.h>
-#include <sick.h>
-
#include <shape_detect.h>
#include "map_handling.h"
return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
}
-void update_map_hokuyo(struct lidar_scan_type *s)
+void update_map_lidar(const struct lidar_params *l, struct lidar_scan_type *s)
{
double x, y;
struct robot_pos_type e;
- unsigned int i;
+ int i;
struct sharp_pos beam;
u_int16_t *data;
robot_get_est_pos(&e.x, &e.y, &e.phi);
- beam.x = SICK_CENTER_OFFSET_M;
+ beam.x = l->center_offset_m;
beam.y = 0;
data = s->data;
bool obstacle = true;
- for (i = 0; i < SICK_ARRAY_SIZE; i++) {
- beam.ang = SICK_INDEX_TO_RAD(i);
- if((beam.ang<(-SICK_RANGE_ANGLE_LEFT/180.0*M_PI))||((beam.ang>(SICK_RANGE_ANGLE_RIGHT/180.0*M_PI))))
+ for (i = 0; i < l->data_lenght; i++) {
+ beam.ang = index2rad(*l, i);
+
+ if ((beam.ang<(-l->range_angle_left/180.0*M_PI)) ||
+ ((beam.ang>(l->range_angle_right/180.0*M_PI)))) {
continue;
+ }
if(data[i] > 19 && data[i] < 2000) {
obst_coord(&e, &beam, data[i]/1000.0, &x, &y);
#define _MAP_HANDLING_H
#include <robodim.h>
+#include <lidar.h>
#ifdef __cplusplus
extern "C" {
void * thread_obstacle_forgeting(void * arg);
/*void update_map(struct sharps_type *s);*/
-void update_map_hokuyo(struct lidar_scan_type *s);
+void update_map_lidar(const struct lidar_params *l, struct lidar_scan_type *s);
#ifdef __cplusplus
}
#include <can_msg_def.h>
#include <actuators.h>
#include <ul_log.h>
+#include <lidar_params.h>
UL_LOG_CUST(ulogd_robot_orte); /* Log domain name = ulogd + name of the file */
ROBOT_UNLOCK(hokuyo);
if(!robot.ignore_hokuyo)
{
- //update_map_hokuyo(instance);
+ //update_map_lidar(&hokuyo_params, instance);
}
break;
}
ROBOT_UNLOCK(sick);
if(!robot.ignore_sick)
{
- update_map_hokuyo(instance);
+ update_map_lidar(&sick_params, instance);
}
break;
}
case DEADLINE:
robot.status[COMPONENT_SICK] = STATUS_FAILED;
- //system("killall -9 hokuyo");
DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
break;
}
# Libraries linked to all programs in this Makefile
lib_LOADLIBES = robot mcl robodim laser-nav robomath roboorte \
robottype pthread rt m orte pathplan sharp map fsm \
- rbtree motion actlib ulut shape_detect
+ rbtree motion actlib ulut shape_detect lidar