MongoDBBridge::MongoDBBridge(const boost::property_tree::ptree &pt) : CamInterface(pt)
{
readConfig(pt);
outInfo("initialize");
storage = rs::Storage(host, db);
actualFrame = 0;
storage.getScenes(frames);
if(continual)
{
actualFrame = frames.size();
}
else
{
if(frames.empty())
{
outError("No frames in database found!");
throw_exception_message("no frames found.")
}
outInfo("found " << frames.size() << " frames in database.");
lastTimestamp = 0x7FFFFFFFFFFFFFFF;
}
void RSPipelineManager::setPipelineOrdering(std::vector<std::string> annotators)
{
// Create empty list of
// typedef std::vector < EngineEntry > TyAnnotatorEntries;
// called 'new_annotators'
//
//
// For each given annotator:
// 1) Look up the index of the desired annotator
// 2) Add a copy of the respectie EngineEntry from the original_annotators to the new list
//
uima::internal::AnnotatorManager::TyAnnotatorEntries new_annotators;
for(int i = 0; i < annotators.size(); i++)
{
// 1) Look up the index of the desired annotator
int idx = getIndexOfAnnotator(annotators.at(i));
if(idx >= 0)
{
// 2) Add a copy of the respectie EngineEntry from the original_annotators to the new list
uima::internal::AnnotatorManager::EngineEntry ee = original_annotators.at(idx);
new_annotators.push_back(ee);
continue;
}
// Right now, we just skip this annotator if it can't be found.
outInfo("Warning in RSPipelineManager::setPipelineOrdering : " << annotators.at(i) <<
" can't be found in your loaded AnalysisEngine. Maybe it has not been "
"defined in your given AnalysisEngine XML file? - Skipping it....");
// return;
}
// Pass the new pipeline to uima's annotator manager
aengine->iv_annotatorMgr.iv_vecEntries = new_annotators;
}
bool DkBatchProcess::deleteOrRestoreExisting() {
QFileInfo outInfo(mFilePathOut);
if (outInfo.exists() && !mBackupFilePath.isEmpty() && QFileInfo(mBackupFilePath).exists()) {
QFile file(mBackupFilePath);
if (!file.remove()) {
mLogStrings.append(QObject::tr("Error: could not delete existing file"));
mLogStrings.append(file.errorString());
return false;
}
}
// fall-back
else if (!outInfo.exists()) {
QFile file(mBackupFilePath);
if (!file.rename(mFilePathOut)) {
mLogStrings.append(QObject::tr("Ui - a lot of things went wrong sorry, your original file can be found here: %1").arg(mBackupFilePath));
mLogStrings.append(file.errorString());
return false;
}
else {
mLogStrings.append(QObject::tr("I could not save to %1 so I restored the original file.").arg(mFilePathOut));
}
}
return true;
}
void ROSKinectBridge::readConfig(const boost::property_tree::ptree &pt)
{
std::string depth_topic = pt.get<std::string>("camera_topics.depth");
std::string color_topic = pt.get<std::string>("camera_topics.color");
boost::optional<std::string> depth_hints = pt.get_optional<std::string>("camera_topics.depthHints");
boost::optional<std::string> color_hints = pt.get_optional<std::string>("camera_topics.colorHints");
std::string cam_info_topic = pt.get<std::string>("camera_topics.camInfo");
filterBlurredImages = pt.get<bool>("camera.filterBlurredImages");
depthOffset = pt.get<int>("camera.depthOffset");
image_transport::TransportHints hintsColor(color_hints ? color_hints.get() : "raw");
image_transport::TransportHints hintsDepth(depth_hints ? depth_hints.get() : "raw");
depthImageSubscriber = new image_transport::SubscriberFilter(it, depth_topic, 5, hintsDepth);
rgbImageSubscriber = new image_transport::SubscriberFilter(it, color_topic, 5, hintsColor);
cameraInfoSubscriber = new message_filters::Subscriber<sensor_msgs::CameraInfo>(nodeHandle, cam_info_topic, 5);
outInfo("Depth topic: " FG_BLUE << depth_topic);
outInfo("Color topic: " FG_BLUE << color_topic);
outInfo("CamInfo topic: " FG_BLUE << cam_info_topic);
if(depth_hints && color_hints)
{
outInfo("Depth Hints: " FG_BLUE << depth_hints.get());
outInfo("Color Hints: " FG_BLUE << color_hints.get());
}
outInfo("DepthOffset: " FG_BLUE << depthOffset);
outInfo("Blur filter: " FG_BLUE << (filterBlurredImages ? "ON" : "OFF"));
}
void ROSKinectBridge::lookupTransform(uima::CAS &tcas, const ros::Time ×tamp)
{
if(!lookUpViewpoint)
{
return;
}
tf::StampedTransform transform;
try
{
outInfo("TIME Before lookup:" << ros::Time::now());
listener->waitForTransform(tfTo, tfFrom, timestamp, ros::Duration(10));
listener->lookupTransform(tfTo, tfFrom, timestamp, transform);
rs::Scene scene = rs::SceneCas(tcas).getScene();
rs::StampedTransform vp(rs::conversion::to(tcas, transform));
scene.viewPoint.set(vp);
outInfo("added viewpoint to scene");
}
catch(tf::TransformException &ex)
{
outError(ex.what());
}
}
/*!
\brief Do periodical checks for weather change
\author Xanatar
*/
void check_region_weatherchange ()
{
int r,sn=0, rn=0, dr=0, sm, i;
outInfo("performing weather change...");
//! \todo revisit this part
for (i=0;i<256;i++)
{
region_st ®ionRef = region[i];
if ((regionRef.keepchance==0)&&(regionRef.drychance==0)) continue;
r = rand()%100;
if ((r<=regionRef.keepchance)||(regionRef.keepchance==100)) continue;
//we're here, let's change the weeeeather
dr = (regionRef.wtype==0) ? 0 : regionRef.drychance;
rn = (regionRef.wtype==1) ? 0 : regionRef.rainchance;
sn = (regionRef.wtype==2) ? 0 : regionRef.snowchance;
if (!regionRef.ignoreseason)
{
//! \todo actually the calendar system for weather variation during the year wasn't a bad idea, but it should NOT involve 3 float multiplications + 3 float to int and 3 int to float conversions for each region every weathercheck
dr = static_cast<int>(static_cast<float>(dr) * Calendar::g_fCurDryMod);
rn = static_cast<int>(static_cast<float>(rn) * Calendar::g_fCurRainMod);
sn = static_cast<int>(static_cast<float>(sn) * Calendar::g_fCurSnowMod);
}
sm = dr+rn+sn;
r = rand()%sm;
if (r < dr) regionRef.wtype = 0;
else if (r < (rn+dr)) regionRef.wtype = 1;
else regionRef.wtype = 2;
}
// Chronodt 17/8/04 - begun additional weathercode
//! \todo sobstitute temporary region subnames with the true ones when regions redone
//! \todo a for cycle with all the regions (cregion is the region currently esamined)
//for (whatever will cycle all region, with cregion as index or iterator)
{
Climates climate = region[cregion].climate;
if (climate == clNone) continue; //no weather change for a dungeon
WeatherType current = region[cregion].weatherCurrent;
sint16_t oldintensity = region[cregion].weatherIntensity;
sint16_t newintensity = oldintensity;
//Getting the 4 adiacent regions. getXXXXregion should return invalid if such region does not exist
uint16_t nregion = getNorthRegion(cregion);
uint16_t eregion = getEastRegion(cregion);
uint16_t sregion = getSouthRegion(cregion);
uint16_t wregion = getWestRegion(cregion);
// modifying the intensity by surrounding squares (obiously the old status)
newintensity += getIntensityModifier(cregion, nregion);
newintensity += getIntensityModifier(cregion, eregion);
newintensity += getIntensityModifier(cregion, sregion);
newintensity += getIntensityModifier(cregion, wregion);
// adding intensity by random raincheck (based on region configuration)
//! \todo modify these with new region parameters
uint8_t dry = region[cregion].drychance;
uint8_t rain = region[cregion].rainchance;
uint8_t snow = region[cregion].snowchance;
uint8_t r = rand()%100;
if (r < dry) newintensity -= (dry - r) * 5;
r = rand()%100;
if (r < rain) newintensity += (rain - r) * 5;
if (newintensity > 140) newintensity = 140;
if (newintensity < 0) newintensity = 0;
bool snow = false;
string message = "";
//! \todo insert a maxlight to limit light in bad beather (but nightvision should counter this)
switch (current) //select message type to send clients based on previous weather in region. Also checking if weather change is too sudden, and if so lessen the impact :D
{
case wtSun: // intensity 0 - 20
if (newintensity > 40)
{
newintensity = 40; //If it was so clear, we need at least a few clouds in the sky before it can rain (or snow)
message = "You see some clouds closing in";
}
break;
case wtCloud: // intensity 21 - 40
if (newintensity > 90) // to have more than a light rain we need heavier clouds first
{
newintensity = 70;
message = "Cloud covering thickens visibly. You hear rumbling in the distance";
}
else if (newintensity <= 20) message = "The sky clears off and the sun shines again";
else if (newintensity > 70)
{ //snow check
r = rand()%100;
if (r < snow) snow = true;
}
break;
case wtStormCloud: // intensity 41 - 70
//from this weather type, we can reach every other, since from here it can start raining with any intensity. Or it can snow or even clear up
if (newintensity <= 40 && newintensity > 20) message = "The sky begins to clear off";
else if (newintensity <= 20) message = "Strong winds clear away the thick clouds, until the sun shines again";
else if (newintensity > 70)
{ //snow check
r = rand()%100;
if (r < snow) snow = true;
}
break;
case wtLightRain: // intensity 71 - 90
if (newintensity < 71)
{
if (newintensity > 20) message = "The rain is stopping";
else message = "The rain is stopping and the sky clears up";
}
else message = "The rain gets stronger";
r = rand()%100;
if (climate == clArtic && r < (snow/4)) snow = true; //If artic climate and raining, check if rain becomes snow (at 1/4 of normal snow chance)
break;
case wtMediumRain: // intensity 91 - 110
if (newintensity <71)
{
if (newintensity > 40) message = "The rain is stopping";
else
{
newintensity = 41;
message = "The rain is stopping and the sky begins to clear up";
}
}
else message = "The rain gets stronger";
r = rand()%100;
if (climate == clArtic && r < (snow/4)) snow = true; //If artic climate and raining, check if rain becomes snow (at 1/4 of normal snow chance)
break;
case wtHeavyRain: // intensity 111 - 140
if (newintensity <111)
{
if (newintensity > 70) message = "The rain begins to slow down";
else
{
newintensity = 71;
message = "The rain has stopped falling, but the sky is still covered up";
}
}
else message = "The rain gets stronger";
r = rand()%100;
if (climate == clArtic && r < (snow/4)) snow = true; //If artic climate and raining, check if rain becomes snow (at 1/4 of normal snow chance)
break;
case wtLightSnow: // intensity 71 - 90
if (newintensity < 71)
{
if (newintensity > 20) message = "The snow is stopping";
else message = "The snow is stopping and the sky clears up";
}
else message = "The snow gets stronger";
r = rand()%100;
if (climate != clNormal || r > ((100 - snow)/4)) snow = true; //If normal climate and snowing, check if snow becomes rain (at 1/4 of normal snow_to_rain chance, the complementary of snow chance)
break;
case wtMediumSnow: // intensity 91 - 110
if (newintensity <71)
{
if (newintensity > 40) message = "The snow is stopping";
else
{
newintensity = 41;
message = "The snow is stopping and the sky begins to clear up";
}
}
else message = "The snow gets stronger";
r = rand()%100;
if (climate != clNormal || r > ((100 - snow)/4)) snow = true; //If normal climate and snowing, check if snow becomes rain (at 1/4 of normal snow_to_rain chance, the complementary of snow chance)
break;
case wtHeavySnow: // intensity 111 - 140
snow = true;
if (newintensity <111)
{
if (newintensity > 70) message = "The snow begins to slow down";
else
{
newintensity = 71;
message = "The snow has stopped falling, but the sky is still covered up";
}
}
else message = "The snow gets stronger";
r = rand()%100;
if (climate != clNormal || r > ((100 - snow)/4)) snow = true; //If normal climate and snowing, check if snow becomes rain (at 1/4 of normal snow_to_rain chance, the complementary of snow chance)
break;
}
if (newintensity <= 20) region[cregion].weatherNew = wtSun;
else if (newintensity <= 40) region[cregion].weatherNew = wtCloud;
else if (newintensity <= 70) region[cregion].weatherNew = wtStormCloud;
else if (newintensity <= 90 && !snow) region[cregion].weatherNew = wtLightRain;
else if (newintensity <= 110 && !snow) region[cregion].weatherNew = wtMediumRain;
else if (!snow) region[cregion].weatherNew = wtHeavyRain;
else if (newintensity <= 90 && snow) region[cregion].weatherNew = wtLightSnow;
else if (newintensity <= 110 && snow) region[cregion].weatherNew = wtMediumSnow;
else region[cregion].weatherNew = wtHeavySnow;
region[cregion].weatherIntensityNew = newintensity;
uint8_t weather = 0;
is (snow) weather = 0x02; //snow effect
if (newintensity <=70) weather = 0xff; //No weather
uint8_t intensity = (newintensity > 70) ? newintensity - 70 : newintensity;
uint8_t intensity2 = intensity;
bool mixedweather = false;
if ( ((current == wtLightRain || current == wtMediumRain || current == wtHeavyRain) && snow) || // Rain has just become snow or...
((current == wtLightSnow || current == wtMediumSnow || current == wtHeavySnow) && !snow)) // Snow has just become rain
{
intensity = rand() % intensity; // randomize % of snow ...
intensity2 -= intensity; // ...and the remaining intensity goes to rain
weather = 0x2; // snow
weather2 = 0x0; // rain
mixedweather = true;
}
//! \todo the limit of light level by weather (it is best to modify existing code in timers.cpp)
nPackets::Sent::Weather pk( weather, intensity);
nPackets::Sent::Weather pk2( weather2, intensity2);
NxwSocketWrapper sw;
sw.fillOnline( pos ); //!< \todo change with something like fillOnlineInRegion
for( sw.rewind(); !sw.isEmpty(); sw++ )
{
pClient j =sw.getSocket();
if( j )
{
j->sendPacket(&pk);
if (mixedweather) j->sendPacket(&pk2);
j->sysmessage(message);
}
}
}
// End region for cycle. Now we must build another one to update the weatherIntensity and weatherCurrent of all regions
// if you ask why, think of what would happen if you changed them as you process them, since each region takes a little info
// from each adiacent region.... :) (yes, this is a system with a memory :D)
//! \todo an online iteration of all online chars in region
//for (whatever will cycle all regions)
{
region[cregion].weatherCurrent = region[cregion].weatherNew;
region[cregion].weatherIntensity = region[cregion].weatherIntensityNew;
}
outPlain("[ OK ]\n");
}
/**
* extract handles from a pointcloud
* @param[pointCloudIn] input point cloud
* @param[pointCloudNormals] normals for the input cloud
* @param[handle_indices] vector of indices, each item being a set of indices representing a handle
* @param[handle_coefficients] vector of cofficients, each item being the coefficients of the line representing the handle
*/
void HandleExtractor::extractHandles(PointCloud::Ptr &cloudInput, PointCloudNormal::Ptr &pointCloudNormals,
std::vector< pcl::PointIndices> &handle_indices, std::vector< pcl::ModelCoefficients> &handle_coefficients
)
{
// setup handle cluster object
pcl::EuclideanClusterExtraction<Point> handle_cluster_;
KdTreePtr clusters_tree_(new KdTree);
handle_cluster_.setClusterTolerance(handle_cluster_tolerance);
handle_cluster_.setMinClusterSize(min_handle_cluster_size);
handle_cluster_.setSearchMethod(clusters_tree_);
handle_cluster_.setInputCloud(cloudInput);
pcl::PointCloud<Point>::Ptr cloud_filtered(new pcl::PointCloud<Point>());
pcl::VoxelGrid<Point> vg;
vg.setInputCloud(cloudInput);
vg.setLeafSize(0.01, 0.01, 0.01);
vg.filter(*cloud_filtered);
// setup line ransac object
pcl::SACSegmentation<Point> seg_line_;
seg_line_.setModelType(pcl::SACMODEL_LINE);
seg_line_.setMethodType(pcl::SAC_RANSAC);
seg_line_.setInputCloud(cloudInput);
seg_line_.setDistanceThreshold(line_ransac_distance);
seg_line_.setOptimizeCoefficients(true);
seg_line_.setMaxIterations(line_ransac_max_iter);
// setup Inlier projection object
pcl::ProjectInliers<Point> proj_;
proj_.setInputCloud(cloud_filtered);
proj_.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
// setup polygonal prism
pcl::ExtractPolygonalPrismData<Point> prism_;
prism_.setHeightLimits(min_handle_height, max_handle_height);
prism_.setInputCloud(cloudInput);
//setup Plane Segmentation
outInfo("Segmenting planes");
pcl::SACSegmentation<Point> seg_plane_;
seg_plane_.setOptimizeCoefficients(true);
seg_plane_.setModelType(pcl::SACMODEL_PLANE);
seg_plane_.setMethodType(pcl::SAC_RANSAC);
seg_plane_.setDistanceThreshold(0.03);
seg_plane_.setMaxIterations(500);
seg_plane_.setInputCloud(cloud_filtered);
pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients());
pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices());
seg_plane_.segment(*plane_inliers, *plane_coefficients);
if(plane_inliers->indices.size() != 0)
{
outDebug("Plane model: "
<< plane_coefficients->values[0] << ","
<< plane_coefficients->values[1] << ","
<< plane_coefficients->values[2] << ","
<< plane_coefficients->values[3] << " with "
<< (int)plane_inliers->indices.size() << "inliers ");
//Project inliers of the planes into a perfect plane
PointCloud::Ptr cloud_projected(new PointCloud());
proj_.setIndices(plane_inliers);
proj_.setModelCoefficients(plane_coefficients);
proj_.filter(*cloud_projected);
// Create a Convex Hull representation using the projected inliers
PointCloud::Ptr cloud_hull(new PointCloud());
pcl::ConvexHull<Point> chull_;
chull_.setDimension(2);
chull_.setInputCloud(cloud_projected);
chull_.reconstruct(*cloud_hull);
outInfo("Convex hull has: " << (int)cloud_hull->points.size() << " data points.");
if((int) cloud_hull->points.size() == 0)
{
outInfo("Convex hull has: no data points. Returning.");
return;
}
// Extract the handle clusters using a polygonal prism
pcl::PointIndices::Ptr handlesIndicesPtr(new pcl::PointIndices());
prism_.setInputPlanarHull(cloud_hull);
prism_.segment(*handlesIndicesPtr);
// cluster the points found in the prism
std::vector< pcl::PointIndices> handle_clusters;
handle_cluster_.setIndices(handlesIndicesPtr);
handle_cluster_.extract(handle_clusters);
for(size_t j = 0; j < handle_clusters.size(); j++)
{
// for each cluster in the prism, attempt to fit a line using ransac
pcl::PointIndices single_handle_indices;
pcl::ModelCoefficients handle_line_coefficients;
seg_line_.setIndices(getIndicesPointerFromObject(handle_clusters[j]));
seg_line_.segment(single_handle_indices, handle_line_coefficients);
if(single_handle_indices.indices.size() > 0)
{
outInfo("Found a handle with " << single_handle_indices.indices.size() << " inliers.");
outDebug("Handle Line coefficients: " << handle_line_coefficients);
handle_indices.push_back(single_handle_indices);
handle_coefficients.push_back(handle_line_coefficients);
}
}
}
else
{
outInfo("no planes found");
return;
}
}
