bool urdf_traverser::helpers::getSubdirPath(const std::string& from, const std::string& to, std::string& result)
{
if (!isDirectoryPath(from))
{
ROS_ERROR_STREAM("Base path (" << from << ") must be a directory");
throw std::exception();
}
if (from == to)
{
result = ".";
return true;
}
boost::filesystem::path _from(from);
boost::filesystem::path _to(to);
boost::filesystem::path _absFrom = boost::filesystem::absolute(_from);
boost::filesystem::path _absTo = boost::filesystem::absolute(_to);
boost::filesystem::path::iterator it(_absFrom.begin());
boost::filesystem::path::iterator it_end(_absFrom.end());
boost::filesystem::path::iterator it2(_absTo.begin());
boost::filesystem::path::iterator it2_end(_absTo.end());
--it_end; // don't go to last entry, which will be either a '.' or a file
for (; it != it_end; ++it, ++it2)
{
if (it2 == it2_end) return false; // cannot be proper sub-directory
if (it->string() != it2->string())
{
// ROS_INFO_STREAM("Comp: "<<it->string()<<", "<<it2->string());
return false;
}
}
boost::filesystem::path buildPath;
for (; it2 != it2_end; ++it2)
{
buildPath /= *it2;
}
result = buildPath.string();
// if the path was a directory, remove the last '.' which was inserted
if (!result.empty() && (result[result.length() - 1] == '.'))
result.erase(result.length() - 1, 1);
// ROS_INFO_STREAM("PATH RESULT: "<<result);
boost::filesystem::path _res(result);
if (!_res.is_relative())
{
ROS_ERROR_STREAM("Could not correctly construct a relative path, got "
<< result << " (input: " << from << " and " << to << ")");
return false;
}
return true;
}
bool _check_packet(const ipv6_addr_t *src, const ipv6_addr_t *dst,
uint8_t proto, void *data, size_t data_len,
uint16_t netif)
{
gnrc_pktsnip_t *pkt, *ipv6;
ipv6_hdr_t *ipv6_hdr;
msg_t msg;
msg_receive(&msg);
if (msg.type != GNRC_NETAPI_MSG_TYPE_SND) {
return false;
}
pkt = msg.content.ptr;
if (netif != SOCK_ADDR_ANY_NETIF) {
gnrc_netif_hdr_t *netif_hdr;
if (pkt->type != GNRC_NETTYPE_NETIF) {
return _res(pkt, false);
}
netif_hdr = pkt->data;
if (netif_hdr->if_pid != netif) {
return _res(pkt, false);
}
ipv6 = pkt->next;
}
else {
ipv6 = pkt;
}
if (ipv6->type != GNRC_NETTYPE_IPV6) {
return _res(pkt, false);
}
ipv6_hdr = ipv6->data;
return _res(pkt, (memcmp(src, &ipv6_hdr->src, sizeof(ipv6_addr_t)) == 0) &&
(memcmp(dst, &ipv6_hdr->dst, sizeof(ipv6_addr_t)) == 0) &&
(ipv6_hdr->nh == proto) &&
(ipv6->next != NULL) &&
(data_len == ipv6->next->size) &&
(memcmp(data, ipv6->next->data, data_len) == 0));
}