mirror of
https://github.com/micro-ROS/micro-ROS-Agent.git
synced 2026-09-06 18:05:17 +02:00
Updated to last version of Micoxrcedds Agent. (#3)
This commit is contained in:
Javier Moreno
committed by
Borja Outerelo
Borja Outerelo
parent
735babba37
commit
53555aa991
+66
-121
@@ -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()
|
||||
@@ -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
|
||||
@@ -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));
|
||||
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]);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
bool initialized = false;
|
||||
|
||||
#ifdef __unix__
|
||||
if(argc == 2 && (strcmp(argv[1], "-h") == 0 || strcmp(argv[1], "--help") == 0))
|
||||
if((1 == cl.size()) && (("-h" == cl[0]) || ("--help" == cl[0])))
|
||||
{
|
||||
showHelp();
|
||||
}
|
||||
else if(argc >= 3 && strcmp(argv[1], "udp") == 0)
|
||||
else if((2 <= cl.size()) && ("udp" == cl[0]))
|
||||
{
|
||||
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())
|
||||
{
|
||||
initialized = true;
|
||||
}
|
||||
std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl;
|
||||
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(argc >= 3 && strcmp(argv[1], "tcp") == 0)
|
||||
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;
|
||||
@@ -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... ";
|
||||
|
||||
@@ -208,79 +200,32 @@ int main(int argc, char** argv)
|
||||
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. */
|
||||
eprosima::uxr::SerialServer* serial_server = new eprosima::uxr::SerialServer(fd, 0x00);
|
||||
if (serial_server->run())
|
||||
if (server->run())
|
||||
{
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
if (initialized)
|
||||
{
|
||||
std::cout << "OK" << std::endl;
|
||||
std::cout << "Device: " << dev << std::endl;
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<uros_agent>());
|
||||
rclcpp::shutdown();
|
||||
server->stop();
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "ERROR" << std::endl;
|
||||
}
|
||||
}
|
||||
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)
|
||||
{
|
||||
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())
|
||||
{
|
||||
initialized = true;
|
||||
}
|
||||
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())
|
||||
{
|
||||
initialized = true;
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user