Skip to content

Feature/update micro agent #3

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
BorjaOuterelo merged 1 commit into from
Nov 15, 2018
Merged

Feature/update micro agent #3

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
181 changes: 63 additions & 118 deletions uROS_Agent/src/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,10 +37,15 @@ 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()
Expand All @@ -50,7 +55,7 @@ void initializationError()
std::exit(EXIT_FAILURE);
}

uint16_t parsePort(const char* str_port)
uint16_t parsePort(const std::string& str_port)
{
uint16_t valid_port = 0;
try
Expand All @@ -70,73 +75,66 @@ uint16_t parsePort(const char* str_port)
return valid_port;
}


class uros_agent : public rclcpp::Node
{
public:
uros_agent() : Node("uROS_Agent")
{
}

~uros_agent()
{
}

private:

};


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

#ifdef __unix__
char exe_path[200];
ssize_t count = readlink("/proc/self/exe", exe_path, sizeof(exe_path));
if (count != -1) {
chdir(dirname(exe_path));
}
#endif
eprosima::uxr::Server* server = nullptr;
std::vector<std::string> cl(0);

bool initialized = false;

#ifdef __unix__
if(argc == 2 && (strcmp(argv[1], "-h") == 0 || strcmp(argv[1], "--help") == 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 if(argc >= 3 && strcmp(argv[1], "udp") == 0)
else
{
std::cout << "UDP agent initialization... ";
uint16_t port = parsePort(argv[2]);
eprosima::uxr::UDPServer* udp_server = (argc == 4) //discovery port
? new eprosima::uxr::UDPServer(port, parsePort(argv[3]))
: new eprosima::uxr::UDPServer(port);
if (udp_server->run())
for (int i = 1; i < argc; ++i)
{
initialized = true;
cl.push_back(argv[i]);
}
std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl;
}
else if(argc >= 3 && strcmp(argv[1], "tcp") == 0)

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(argv[2]);
eprosima::uxr::TCPServer* tcp_server = (argc == 4) //discovery port
? new eprosima::uxr::TCPServer(port, parsePort(argv[3]))
: new eprosima::uxr::TCPServer(port);
if (tcp_server->run())
{
initialized = true;
}
std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl;
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
}
else if(argc == 3 && strcmp(argv[1], "serial") == 0)
#ifndef _WIN32
else if((2 == cl.size()) && ("serial" == cl[0]))
{
std::cout << "Serial agent initialization... ";

/* Open serial device. */
int fd = open(argv[2], O_RDWR | O_NOCTTY);
int fd = open(cl[1].c_str(), O_RDWR | O_NOCTTY);
if (0 < fd)
{
struct termios tty_config;
Expand Down Expand Up @@ -181,18 +179,12 @@ int main(int argc, char** argv)

if (0 == tcsetattr(fd, TCSANOW, &tty_config))
{
eprosima::uxr::SerialServer* serial_server = new eprosima::uxr::SerialServer(fd, 0);
if (serial_server->run())
{
initialized = true;
}
server = new eprosima::uxr::SerialServer(fd, 0);
}
}
}

std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl;
}
else if (argc == 2 && strcmp(argv[1], "pseudo-serial") == 0)
else if ((1 == cl.size()) && ("pseudo-serial" == cl[0]))
{
std::cout << "Pseudo-Serial initialization... ";

Expand All @@ -208,78 +200,31 @@ int main(int argc, char** argv)
cfmakeraw(&attr);
tcflush(fd, TCIOFLUSH);
tcsetattr(fd, TCSANOW, &attr);
std::cout << "Device: " << dev << std::endl;
}
}

/* Launch server. */
eprosima::uxr::SerialServer* serial_server = new eprosima::uxr::SerialServer(fd, 0x00);
if (serial_server->run())
{
initialized = true;
}

if (initialized)
{
std::cout << "OK" << std::endl;
std::cout << "Device: " << dev << std::endl;
}
else
{
std::cout << "ERROR" << std::endl;
}
server = new eprosima::uxr::SerialServer(fd, 0x00);
}
#endif
else
{
initializationError();
}
#elif _WIN32
(void) argc;
(void) argv;

std::string server_type = "";
std::cout << "Select server type [udp|tcp]: ";
std::cin >> server_type;

if ("udp" == server_type)
if (nullptr != server)
{
uint16_t port = 0;
std::cout << "Select port: ";
std::cin >> port;

std::cout << "UDP agent initialization.... ";
eprosima::uxr::UDPServer* udp_server = new eprosima::uxr::UDPServer(port);
if (udp_server->run())
/* Launch server. */
if (server->run())
{
initialized = true;
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<uros_agent>());
rclcpp::shutdown();
server->stop();
}
std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl;
}
else if ("tcp" == server_type)
{
uint16_t port = 0;
std::cout << "Select port: ";
std::cin >> port;

std::cout << "UDP agent initialization.... ";
eprosima::uxr::TCPServer* tcp_server = new eprosima::uxr::TCPServer(port);
if (tcp_server->run())
else
{
initialized = true;
std::cout << "ERROR" << std::endl;
}
std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl;
}
else
{
std::cout << "Error server type" << std::endl;
}

#endif

if(initialized)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<uros_agent>());
rclcpp::shutdown();
}

return 0;
Expand Down