53 std::vector<rcdiscover::DeviceInfo> infos;
60 std::vector<std::vector<std::string>> devices;
61 std::vector<std::string> filtered_info = {
"",
"" };
62 for (
const auto& info : infos)
64 if (!interface.empty() &&
interface != info.getIfaceName())
71 if (user_defined_name.empty())
73 user_defined_name = info.getModelName();
77 if (device_name.empty())
79 if (info.getModelName().find(
"rc_visard") != std::string::npos)
81 filtered_info[0] = user_defined_name;
82 filtered_info[1] =
ip2string(info.getIP());
83 devices.push_back(filtered_info);
86 else if ((device_name == info.getSerialNumber()) || (device_name == user_defined_name))
88 filtered_info[0] = user_defined_name;
89 filtered_info[1] =
ip2string(info.getIP());
90 devices.push_back(filtered_info);
93 std::sort(devices.begin(), devices.end());
94 auto unique_devices_it = std::unique(devices.begin(), devices.end());
95 devices.erase(unique_devices_it, devices.end());
102 else if (devices.size() > 1)
104 ROS_FATAL_STREAM(
"Found " << devices.size() <<
" devices with the name '" << device_name
105 <<
"'. Please specify a unique device name.");
109 ROS_INFO_STREAM(
"Using device '" << device_name <<
"' with name '" << devices[0][0] <<
"' and IP address " 111 return devices[0][1];
114 int main(
int argc,
char** argv)
127 if (!host.empty() && !device.empty())
129 ROS_WARN(
"Both parameters: 'device' and 'host' are set. Using 'device' to start the client.");
132 if (!device.empty() || host.empty())
135 delim_pos = device.find(
':');
137 if (delim_pos != std::string::npos)
139 interface = device.substr(0, delim_pos);
140 device = device.substr(delim_pos + 1);
143 host =
getHost(device, interface);
156 catch (
const std::exception& ex)
158 ROS_FATAL(
"Client could not be created due to an error: %s", ex.what());
std::string ip2string(const uint32_t ip)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::string getHost(const std::string &device_name, const std::string &interface)
ROSCPP_DECL void spin(Spinner &spinner)
int main(int argc, char **argv)
curl_easy_setopt expects a curl_off_t argument for this option curl_easy_setopt expects a curl_write_callback argument for this option curl_easy_setopt expects a curl_ioctl_callback argument for this option curl_easy_setopt expects a curl_opensocket_callback argument for this option curl_easy_setopt expects a curl_debug_callback argument for this option curl_easy_setopt expects a curl_conv_callback argument for this option curl_easy_setopt expects a private data pointer as argument for this option curl_easy_setopt expects a FILE *argument for this option curl_easy_setopt expects a struct curl_httppost *argument for this option curl_easy_setopt expects a struct curl_slist *argument for this option curl_easy_getinfo expects a pointer to char *for this info curl_easy_getinfo expects a pointer to double for this info curl_easy_getinfo expects a pointer to struct curl_tlssessioninfo *for this info curl_easy_getinfo expects a pointer to curl_socket_t for this info size_t
#define ROS_FATAL_STREAM(args)
std::unique_ptr< rc_tagdetect_client::RosTagdetectClient > tagdetect_wrapper
bool getResponse(std::vector< DeviceInfo > &info, int timeout_per_socket=1000)
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
ROSCPP_DECL void shutdown()