41 std::unique_ptr<SilhouetteMatchClient>
client;
50 std::string
getHost(
const std::string& device_name,
const std::string& interface)
56 std::vector<rcdiscover::DeviceInfo> infos;
63 std::vector<std::vector<std::string>> devices;
64 std::vector<std::string> filtered_info = {
"",
"" };
65 for (
const auto& info : infos)
67 if (!interface.empty() &&
interface != info.getIfaceName())
73 std::string user_defined_name = info.getUserName();
74 if (user_defined_name.empty())
76 user_defined_name = info.getModelName();
80 if (device_name.empty())
82 if (info.getModelName().find(
"rc_visard") != std::string::npos)
84 filtered_info[0] = user_defined_name;
85 filtered_info[1] =
ip2string(info.getIP());
86 devices.push_back(filtered_info);
89 else if ((device_name == info.getSerialNumber()) || (device_name == user_defined_name))
91 filtered_info[0] = user_defined_name;
92 filtered_info[1] =
ip2string(info.getIP());
93 devices.push_back(filtered_info);
96 std::sort(devices.begin(), devices.end());
97 auto unique_devices_it = std::unique(devices.begin(), devices.end());
98 devices.erase(unique_devices_it, devices.end());
105 else if (devices.size() > 1)
107 ROS_FATAL_STREAM(
"Found " << devices.size() <<
" devices with the name '" << device_name
108 <<
"'. Please specify a unique device name.");
112 ROS_INFO_STREAM(
"Using device '" << device_name <<
"' with name '" << devices[0][0] <<
"' and IP address " 114 return devices[0][1];
117 int main(
int argc,
char** argv)
128 if (!host.empty() && !device.empty())
130 ROS_WARN(
"Both parameters: 'device' and 'host' are set. Using 'device' to " 131 "start the client.");
134 if (!device.empty() || host.empty())
136 std::size_t delim_pos = 0;
137 delim_pos = device.find(
':');
138 std::string interface;
139 if (delim_pos != std::string::npos)
141 interface = device.substr(0, delim_pos);
142 device = device.substr(delim_pos + 1);
145 host =
getHost(device, interface);
157 catch (
const std::exception& ex)
159 ROS_FATAL(
"Client could not be created due to an error: %s", ex.what());
163 ROS_INFO_STREAM(
"SilhouetteMatch client node started for host: " << host);
std::string ip2string(const uint32_t ip)
std::string getHost(const std::string &device_name, const std::string &interface)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::unique_ptr< SilhouetteMatchClient > client
ROSCPP_DECL void spin(Spinner &spinner)
#define ROS_FATAL_STREAM(args)
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
int main(int argc, char **argv)
ROSCPP_DECL void shutdown()