int main(int argc, const char * argv[]) {
const char * opt_com_path = NULL;
_u32 opt_com_baudrate = 115200;
u_result op_result;
// read serial port from the command line...
if (argc>1) opt_com_path = argv[1]; // or set to a fixed value: e.g. "com3"
// read baud rate from the command line if specified...
if (argc>2) opt_com_baudrate = strtoul(argv[2], NULL, 10);
if (!opt_com_path) {
#ifdef _WIN32
// use default com port
opt_com_path = "\\\\.\\com3";
#else
opt_com_path = "/dev/ttyUSB0";
#endif
}
// create the driver instance
RPlidarDriver * drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);
if (!drv) {
fprintf(stderr, "insufficent memory, exit\n");
exit(-2);
}
// make connection...
if (IS_FAIL(drv->connect(opt_com_path, opt_com_baudrate))) {
fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n"
, opt_com_path);
RPlidarDriver::DisposeDriver(drv);
return 0;
}
// check health...
if (!checkRPLIDARHealth(drv)) {
RPlidarDriver::DisposeDriver(drv);
return 0;
}
// start scan...
drv->startScan();
#define WINDOW_SIZE 800
SDL_Init(SDL_INIT_EVERYTHING);
SDL_Surface *screen = SDL_SetVideoMode(WINDOW_SIZE, WINDOW_SIZE, 32, SDL_SWSURFACE);
bool running = true;
// fetch result and print it out...
while (running) {
rplidar_response_measurement_node_t nodes[360*2];
size_t count = _countof(nodes);
op_result = drv->grabScanData(nodes, count);
SDL_Event event;
while (SDL_PollEvent(&event)) {
if (event.type == SDL_QUIT) {
running = false;
break;
}
}
SDL_FillRect(screen, &screen->clip_rect, SDL_MapRGB(screen->format, 0x00, 0x00, 0x00));
uint32_t color = SDL_MapRGB(screen->format, 0x00, 0xFF, 0xFF);
SDL_Rect rect;
rect.w = 1;
rect.h = 1;
if (IS_OK(op_result)) {
drv->ascendScanData(nodes, count);
for (int pos = 0; pos < (int)count ; ++pos) {
// printf("%s theta: %03.2f Dist: %08.2f Q: %d \n",
// (nodes[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) ?"S ":" ",
// (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f,
// nodes[pos].distance_q2/4.0f,
// nodes[pos].sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
double theta = (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f + 180.0; // rotate 180 deg
double distance = nodes[pos].distance_q2/20.0f;
rect.x = (int)(distance * cos((double)theta * 3.1415926535 / 180.0)) + WINDOW_SIZE / 2;
rect.y = (int)(distance * sin((double)theta * 3.1415926535 / 180.0)) + WINDOW_SIZE / 2;
if (rect.x >= 0 && rect.x < WINDOW_SIZE && rect.y >= 0 && rect.y < WINDOW_SIZE)
SDL_FillRect(screen, &rect, color);
rect.x++;
if (rect.x >= 0 && rect.x < WINDOW_SIZE && rect.y >= 0 && rect.y < WINDOW_SIZE)
SDL_FillRect(screen, &rect, color);
rect.x -= 2;
if (rect.x >= 0 && rect.x < WINDOW_SIZE && rect.y >= 0 && rect.y < WINDOW_SIZE)
SDL_FillRect(screen, &rect, color);
rect.x++;
rect.y++;
if (rect.x >= 0 && rect.x < WINDOW_SIZE && rect.y >= 0 && rect.y < WINDOW_SIZE)
SDL_FillRect(screen, &rect, color);
rect.y -= 2;
if (rect.x >= 0 && rect.x < WINDOW_SIZE && rect.y >= 0 && rect.y < WINDOW_SIZE)
SDL_FillRect(screen, &rect, color);
rect.y++;
}
}
SDL_Flip(screen);
}
SDL_Quit();
// done!
RPlidarDriver::DisposeDriver(drv);
return 0;
}
int main(int argc, char * argv[]) {
ros::init(argc, argv, "rplidar_node");
std::string serial_port;
int serial_baudrate = 115200;
std::string frame_id;
bool inverted = false;
bool angle_compensate = true;
ros::NodeHandle nh;
ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1000);
ros::NodeHandle nh_private("~");
nh_private.param<std::string>("serial_port", serial_port, "/dev/ttyUSB0");
nh_private.param<int>("serial_baudrate", serial_baudrate, 115200);
nh_private.param<std::string>("frame_id", frame_id, "laser_frame");
nh_private.param<bool>("inverted", inverted, "false");
nh_private.param<bool>("angle_compensate", angle_compensate, "true");
u_result op_result;
// create the driver instance
RPlidarDriver * drv =
RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);
if (!drv) {
fprintf(stderr, "Create Driver fail, exit\n");
return -2;
}
// make connection...
if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {
fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n"
, serial_port.c_str());
RPlidarDriver::DisposeDriver(drv);
return -1;
}
// check health...
if (!checkRPLIDARHealth(drv)) {
RPlidarDriver::DisposeDriver(drv);
return -1;
}
// start scan...
drv->startScan();
ros::Time start_scan_time;
ros::Time end_scan_time;
double scan_duration;
while (ros::ok()) {
rplidar_response_measurement_node_t nodes[360*2];
size_t count = _countof(nodes);
start_scan_time = ros::Time::now();
op_result = drv->grabScanData(nodes, count);
end_scan_time = ros::Time::now();
scan_duration = (end_scan_time - start_scan_time).toSec() * 1e-3;
if (op_result == RESULT_OK) {
op_result = drv->ascendScanData(nodes, count);
float angle_min = DEG2RAD(0.0f);
float angle_max = DEG2RAD(359.0f);
if (op_result == RESULT_OK) {
if (angle_compensate) {
const int angle_compensate_nodes_count = 360;
const int angle_compensate_multiple = 1;
int angle_compensate_offset = 0;
rplidar_response_measurement_node_t angle_compensate_nodes[angle_compensate_nodes_count];
memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_t));
int i = 0, j = 0;
for( ; i < count; i++ ) {
if (nodes[i].distance_q2 != 0) {
float angle = (float)((nodes[i].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
int angle_value = (int)(angle * angle_compensate_multiple);
if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value;
for (j = 0; j < angle_compensate_multiple; j++) {
angle_compensate_nodes[angle_value-angle_compensate_offset+j] = nodes[i];
}
}
}
publish_scan(&scan_pub, angle_compensate_nodes, angle_compensate_nodes_count,
start_scan_time, scan_duration, inverted,
angle_min, angle_max,
frame_id);
} else {
int start_node = 0, end_node = 0;
int i = 0;
// find the first valid node and last valid node
while (nodes[i++].distance_q2 == 0);
start_node = i-1;
i = count -1;
while (nodes[i--].distance_q2 == 0);
end_node = i+1;
angle_min = DEG2RAD((float)(nodes[start_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
angle_max = DEG2RAD((float)(nodes[end_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
publish_scan(&scan_pub, &nodes[start_node], end_node-start_node +1,
start_scan_time, scan_duration, inverted,
angle_min, angle_max,
frame_id);
}
} else if (op_result == RESULT_OPERATION_FAIL) {
int main(int argc, const char * argv[]) {
// Tests
printf("Point distance test, expected 321.00623046912966: Actual %f\n", pointDistance(22, 32, 343, 34));
pair <float, float> test_dist = distance(34.4, 45, 53, 62.4);
printf("Difference in coordinates between two points, expected (18.6, 17.4) %f %f\n", test_dist.first, test_dist.second);
printf("Dot product between two vectors, expected 27.31: Actual %f\n", dotProduct(1.1, 2.3, 5.8, 9.1));
printf("Length of a vector (used for normalize), expected 5.594640292279745: Actual %f\n", length(5.1, 2.3));
pair <float, float> test_norm1 = normalize(-3.3, 4.5);
pair <float, float> test_norm2 = normalize(0, 0);
printf("Normalizing a vector, expected (-0.5913636636275174, 0.8064049958557056), (1,1): Actual %f %f %f %f\n", test_norm1.first, test_norm1.second, test_norm2.first, test_norm2.second);
printf("Check if line segment intersects with a circle, expected True(1), False(0): Actual %d %d\n", segment_circle(make_pair(3, 4), make_pair(400, 400), make_pair(200, 200), 50), segment_circle(make_pair(3, 4), make_pair(400, 400), make_pair(500, 200), 50));
twoPoints test_tang = calculateTangentLines(make_pair(3, 1));
printf("Calculating Tangent Lines, with R = 5, quadcopterc = (3,4), obstalce (3,1): Expected (84.79797338056486, 4.0, -78.79797338056486, 4.0), Actual %f %f %f %f\n", test_tang.point1.first, test_tang.point1.second, test_tang.point2.first, test_tang.point2.second);
pair <float, float> point = updatePoint(make_pair(2, 2));
printf("Expecting points (1.5527864045000421, 1.18985352037725), Actual: %f %f\n", point.first, point.second);
const char * opt_com_path = NULL;
_u32 opt_com_baudrate = 115200;
u_result op_result;
// read serial port from the command line...
if (argc > 1) opt_com_path = argv[1]; // or set to a fixed value: e.g. "com3"
// read baud rate from the command line if specified...
if (argc > 2) opt_com_baudrate = strtoul(argv[2], NULL, 10);
if (!opt_com_path) {
#ifdef _WIN32
// use default com port
opt_com_path = "\\\\.\\com3";
#else
opt_com_path = "/dev/ttyUSB0";
#endif
}
// create the driver instance
RPlidarDriver * drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);
if (!drv) {
fprintf(stderr, "insufficent memory, exit\n");
exit(-2);
}
// make connection...
if (IS_FAIL(drv->connect(opt_com_path, opt_com_baudrate))) {
fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n"
, opt_com_path);
goto on_finished;
}
// check health...
if (!checkRPLIDARHealth(drv)) {
goto on_finished;
}
// start scan...
drv->startScan();
// fetech result and print it out...
while (1) {
std::vector<pair<float, float> > obstacles;
std::vector<std::vector<pair<float, float > > > obstacles_bucket;
std::vector<pair<float, float> > temp_bucket;
rplidar_response_measurement_node_t nodes[360 * 2];
size_t count = _countof(nodes);
op_result = drv->grabScanData(nodes, count);
if (IS_OK(op_result)) {
drv->ascendScanData(nodes, count);
for (int pos = 0; pos < (int)count; ++pos) {
float x = nodes[pos].distance_q2 / 4.0f;
float angle = (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f;
float sine = sin(angle*PI / 180);
float cosine = cos(angle*PI / 180);
float a = x*sine/10;
float b = x*cosine/10;
obstacles.push_back(make_pair(a, b));
//printf("%f %f\n", a, b);
/*printf("%s theta: %03.2f X coordinates: %f Y coordinates: %f \n",
(nodes[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) ? "S " : " ",
(nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f,
a,
b);*/
}
}
std::pair<float, float> direction = make_pair(1, 1);
pair <float, float> previous_point = obstacles[0];
temp_bucket.push_back(previous_point);
int bucket_counter = 0;
for (int i = 1; i<obstacles.size(); i++) {
if (pointDistance(previous_point.first, previous_point.second, obstacles[i].first, obstacles[i].second)<2 * R) {
temp_bucket.push_back(obstacles[i]);
}
else {
obstacles_bucket.push_back(temp_bucket);
temp_bucket = { obstacles[i] };
}
previous_point = obstacles[i];
}
if (temp_bucket.size() > 1) {
obstacles_bucket.push_back(temp_bucket);
}
//printf("%i %i \n", obstacles.size(), obstacles_bucket.size());
pair<float, float> current_point;
closest_obstacle(obstacles_bucket);
twoPoints tangents = closest_tangents(obstacles_bucket);
if (delta2 > 0) {
if (length(tangents.point1.first - goal.first, tangents.point1.second - goal.second) > length(tangents.point2.first - goal.first, tangents.point2.second) && current_obstacle1.first != current_obstacle2.first && current_obstacle1.second != current_obstacle2.second) {
tangents.point2 = updatePoint(tangents.point2);
pair<float, float> dist = distance(quadcopterc.first, quadcopterc.second, tangents.point2.first, tangents.point2.second);
direction = normalize(dist.first,dist.second);
current_point = tangents.point2;
}
else if (current_obstacle1!=current_obstacle2) {
tangents.point1 = updatePoint(tangents.point1);
pair<float, float> dist = distance(quadcopterc.first, quadcopterc.second, tangents.point1.first, tangents.point1.second);
direction = normalize(dist.first, dist.second);
current_point = tangents.point1;
}
else if (pointDistance(current_point.first, current_point.second, tangents.point1.first, tangents.point1.second) > pointDistance(current_point.first, current_point.second, tangents.point2.first, tangents.point1.second)) {
tangents.point2 = updatePoint(tangents.point2);
pair<float, float> dist = distance(quadcopterc.first, quadcopterc.second, tangents.point2.first, tangents.point2.second);
direction = normalize(dist.first, dist.second);
current_point = tangents.point2;
}
else {
tangents.point1 = updatePoint(tangents.point1);
pair<float, float> dist = distance(quadcopterc.first, quadcopterc.second, tangents.point1.first, tangents.point1.second);
direction = normalize(dist.first, dist.second);
current_point = tangents.point1;
}
}
else {
pair<float, float> dist = distance(quadcopterc.first, quadcopterc.second, goal.first, goal.second);
direction = normalize(dist.first, dist.second);
}
printf("%f %f\n", direction.first, direction.second);
}
// done!
on_finished:
RPlidarDriver::DisposeDriver(drv);
double param, result;
param = 350.0;
result = sin(param*PI / 180);
printf("The sine of %f degrees is %f.\n", param, result);
std::cin.get();
return 0;
}
