void LRF::OpenLRF(sensor_msgs::LaserScan &data)
{
data.angle_min = deg_min*M_PI/180.0;
data.angle_max = deg_max*M_PI/180.0;
data.range_min = range_min;
data.range_max = range_max;
data.angle_increment = (skip+1)/1080.0*270.0*M_PI/180.0;
step = (deg_max - deg_min)*4.0 + 1;
data.ranges.resize(step);
data.intensities.resize(step);
if(eth_flg){
if(urg_open(&urg,URG_ETHERNET,ip_address.c_str(),(long)ip_port) < 0){
ROS_WARN("Ethernet LRF open error.");
throw runtime_error("Ethernet LRF open error.");
}
}
else{
if(urg_open(&urg,URG_SERIAL,serial_port.c_str(),(long)serial_baud) < 0){
ROS_WARN("Serial LRF open error.");
throw runtime_error("Serial LRF open error.");
}
}
urg_set_scanning_parameter(&urg, urg_deg2step(&urg,deg_min), urg_deg2step(&urg,deg_max), skip);
if(publish_intensity)
urg_start_measurement(&urg, URG_DISTANCE_INTENSITY, URG_SCAN_INFINITY, skip);
else
urg_start_measurement(&urg, URG_DISTANCE, URG_SCAN_INFINITY, skip);
}
int open_urg_sensor(urg_t *urg, int argc, char *argv[])
{
#if defined(URG_WINDOWS_OS)
const char *device = "COM4";
#elif defined(URG_LINUX_OS)
const char *device = "/dev/ttyACM0";
//const char *device = "/dev/ttyUSB0";
#else
const char *device = "/dev/tty.usbmodemfd333131";
#endif
urg_connection_type_t connection_type = URG_SERIAL;
long baudrate_or_port = 115200;
//const char *ip_address = "localhost";
const char *ip_address = "192.168.0.10";
int i;
// \~japanese 接続タイプの切替え
for (i = 1; i < argc; ++i) {
if (!strcmp(argv[i], "-e")) {
connection_type = URG_ETHERNET;
baudrate_or_port = 10940;
device = ip_address;
}
}
// \~japanese 接続
if (urg_open(urg, connection_type, device, baudrate_or_port) < 0) {
printf("urg_open: %s, %ld: %s\n",
device, baudrate_or_port, urg_error(urg));
return -1;
}
return 0;
}
int init(urg_t *urg){
int error;
int i;
char *device = "/dev/ttyACM0";
//SERIAL PORT DETECTION
fprintf(stderr, "List of serial ports :\n");
int found_port_size = urg_serial_find_port();
if (found_port_size == 0) {
fprintf(stderr, "could not found serial ports.\n");
exit(EXIT_FAILURE);
}
for (i = 0; i < found_port_size; ++i) {
fprintf(stderr, "%s", (char *)urg_serial_port_name(i));
device = (char *)urg_serial_port_name(i);
}
fprintf(stderr, "\n");
fprintf(stderr, "Connection à %s\n", device);
error = urg_open(urg, URG_SERIAL, device, BAUDRATE);
if(error < 0){
error_func(urg, "connection failed");
}
else{
fprintf(stderr, "Connection établie à %s\n", device);
urg_set_scanning_parameter(urg, urg_rad2step(urg, ANGLE_MIN), urg_rad2step(urg, ANGLE_MAX), 0);//scan en continu, on ne garde que les point entre -PI/2 et PI/2
fprintf(stderr, "Parameters set\n");
error = urg_start_measurement(urg, URG_DISTANCE, URG_SCAN_INFINITY, 0);
if(error < 0){
error_func(urg, "failed to start measurement");
}
}
get_val(calc, 0, urg);//calcule les tables de cos/sin à l'avance
return error;
}
int main(int argc, char **argv) {
urg_t connection;
urg_connection_type_t type = URG_ETHERNET;
char *device = "192.168.0.10";
int direction_count, max_data_size, i;
long timestamp;
long *data;
// open connection
if (urg_open(&connection, type, device, 10940) < 0)
return 1;
max_data_size = urg_max_data_size(&connection);
data = malloc(sizeof(long)*max_data_size);
// start measurement
// (connection, type, scan times (0 is keep going), skip)
urg_start_measurement(&connection, type, 0, 0);
while (1) {
direction_count = urg_get_distance(&connection, data, ×tamp);
for (i = 0; i < max_data_size; i++) {
printf("%i ", data[i]);
}
printf("\nmax_data_size: %i\n", max_data_size);
sleep(1);
}
return 0;
}
int main(void)
{
const char connect_device[] = "/dev/ttyACM0";
const long connect_baudrate = 115200;
urg_t urg;
int first_step;
int last_step;
int skip_step;
int scan_times;
int skip_scan;
int ret;
// 計測パラメータの設定
// センサに対して接続を行う。
// 接続を行うと、計測パラメータの設定は初期化される
ret = urg_open(&urg, URG_SERIAL, connect_device, connect_baudrate);
// \todo check error code
// 計測範囲を指定する
// センサ正面方向の 90 [deg] 範囲のデータ取得を行い、ステップ間引きを行わない例
first_step = urg_rad2step(&urg, -45);
last_step = urg_rad2step(&urg, +45);
skip_step = 0;
ret = urg_set_scanning_parameter(&urg, first_step, last_step, skip_step);
// \todo check error code
// 計測回数と計測の間引きを指定して、計測を開始する
// 123 回の計測を指示し、スキャンの間引きを行わない例
scan_times = 123;
skip_scan = 0;
ret = urg_start_measurement(&urg, URG_DISTANCE, scan_times, skip_scan);
// \todo check error code
return 0;
}
int lidar_init() {
if (urg_open(&lidar, URG_SERIAL, dev, baud) < 0) {
printf("ERROR: unable to open LIDAR\n");
return -1;
}
length_data = (long*) malloc(sizeof(long) * urg_max_data_size(&lidar));
lidar_open = 1;
return 0;
}
bool openSensor(void * params)
{
URGPARAMS * tempparams=(URGPARAMS *)params;
int n=urg_open(&(tempparams->urg),tempparams->connection_type,tempparams->device_or_address.toStdString().c_str(),tempparams->baudreate_or_port);
if(n<0)
{
return 0;
}
n=urg_set_scanning_parameter(&(tempparams->urg),tempparams->first_step,tempparams->last_step,tempparams->skip_step);
if(n<0)
{
return 0;
}
return 1;
}
bool openSensor(void * params, QObject ** trigger, QString & triggersignal) //open the sensor using params
{
URGSENSORPARAMS * URGparams=(URGSENSORPARAMS *)params;
int n=urg_open(&(URGparams->urg),URGparams->connection_type,URGparams->device_or_address.toStdString().c_str(),URGparams->baudreate_or_port);
if(n<0)
{
return 0;
}
n=urg_set_scanning_parameter(&(URGparams->urg),URGparams->first_step,URGparams->last_step,URGparams->skip_step);
if(n<0)
{
return 0;
}
return 1;
}
int main(void)
{
urg_t urg;
int ret;
// \~japanese イーサーネット接続でのセンサとの接続と距離データの取得
// \~english Connects to the sensor via Ethernet and receives range data
const char connect_address[] = "192.168.0.10";
const long connect_port = 10940;
// \~japanese センサに対して接続を行う。
// \~english Connects to the sensor
ret = urg_open(&urg, URG_ETHERNET, connect_address, connect_port);
// \todo check error code
return 0;
}
void* sensor_init(void *null_pointer) {
int max_data_size;
urg_connection_type_t type_eth = URG_ETHERNET;
urg_measurement_type_t m_type_eth = URG_MULTIECHO;
char *device_eth = "192.168.0.10";
char *device_usb = "/dev/ttyACM0";
eth = 0;
usb = 0;
if (urg_open(&connection_eth, type_eth, device_eth, 10940) < 0) {
printf("Could not connect to the ethernet sensor\n");
} else eth = 1;
connection_usb = scipConnect(device_usb);
if (connection_usb == NULL) {
printf("Could not connect to the usb sensor\n");
} else usb = 1;
// start measurement
if (eth) {
max_data_size = urg_max_data_size(&connection_eth);
buffer_eth = malloc(sizeof(int)*max_data_size);
urg_start_measurement(&connection_eth, m_type_eth, 0, 0);
last_poll_eth = utime() - poll_time_eth;
}
if (usb) {
switchToScip2(connection_usb);
scip2SetComSpeed(connection_usb,115200);
last_poll_usb = utime() - poll_time_usb;
}
return NULL;
}
int main(int argc, char *argv[])
{
enum {
CAPTURE_TIMES = 1,
};
urg_t urg;
urg_connection_type_t connection_type = URG_SERIAL;
long *data = NULL;
long time_stamp;
int n;
int i;
#if defined(URG_WINDOWS_OS)
const char *device = "COM3";
#elif defined(URG_LINUX_OS)
const char *device = "/dev/ttyACM0";
#else
#endif
long baudrate_or_port = 115200;
const char *ip_address = "192.168.0.10";
// 接続タイプの切替え
for (i = 1; i < argc; ++i) {
if (!strcmp(argv[i], "-e")) {
connection_type = URG_ETHERNET;
baudrate_or_port = 10940;
device = ip_address;
}
}
for (i = 1; i < argc; ++i) {
if (!strcmp(argv[i], "-e")) {
connection_type = URG_ETHERNET;
}
}
// \~japanese 接続
if (urg_open(&urg, connection_type, device, baudrate_or_port) < 0) {
printf("urg_open: %s\n", urg_error(&urg));
return 1;
}
data = (long *)malloc(urg_max_data_size(&urg) * 3 * sizeof(data[0]));
if (!data) {
perror("urg_max_index()");
return 1;
}
// \~japanese データ取得
urg_start_measurement(&urg, URG_MULTIECHO, CAPTURE_TIMES, 0);
for (i = 0; i < CAPTURE_TIMES; ++i) {
n = urg_get_distance(&urg, data, &time_stamp);
if (n <= 0) {
printf("urg_distance: %s\n", urg_error(&urg));
free(data);
urg_close(&urg);
return 1;
}
print_data(&urg, data, n, time_stamp);
}
// \~japanese 切断
free(data);
urg_close(&urg);
#if defined(URG_MSC)
getchar();
#endif
return 0;
}
