void InitSerialPort(const wxString& portname, int baudrate)
{
static char errmsg[100];
if (portname == "NotConnected") return;
serptr=new SerialPort();
int errcode=serptr->Open(portname, baudrate, SerialConfig);
if (errcode < 0)
{
sprintf(errmsg,"unable to open serial port, error code=%d",errcode);
throw errmsg;
}
};
int main(int argc, char *argv[])
{
const char default_path[] = "/dev/ttyUSB2";
const char *path;
if(argc < 2)
path = default_path;
else
path = argv[1];
char buf[1024];
SerialPort sp;
sp.Init();
sp.Open(path, 9600, 8, 1, 'n');
while(1)
{
cout<<"Please key in data"<<endl;
cin>>buf;
sp.Write(buf, strlen(buf)+1);
}
return 0;
}
int main( int argc, char **argv )
{
int sig;
sigset_t termSig;
pthread_t readSerialThreadId;
pthread_t readStdinThreadId;
int rc;
int opt;
const char *baudStr = NULL;
#if defined( linux )
const char *portStr = "ttyS0";
#else
const char *portStr = "com1";
#endif
#if USE_I2C
PKT_TextChar = PacketTextChar;
PKT_SendChar = PacketSendChar;
PKT_PacketReceived = PacketReceived;
#endif
// signal( SIGINT, ControlC );
// signal( SIGTERM, ControlC );
LogInit( stdout );
while (( opt = getopt_long( argc, argv, "b:dhmp:rsv", gLongOption, NULL )) > 0 )
{
switch ( opt )
{
case 'b':
{
baudStr = optarg;
break;
}
case 'd':
{
gDebug = true;
break;
}
case 'g':
{
gDongle = true;
break;
}
case 'm':
{
gMegaLoad = true;
break;
}
case 'p':
{
portStr = optarg;
break;
}
case 'r':
{
gUseRtsToReset = true;
break;
}
case 's':
{
gStk500 = true;
break;
}
case 'v':
{
gVerbose = true;
break;
}
#if USE_I2C
case OPT_DEBUG_DONGLE:
{
gSerialDongle.m_debugDongle = true;
break;
}
case OPT_DEBUG_DONGLE_DATA:
{
gSerialDongle.m_debugDongleData = true;
break;
}
#endif
case '?':
case 'h':
{
Usage();
return 1;
}
}
}
if ( optind < argc )
{
if (( optind + 1 ) != argc )
{
fprintf( stderr, "Only one download file supported\n" );
return 1;
}
gDownloadFileName = argv[ optind ];
}
// Open the file to download
if ( gDownloadFileName != NULL )
{
// If we are asked to download a file, then read the entire file
// into memory.
if (( gDownloadInfo = ReadFile( gDownloadFileName )) == NULL )
{
return 1;
}
}
if ( !gSerialPort.Open( portStr, baudStr ))
{
return 1;
}
if ( gUseRtsToReset )
{
gSerialPort.StrobeRTS( 10 );
}
// Put stdin in raw mode
setbuf( stdin, NULL );
setbuf( stdout, NULL );
#if defined( unix )
sigemptyset( &termSig );
sigaddset( &termSig, SIGINT );
sigaddset( &termSig, SIGTERM );
pthread_sigmask( SIG_BLOCK, &termSig, NULL );
struct termios tio_new;
if ( tcgetattr( fileno( stdin ), &gTio_org ) < 0 )
{
LogError( "Unable to retrieve terminal settings\n" );
return 1;
}
tio_new = gTio_org;
tio_new.c_lflag &= ~( ICANON | ECHO );
tio_new.c_cc[VMIN] = 1;
tio_new.c_cc[VTIME] = 0;
if ( tcsetattr( fileno( stdin ), TCSANOW, &tio_new ) < 0 )
{
LogError( "Unable to update terminal settings\n" );
return 1;
}
#endif
const char *bootLoaderType = "*** Unknown ***";
if ( gDongle )
{
bootLoaderType = "Serial Dongle";
}
else
if ( gMegaLoad )
{
bootLoaderType = "MegaLoad v2.3";
}
else
if ( gStk500 )
{
bootLoaderType = "STK500";
}
gLogFs2 = fopen( "BootHost.log", "wb" );
Log( "BootHost - BootLoader: %s\n", bootLoaderType );
// Kick off the serial port reader thread.
rc = pthread_create( &readSerialThreadId, NULL, ReadSerialThread, &gSerialPort );
if ( rc != 0 )
{
fprintf( stderr, "Error creating ReadSerialThread: %d\n", rc );
return 1;
}
// Kick off the stdin reader thread.
rc = pthread_create( &readStdinThreadId, NULL, ReadStdinThread, NULL );
if ( rc != 0 )
{
fprintf( stderr, "Error creating ReadSerialThread: %d\n", rc );
return 1;
}
#if defined( unix )
// Wait for a termmination signal
if (( rc = sigwait( &termSig, &sig )) != 0 )
{
fprintf( stderr, "sigwait failed\n" );
}
else
{
fprintf( stderr, "Exiting...\n" );
}
pthread_cancel( readSerialThreadId );
pthread_cancel( readStdinThreadId );
// Restore stdin back to the way it was when we started
if ( tcsetattr( fileno( stdin ), TCSANOW, &gTio_org ) == -1 )
{
LogError( "Unable to restore terminal settings\n" );
}
#endif
#if defined( __CYGWIN__ )
// Under Windows closing the serial port and stdin will cause the reads
// to unblock. Under linux, this isn't required, but it doesn't hurt
// either.
gSerialPort.Close();
fclose( stdin );
#endif
// Unblock the termination signals so the user can kill us if we hang up
// waiting for the reader threads to exit.
#if defined( unix )
pthread_sigmask( SIG_UNBLOCK, &termSig, NULL );
#endif
pthread_join( readSerialThreadId, NULL );
pthread_join( readStdinThreadId, NULL );
#if !defined( __CYGWIN__ )
gSerialPort.Close();
fclose( stdin );
#endif
if ( gVerbose )
{
printf( "Done\n" );
}
return 0;
} // main
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;
}
