mirror of
https://github.com/micro-ROS/micro-ROS-Agent.git
synced 2026-09-06 09:55:15 +02:00
Rename package. (#12)
This commit is contained in:
Borja Outerelo
@@ -0,0 +1,237 @@
|
||||
|
||||
// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima).
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifdef _WIN32
|
||||
#include <uxr/agent/transport/udp/UDPServerWindows.hpp>
|
||||
#include <uxr/agent/transport/tcp/TCPServerWindows.hpp>
|
||||
|
||||
#elif __unix__
|
||||
#include <unistd.h>
|
||||
#include <libgen.h>
|
||||
|
||||
#include <uxr/agent/transport/serial/SerialServerLinux.hpp>
|
||||
#include <uxr/agent/transport/udp/UDPServerLinux.hpp>
|
||||
#include <uxr/agent/transport/tcp/TCPServerLinux.hpp>
|
||||
#include <termios.h>
|
||||
#include <fcntl.h>
|
||||
#endif
|
||||
|
||||
#include <iterator>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <limits>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
void showHelp()
|
||||
{
|
||||
std::cout << "Usage: program <command>" << std::endl;
|
||||
std::cout << "List of commands:" << std::endl;
|
||||
#ifdef _WIN32
|
||||
std::cout << " udp <local_port>" << std::endl;
|
||||
std::cout << " tcp <local_port>" << std::endl;
|
||||
#else
|
||||
std::cout << " serial <device_name>" << std::endl;
|
||||
std::cout << " pseudo-serial" << std::endl;
|
||||
std::cout << " udp <local_port> [<discovery_port>]" << std::endl;
|
||||
std::cout << " tcp <local_port> [<discovery_port>]" << std::endl;
|
||||
#endif
|
||||
}
|
||||
|
||||
void initializationError()
|
||||
{
|
||||
std::cout << "Error: Invalid arguments." << std::endl;
|
||||
showHelp();
|
||||
std::exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
uint16_t parsePort(const std::string& str_port)
|
||||
{
|
||||
uint16_t valid_port = 0;
|
||||
try
|
||||
{
|
||||
int port = std::stoi(str_port);
|
||||
if(port > (std::numeric_limits<uint16_t>::max)())
|
||||
{
|
||||
std::cout << "Error: port number '" << port << "out of range." << std::endl;
|
||||
initializationError();
|
||||
}
|
||||
valid_port = uint16_t(port);
|
||||
}
|
||||
catch (const std::invalid_argument& )
|
||||
{
|
||||
initializationError();
|
||||
}
|
||||
return valid_port;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
eprosima::uxr::Server* server = nullptr;
|
||||
std::vector<std::string> cl(0);
|
||||
|
||||
if (1 == argc)
|
||||
{
|
||||
showHelp();
|
||||
std::cout << std::endl;
|
||||
std::cout << "Enter command: ";
|
||||
|
||||
std::string raw_cl;
|
||||
std::getline(std::cin, raw_cl);
|
||||
std::istringstream iss(raw_cl);
|
||||
cl.insert(cl.begin(), std::istream_iterator<std::string>(iss), std::istream_iterator<std::string>());
|
||||
std::cout << raw_cl << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int i = 1; i < argc; ++i)
|
||||
{
|
||||
cl.push_back(argv[i]);
|
||||
}
|
||||
}
|
||||
|
||||
if((1 == cl.size()) && (("-h" == cl[0]) || ("--help" == cl[0])))
|
||||
{
|
||||
showHelp();
|
||||
}
|
||||
else if((2 <= cl.size()) && ("udp" == cl[0]))
|
||||
{
|
||||
std::cout << "UDP agent initialization... ";
|
||||
uint16_t port = parsePort(cl[1]);
|
||||
#ifdef _WIN32
|
||||
server = new eprosima::uxr::UDPServer(port);
|
||||
#else
|
||||
server = (3 == cl.size()) //discovery port
|
||||
? new eprosima::uxr::UDPServer(port, parsePort(cl[2]))
|
||||
: new eprosima::uxr::UDPServer(port);
|
||||
#endif
|
||||
}
|
||||
else if((2 <= cl.size()) && ("tcp" == cl[0]))
|
||||
{
|
||||
std::cout << "TCP agent initialization... ";
|
||||
uint16_t port = parsePort(cl[1]);
|
||||
#ifdef _WIN32
|
||||
server = new eprosima::uxr::TCPServer(port);
|
||||
#else
|
||||
server = (3 == cl.size()) //discovery port
|
||||
? new eprosima::uxr::TCPServer(port, parsePort(cl[2]))
|
||||
: new eprosima::uxr::TCPServer(port);
|
||||
#endif
|
||||
}
|
||||
#ifndef _WIN32
|
||||
else if((2 == cl.size()) && ("serial" == cl[0]))
|
||||
{
|
||||
std::cout << "Serial agent initialization... ";
|
||||
|
||||
/* Open serial device. */
|
||||
int fd = open(cl[1].c_str(), O_RDWR | O_NOCTTY);
|
||||
if (0 < fd)
|
||||
{
|
||||
struct termios tty_config;
|
||||
memset(&tty_config, 0, sizeof(tty_config));
|
||||
if (0 == tcgetattr(fd, &tty_config))
|
||||
{
|
||||
/* Setting CONTROL OPTIONS. */
|
||||
tty_config.c_cflag |= CREAD; // Enable read.
|
||||
tty_config.c_cflag |= CLOCAL; // Set local mode.
|
||||
tty_config.c_cflag &= ~PARENB; // Disable parity.
|
||||
tty_config.c_cflag &= ~CSTOPB; // Set one stop bit.
|
||||
tty_config.c_cflag &= ~CSIZE; // Mask the character size bits.
|
||||
tty_config.c_cflag |= CS8; // Set 8 data bits.
|
||||
tty_config.c_cflag &= ~CRTSCTS; // Disable hardware flow control.
|
||||
|
||||
/* Setting LOCAL OPTIONS. */
|
||||
tty_config.c_lflag &= ~ICANON; // Set non-canonical input.
|
||||
tty_config.c_lflag &= ~ECHO; // Disable echoing of input characters.
|
||||
tty_config.c_lflag &= ~ECHOE; // Disable echoing the erase character.
|
||||
tty_config.c_lflag &= ~ISIG; // Disable SIGINTR, SIGSUSP, SIGDSUSP and SIGQUIT signals.
|
||||
|
||||
/* Setting INPUT OPTIONS. */
|
||||
tty_config.c_iflag &= ~IXON; // Disable output software flow control.
|
||||
tty_config.c_iflag &= ~IXOFF; // Disable input software flow control.
|
||||
tty_config.c_iflag &= ~INPCK; // Disable parity check.
|
||||
tty_config.c_iflag &= ~ISTRIP; // Disable strip parity bits.
|
||||
tty_config.c_iflag &= ~IGNBRK; // No ignore break condition.
|
||||
tty_config.c_iflag &= ~IGNCR; // No ignore carrier return.
|
||||
tty_config.c_iflag &= ~INLCR; // No map NL to CR.
|
||||
tty_config.c_iflag &= ~ICRNL; // No map CR to NL.
|
||||
|
||||
/* Setting OUTPUT OPTIONS. */
|
||||
tty_config.c_oflag &= ~OPOST; // Set raw output.
|
||||
|
||||
/* Setting OUTPUT CHARACTERS. */
|
||||
tty_config.c_cc[VMIN] = 10;
|
||||
tty_config.c_cc[VTIME] = 1;
|
||||
|
||||
/* Setting BAUD RATE. */
|
||||
cfsetispeed(&tty_config, B115200);
|
||||
cfsetospeed(&tty_config, B115200);
|
||||
|
||||
if (0 == tcsetattr(fd, TCSANOW, &tty_config))
|
||||
{
|
||||
server = new eprosima::uxr::SerialServer(fd, 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if ((1 == cl.size()) && ("pseudo-serial" == cl[0]))
|
||||
{
|
||||
std::cout << "Pseudo-Serial initialization... ";
|
||||
|
||||
/* Open pseudo-terminal. */
|
||||
char* dev = NULL;
|
||||
int fd = posix_openpt(O_RDWR | O_NOCTTY);
|
||||
if (-1 != fd)
|
||||
{
|
||||
if (grantpt(fd) == 0 && unlockpt(fd) == 0 && (dev = ptsname(fd)))
|
||||
{
|
||||
struct termios attr;
|
||||
tcgetattr(fd, &attr);
|
||||
cfmakeraw(&attr);
|
||||
tcflush(fd, TCIOFLUSH);
|
||||
tcsetattr(fd, TCSANOW, &attr);
|
||||
std::cout << "Device: " << dev << std::endl;
|
||||
}
|
||||
}
|
||||
server = new eprosima::uxr::SerialServer(fd, 0x00);
|
||||
}
|
||||
#endif
|
||||
else
|
||||
{
|
||||
initializationError();
|
||||
}
|
||||
|
||||
if (nullptr != server)
|
||||
{
|
||||
/* Launch server. */
|
||||
if (server->run())
|
||||
{
|
||||
std::cout << "OK" << std::endl;
|
||||
std::cin.clear();
|
||||
char exit_flag = 0;
|
||||
while ('q' != exit_flag)
|
||||
{
|
||||
std::cout << "Enter 'q' for exit" << std::endl;
|
||||
std::cin >> exit_flag;
|
||||
}
|
||||
server->stop();
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "ERROR" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user