Exemplo n.º 1
0
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;
}
Exemplo n.º 2
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) {
Exemplo n.º 3
0
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;
}