u32 PeersManager::clearPasvConnection(unsigned int pasvid)
{
std::map<u32, ApplicationData>::iterator it = m_applications.begin();
while(it != m_applications.end())
{
if(it->second.m_passiveConnection == pasvid)
{
it->second.m_passiveConnection = TransferServer::INVALID_CONNECTION_ID;
return it->first;
}
++it;
}
// anonymous
cacti::RecursiveMutex::ScopedLock sclock(m_applock);
for(size_t i=0; i<m_anonymousApps.size(); ++i)
{
if(m_anonymousApps[i].m_passiveConnection == pasvid)
{
m_anonymousApps[i].m_passiveConnection = TransferServer::INVALID_CONNECTION_ID;
return m_anonymousApps[i].m_appid;
}
}
return 0;
}
bool PeersManager::isPeerDead(u32 appid)
{
std::map<u32, ApplicationData>::iterator it = m_applications.find(appid);
if(it != m_applications.end())
{
ApplicationData& ad = it->second;
if(ad.m_passiveConnection != ActiveServer::INVALID_CONNECTION_ID)
return false;
if(!ad.m_tcpclient)
return true;
return !ad.m_tcpclient->connected();
}
else
{
// anonymous
cacti::RecursiveMutex::ScopedLock sclock(m_applock);
for(size_t i=0; i<m_anonymousApps.size(); ++i)
{
if(m_anonymousApps[i].m_appid == appid)
{
if(m_anonymousApps[i].m_passiveConnection != ActiveServer::INVALID_CONNECTION_ID)
return false;
// anonymous no tcp client
}
}
}
return false;
}
std::vector<ServiceIdentifier> PeersManager::getAllActiveServices()
{
std::vector<ServiceIdentifier> ret;
std::map<u32, ApplicationData>::iterator it = m_applications.begin();
while(it != m_applications.end())
{
ServiceIdentifier sid;
sid.m_appid = it->first;
ApplicationData& ad = it->second;
for(size_t i=0; i<ad.m_activeServices.size(); ++i)
{
sid.m_appport = ad.m_activeServices[i];
if(sid.m_appport != AppPort::_apMessageD)
ret.push_back(sid);
}
++it;
}
// anonymous
cacti::RecursiveMutex::ScopedLock sclock(m_applock);
for(size_t i=0; i<m_anonymousApps.size(); ++i)
{
ServiceIdentifier sid;
ApplicationData& ad = m_anonymousApps[i];
sid.m_appid = ad.m_appid;
for(size_t i=0; i<ad.m_activeServices.size(); ++i)
{
sid.m_appport = ad.m_activeServices[i];
if(sid.m_appport != AppPort::_apMessageD)
ret.push_back(sid);
}
}
return ret;
}
static void test_sync_wait(const char *mboxname)
{
char *filename;
struct stat sb;
clock_t start;
int status = 0;
int r;
#define TIMEOUT (30 * CLOCKS_PER_SEC)
if (!test_sync_mode)
return;
/* aha, we're in test synchronisation mode */
syslog(LOG_ERR, "quota -Z waiting for signal to do %s", mboxname);
filename = strconcat(config_dir, "/quota-sync/", mboxname, (char *)NULL);
start = sclock();
while ((r = stat(filename, &sb)) < 0 && errno == ENOENT) {
if (sclock() - start > TIMEOUT) {
status = 2;
break;
}
status = 1;
poll(NULL, 0, 20); /* try again in 20 millisec */
}
switch (status)
{
case 0:
syslog(LOG_ERR, "quota -Z did not wait");
break;
case 1:
syslog(LOG_ERR, "quota -Z waited %2.3f sec",
(sclock() - start) / (double) CLOCKS_PER_SEC);
break;
case 2:
syslog(LOG_ERR, "quota -Z timed out");
break;
}
free(filename);
#undef TIMEOUT
}
void PeersManager::dumpConnectionMap()
{
// 这里上锁仅仅是为了锁住对ApplicationData的访问
cacti::RecursiveMutex::ScopedLock sclock(m_applock);
m_connMap.info("\n%s:\n",
Timestamp::getNowTime().getFormattedTime("%m%d %H:%M:%S").c_str());
std::map<u32, ApplicationData>::iterator it = m_applications.begin();
while(it != m_applications.end())
{
ApplicationData& ad = it->second;
m_connMap.info("[%d, %s] \n",
ad.m_appid, ad.m_appName.c_str());
m_connMap.info("\t addr=%s|%s:%d\n",
ad.m_primAddress.c_str(), ad.m_backAddress.c_str(), ad.m_connectPort);
m_connMap.info("\t pasv=%08X\n", ad.m_passiveConnection);
m_connMap.info("\t svrs=");
u32 starttime = 0;
for(size_t i=0; i<ad.m_activeServices.size(); ++i)
{
m_connMap.info("%d ", ad.m_activeServices[i]);
starttime = ad.m_starttimes[ad.m_activeServices[i]];
}
m_connMap.info("\n\t boot timestamp:%s\n", Timestamp((time_t)starttime).getFormattedTime("%Y%m%d %H:%M:%S").c_str());
++it;
}
// check anonymous
for(size_t i=0; i<m_anonymousApps.size(); ++i)
{
ApplicationData& ad = m_anonymousApps[i];
m_connMap.info("[%d, %s] \n",
ad.m_appid, ad.m_appName.c_str());
m_connMap.info("\t addr=%s|%s:%d\n",
ad.m_primAddress.c_str(), ad.m_backAddress.c_str(), ad.m_connectPort);
m_connMap.info("\t pasv=%08X\n", ad.m_passiveConnection);
m_connMap.info("\t svrs=");
u32 starttime = 0;
for(size_t i=0; i<ad.m_activeServices.size(); ++i)
{
m_connMap.info("%d ", ad.m_activeServices[i]);
starttime = ad.m_starttimes[ad.m_activeServices[i]];
}
m_connMap.info("\n\t boot timestamp:%s\n", Timestamp((time_t)starttime).getFormattedTime("%Y%m%d %H:%M:%S").c_str());
}
}
std::vector<u32> PeersManager::getAllPeerApplication()
{
std::vector<u32> ret;
std::map<u32, ApplicationData>::iterator it = m_applications.begin();
while(it != m_applications.end())
{
ret.push_back(it->first);
++it;
}
// anonymous
cacti::RecursiveMutex::ScopedLock sclock(m_applock);
for(size_t i=0; i<m_anonymousApps.size(); ++i)
{
ret.push_back(m_anonymousApps[i].m_appid);
}
return ret;
}
ApplicationData PeersManager::getApplicationInfo(u32 appid)
{
std::map<u32, ApplicationData>::iterator it = m_applications.find(appid);
if(it != m_applications.end())
{
return it->second;
}
// anonymous
cacti::RecursiveMutex::ScopedLock sclock(m_applock);
for(size_t i=0; i<m_anonymousApps.size(); ++i)
{
if(m_anonymousApps[i].m_appid == appid)
{
return m_anonymousApps[i];
}
}
return ApplicationData();
}
unsigned int PeersManager::getPasvConnection(const ServiceIdentifier& sid)
{
std::map<u32, ApplicationData>::iterator it = m_applications.find(sid.m_appid);
if(it != m_applications.end())
{
return it->second.m_passiveConnection;
}
// anonymous
cacti::RecursiveMutex::ScopedLock sclock(m_applock);
for(size_t i=0; i<m_anonymousApps.size(); ++i)
{
if(m_anonymousApps[i].m_appid == sid.m_appid)
{
return m_anonymousApps[i].m_passiveConnection;
}
}
return 0;
}
bool PeersManager::isServiceActive(u32 app, u32 port)
{
std::map<u32, ApplicationData>::iterator it = m_applications.find(app);
if(it != m_applications.end())
{
ApplicationData& ad = it->second;
return ad.isActiveService(port);
}
else
{
// check anonymous
cacti::RecursiveMutex::ScopedLock sclock(m_applock);
int idx = findanonymousApps(app);
if(idx == -1)
return false;
return m_anonymousApps[idx].isActiveService(port);
}
}
////////////////////////////////////////////////////////////////////////////////
// Put laser data to the interface
void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime)
{
int i, hja, hjb;
int j, vja, vjb;
double vb, hb;
int j1, j2, j3, j4; // four corners indices
double r1, r2, r3, r4, r; // four corner values + interpolated range
double intensity;
this->parent_ray_sensor_->SetActive(false);
#if GAZEBO_MAJOR_VERSION >= 6
auto maxAngle = this->parent_ray_sensor_->AngleMax();
auto minAngle = this->parent_ray_sensor_->AngleMin();
#else
math::Angle maxAngle = this->parent_ray_sensor_->GetAngleMax();
math::Angle minAngle = this->parent_ray_sensor_->GetAngleMin();
#endif
double maxRange = this->parent_ray_sensor_->GetRangeMax();
double minRange = this->parent_ray_sensor_->GetRangeMin();
int rayCount = this->parent_ray_sensor_->GetRayCount();
int rangeCount = this->parent_ray_sensor_->GetRangeCount();
int verticalRayCount = this->parent_ray_sensor_->GetVerticalRayCount();
int verticalRangeCount = this->parent_ray_sensor_->GetVerticalRangeCount();
#if GAZEBO_MAJOR_VERSION >= 6
auto verticalMaxAngle = this->parent_ray_sensor_->VerticalAngleMax();
auto verticalMinAngle = this->parent_ray_sensor_->VerticalAngleMin();
#else
math::Angle verticalMaxAngle = this->parent_ray_sensor_->GetVerticalAngleMax();
math::Angle verticalMinAngle = this->parent_ray_sensor_->GetVerticalAngleMin();
#endif
double yDiff = maxAngle.Radian() - minAngle.Radian();
double pDiff = verticalMaxAngle.Radian() - verticalMinAngle.Radian();
// set size of cloud message everytime!
//int r_size = rangeCount * verticalRangeCount;
this->cloud_msg_.points.clear();
this->cloud_msg_.channels.clear();
this->cloud_msg_.channels.push_back(sensor_msgs::ChannelFloat32());
/***************************************************************/
/* */
/* point scan from laser */
/* */
/***************************************************************/
boost::mutex::scoped_lock sclock(this->lock);
// Add Frame Name
this->cloud_msg_.header.frame_id = this->frame_name_;
this->cloud_msg_.header.stamp.sec = _updateTime.sec;
this->cloud_msg_.header.stamp.nsec = _updateTime.nsec;
for (j = 0; j<verticalRangeCount; j++)
{
// interpolating in vertical direction
vb = (verticalRangeCount == 1) ? 0 : (double) j * (verticalRayCount - 1) / (verticalRangeCount - 1);
vja = (int) floor(vb);
vjb = std::min(vja + 1, verticalRayCount - 1);
vb = vb - floor(vb); // fraction from min
assert(vja >= 0 && vja < verticalRayCount);
assert(vjb >= 0 && vjb < verticalRayCount);
for (i = 0; i<rangeCount; i++)
{
// Interpolate the range readings from the rays in horizontal direction
hb = (rangeCount == 1)? 0 : (double) i * (rayCount - 1) / (rangeCount - 1);
hja = (int) floor(hb);
hjb = std::min(hja + 1, rayCount - 1);
hb = hb - floor(hb); // fraction from min
assert(hja >= 0 && hja < rayCount);
assert(hjb >= 0 && hjb < rayCount);
// indices of 4 corners
j1 = hja + vja * rayCount;
j2 = hjb + vja * rayCount;
j3 = hja + vjb * rayCount;
j4 = hjb + vjb * rayCount;
// range readings of 4 corners
r1 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j1) , maxRange-minRange);
r2 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j2) , maxRange-minRange);
r3 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j3) , maxRange-minRange);
r4 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j4) , maxRange-minRange);
// Range is linear interpolation if values are close,
// and min if they are very different
r = (1-vb)*((1 - hb) * r1 + hb * r2)
+ vb *((1 - hb) * r3 + hb * r4);
// Intensity is averaged
intensity = 0.25*(this->parent_ray_sensor_->GetLaserShape()->GetRetro(j1) +
this->parent_ray_sensor_->GetLaserShape()->GetRetro(j2) +
this->parent_ray_sensor_->GetLaserShape()->GetRetro(j3) +
this->parent_ray_sensor_->GetLaserShape()->GetRetro(j4));
// std::cout << " block debug "
// << " ij("<<i<<","<<j<<")"
// << " j1234("<<j1<<","<<j2<<","<<j3<<","<<j4<<")"
// << " r1234("<<r1<<","<<r2<<","<<r3<<","<<r4<<")"
// << std::endl;
// get angles of ray to get xyz for point
double yAngle = 0.5*(hja+hjb) * yDiff / (rayCount -1) + minAngle.Radian();
double pAngle = 0.5*(vja+vjb) * pDiff / (verticalRayCount -1) + verticalMinAngle.Radian();
/***************************************************************/
/* */
/* point scan from laser */
/* */
/***************************************************************/
//compare 2 doubles
double diffRange = maxRange - minRange;
double diff = diffRange - r;
if (fabs(diff) < EPSILON_DIFF)
{
// no noise if at max range
geometry_msgs::Point32 point;
//pAngle is rotated by yAngle:
point.x = r * cos(pAngle) * cos(yAngle);
point.y = r * cos(pAngle) * sin(yAngle);
point.z = -r * sin(pAngle);
this->cloud_msg_.points.push_back(point);
}
else
{
geometry_msgs::Point32 point;
//pAngle is rotated by yAngle:
point.x = r * cos(pAngle) * cos(yAngle) + this->GaussianKernel(0,this->gaussian_noise_);
point.y = r * cos(pAngle) * sin(yAngle) + this->GaussianKernel(0,this->gaussian_noise_);
point.z = -r * sin(pAngle) + this->GaussianKernel(0,this->gaussian_noise_);
this->cloud_msg_.points.push_back(point);
} // only 1 channel
this->cloud_msg_.channels[0].values.push_back(intensity + this->GaussianKernel(0,this->gaussian_noise_)) ;
}
}
this->parent_ray_sensor_->SetActive(true);
// send data out via ros message
this->pub_.publish(this->cloud_msg_);
}
// return 0, no this application
// return 1, active
// return 2, reactive
// return 3, duplicate active
u32 PeersManager::onServiceActive(u32 app, u32 port, u32 starttime)
{
// 这里上锁仅仅是为了锁住对ApplicationData的访问
cacti::RecursiveMutex::ScopedLock sclock(m_applock);
std::map<u32, ApplicationData>::iterator it = m_applications.find(app);
if(it != m_applications.end())
{
ApplicationData& ad = it->second;
// 如果发现有个服务的_apMessageD没有登记在active列表的话，补上
if(!ad.isActiveService((u32)AppPort::_apMessageD))
ad.m_activeServices.push_back((u32)AppPort::_apMessageD);
if(!ad.isActiveService(port))
{
// first time active
if(starttime != 0)
ad.m_starttimes[port] = starttime;
ad.m_activeServices.push_back(port);
dumpConnectionMap();
return 1;
}
else
{
// already active
if(starttime != 0 && ad.m_starttimes[port] != starttime)
{
ad.m_starttimes[port] = starttime;
dumpConnectionMap();
return 2;
}
else
{
return 3;
}
}
}
else
{
// check anonymous
cacti::RecursiveMutex::ScopedLock sclock(m_applock);
int idx = findanonymousApps(app);
if(idx == -1)
{
// create a new application data for anonymous
ApplicationData ad;
ad.m_appid = app;
ad.m_appName = StringUtil::format("_%s_%d", "_anonymous_", app);
ad.m_activeServices.push_back(AppPort::_apMessageD);
ad.m_activeServices.push_back(port);
if(starttime != 0)
ad.m_starttimes[port] = starttime;
m_anonymousApps.push_back(ad);
dumpConnectionMap();
return 0;
}
else
{
ApplicationData& ad = m_anonymousApps[idx];
if(!ad.isActiveService((u32)AppPort::_apMessageD))
ad.m_activeServices.push_back((u32)AppPort::_apMessageD);
if(!ad.isActiveService(port))
{
// first time active
if(starttime != 0)
ad.m_starttimes[port] = starttime;
ad.m_activeServices.push_back(port);
dumpConnectionMap();
return 1;
}
else
{
// already active
if(starttime != 0 && ad.m_starttimes[port] != starttime)
{
ad.m_starttimes[port] = starttime;
dumpConnectionMap();
return 2;
}
else
{
return 3;
}
}
}
}
}
