C++ (Cpp) SerialPort::Initialise Examples

Example #1

File: sonar.cpp Project: NIFTi-Fraunhofer/nifti_uav

int main(int argc, char* argv[])
{

sleep(1);
    ros::init(argc, argv, "sonar");
    ros::NodeHandle n;
    ros::NodeHandle nh("~");
    // get the number of sonar sensors
    nh.getParam("sensors", no_of_sensors);

  
    if (no_of_sensors <= 0) {
        ROS_ERROR("The number of sensors must be specified");
    }
    else {

        // get the device to read from
        std::string device_name = "";
        nh.getParam("device", device_name);
        if (device_name == "") device_name = "/dev/ttyUSB0";

        // do we need to set the address?
        std::string sensor_address_str = "";        
        nh.getParam("address", sensor_address_str);
        nh.setParam("address", ""); // clear the address parameter after it has been read
        if (sensor_address_str != "") {
            char* endstr;
            sensor_address = strtol(sensor_address_str.c_str(), &endstr, 16);
            if ((sensor_address > 0xfe) || (sensor_address < 0xe0)) {
                ROS_ERROR("The sensor address must be between 0xe0 and 0xfe");
                sensor_address_str = "";
            }
            else {
                if (no_of_sensors != 1) {
                    ROS_ERROR("When setting the address %s only one sensor should be attached to the module", sensor_address_str.c_str());
                    sensor_address_str = "";
                }
                else {
                    ROS_INFO("Setting sensor address to %s", sensor_address_str.c_str());
                }
            }
        }

        // sonar ranges are stored in this array
        int range_mm[no_of_sensors];

        // create the publisher
        sonar_pub = n.advertise<usbi2c::sonar_params>("sonar", 10);
	sonar_filtered_pub = n.advertise<sensor_msgs::Range> ("/sonar_height", 2, true);

	// create filters
	cAFilter = new ContingencyAnalysisWindowMeanFilter(3, 1.0);
  	outlierFilter = new OutlierRejectionFilter(-0.25, 0.5);

        SerialPort serial;
        if (serial.Open(device_name) == 0) {
            // 19200 baud, 8 data bits, no parity, 2 stop bits
            serial.Initialise(19200, 8, 2, PARITY_NONE);

            ROS_INFO("Connected to %s", device_name.c_str());

            if (sensor_address_str != "") {
                // set the address of the sonar sensor
                SetSensorAddress(serial, (unsigned char)sensor_address, 2000);
            }

            //ros::Rate loop_rate(20);
            while ((n.ok()) && (!transmission_complete)) {
                Update(serial,10,range_mm);
                ros::spinOnce();
                //loop_rate.sleep();
            }

            ROS_INFO("Closing %s", device_name.c_str());
        }
        else {
            ROS_WARN("Can't open %s", device_name.c_str());
        }
    }

    return 0;
}