void AllJoynConnection::Connect(char* tag, bool asAdvertiser)
{
status = ER_OK;
if (asAdvertiser) {
advertisedName = NAME_PREFIX;
advertisedName += "xfer";
joinName = "";
NotifyUser(MSG_STATUS, "%s is advertiser \n", advertisedName.c_str());
} else {
joinName = NAME_PREFIX;
joinName += "xfer";
advertisedName = "";
NotifyUser(MSG_STATUS, "%s is joiner\n", joinName.c_str());
}
assert(invariants());
myTag = tag;
createMessageBus();
startMessageBus();
SessionOpts opts(SessionOpts::TRAFFIC_MESSAGES, true, SessionOpts::PROXIMITY_ANY, TRANSPORT_ANY);
bindSessionPort(opts);
if (!this->advertisedName.empty()) {
NotifyUser(MSG_STATUS, "Request name");
status = this->busAttachment->RequestName(this->advertisedName.c_str(), DBUS_NAME_FLAG_DO_NOT_QUEUE);
if (ER_OK != status) {
NotifyUser(MSG_ERROR, "RequestName(%s) failed (status=%s)\n", this->advertisedName.c_str(), QCC_StatusText(status));
status = (status == ER_OK) ? ER_FAIL : status;
}
/* Advertise same well-known name */
if (ER_OK == status) {
status = this->busAttachment->AdvertiseName(this->advertisedName.c_str(), opts.transports);
if (status != ER_OK) {
NotifyUser(MSG_ERROR, "Failed to advertise name %s (%s)\n", this->advertisedName.c_str(), QCC_StatusText(status));
}
}
} else {
status = this->busAttachment->FindAdvertisedName(this->joinName.c_str());
if (status != ER_OK) {
NotifyUser(MSG_ERROR, "org.alljoyn.Bus.FindAdvertisedName failed (%s)\n", QCC_StatusText(status));
}
NotifyUser(MSG_STATUS, "Found Advertised Name \n");
}
createBusObjects(tag);
m_fConnected = (ER_OK == status);
NotifyUser(MSG_STATUS, "Ready %s ...", tag);
}
XalanDOMString::XalanDOMString(
size_type theCount,
XalanDOMChar theChar,
MemoryManager& theManager) :
m_data(theManager),
m_size(0)
{
if (theCount != 0)
{
XalanDOMCharVectorType(theCount + 1, theChar, theManager).swap(m_data);
// Null-terminate it...
m_data.back() = 0;
m_size = theCount;
}
invariants();
}
// Fills the current tlab with a dummy filler array to create
// an illusion of a contiguous Eden and optionally retires the tlab.
// Waste accounting should be done in caller as appropriate; see,
// for example, clear_before_allocation().
void ThreadLocalAllocBuffer::make_parsable(bool retire) {
if (end() != NULL) {
invariants();
if (retire) {
myThread()->incr_allocated_bytes(used_bytes());
}
CollectedHeap::fill_with_object(top(), hard_end(), retire);
if (retire || ZeroTLAB) { // "Reset" the TLAB
set_start(NULL);
set_top(NULL);
set_pf_top(NULL);
set_end(NULL);
}
}
assert(!(retire || ZeroTLAB) ||
(start() == NULL && end() == NULL && top() == NULL),
"TLAB must be reset");
}
void localizationCallback(const sensor_msgs::PointCloud2ConstPtr& cloud, localizationStruct* callbackStruct){
// reading the point cloud data from the kinect sensor
sensor_msgs::PointCloud2 _cloud = *cloud;
//changing the global variable when the callback function is called
is_callback_called=true;
// create variables for invariant calculation
int satLower = 30;
int satUpper = 230;
int valLower = 30;
int valUpper = 230;
int focalLengthPixels = 525;
int noHarmonics=10;
double maxRangeMeters = 6;
bool normalize_invariants=callbackStruct->normalize_invariants;
bool exclude_depth= callbackStruct->exclude_depth;
double bubble_update_period = callbackStruct->bubble_update_period;
cv::Mat filters[5]= callbackStruct->filters;
double angular_velocity=callbackStruct->angular_velocity;
cv::Mat invariants(1,noHarmonics*noHarmonics,CV_32FC1);
//Create variables for processing time calculation
struct timespec t1, t2;
double elapsed_time;
clock_gettime(CLOCK_MONOTONIC, &t1);
//Create vector for bubbles
std::vector<bubblePointXYZ> bubble;
//Create vector for cloud fields
std::vector<sensor_msgs::PointField> fields = _cloud.fields;
//Create empty invariant vector for current data
//Create temporary vector for normalized single feature vectors
cv::Mat normalizedDummyVector;
if(fields.at(3).name == "rgba")
{
ROS_INFO("Processing Pointcloud - name=rgba");
pcl::PointCloud<pcl::PointXYZRGBA> normalCloud;
pcl::fromROSMsg(_cloud,normalCloud);
}
else
{
// saving the point cloud data
pcl::PointCloud<pcl::PointXYZRGB> normalCloud;
pcl::fromROSMsg(_cloud,normalCloud);
QString fileName = "/home/turtlebot2/Desktop/cloud_";
fileName.append(QString::number(cloud_name));
fileName.append(".pcd");
pcl::io::savePCDFileBinary(fileName.toStdString(),normalCloud);
cloud_name++;
//Create BGR image matrix
cv::Mat bgr_image;
//Extract BGR data from pointcloud into BGR image matrix
if (normalCloud.isOrganized()) {
bgr_image = cv::Mat(normalCloud.height, normalCloud.width, CV_8UC3);
if (!normalCloud.empty()){
unsigned int index;
for (int h=0; h<bgr_image.rows; h++) {
index = h*bgr_image.cols;
for (int w=0; w<bgr_image.cols; w++) {
/*pcl::PointXYZRGB point = normalCloudRGB.at(w, h);
Eigen::Vector3i rgb = point.getRGBVector3i();*/
bgr_image.at<cv::Vec3b>(h,w)[2] = normalCloud.points.at(index).r;
bgr_image.at<cv::Vec3b>(h,w)[1] = normalCloud.points.at(index).g;
bgr_image.at<cv::Vec3b>(h,w)[0] = normalCloud.points.at(index).b;
index++;
}
}
}
}
std::cout << "image is constructed" << std::endl;
// for all filters, calculate image invariants, normalize them if normalization is true and append them
cv::Mat resg;
for(unsigned int i=0; i<5; i++){
//Convert bgr image to gray image
cv::cvtColor(bgr_image,resg,CV_BGR2GRAY);
//Apply the filter
cv::Mat sonuc = ImageProcess::mstApplyFilter(resg, filters[i]);
//Calculate bubbles and then the invariant vector for this filter
vector<bubblePoint> imgBubble = bubbleProcess::convertGrayImage2Bub(sonuc,focalLengthPixels,255);
vector<bubblePoint> resred;
resred = bubbleProcess::reduceBubble(imgBubble);
DFCoefficients dfcoeff = bubbleProcess::calculateDFCoefficients(resred,noHarmonics,noHarmonics);
//Normalize the invariant and append to the overall invariant vector
if(i==0){
if(normalize_invariants == true){
cv::normalize(bubbleProcess::mstCalculateInvariants(resred, dfcoeff,noHarmonics, noHarmonics), invariants);
}
else{
invariants = bubbleProcess::mstCalculateInvariants(resred, dfcoeff,noHarmonics, noHarmonics);
}
}
else{
if(normalize_invariants == true){
cv::normalize(bubbleProcess::mstCalculateInvariants(resred,dfcoeff,noHarmonics,noHarmonics), normalizedDummyVector);
cv::hconcat(invariants, normalizedDummyVector, invariants);
}
else{
cv::hconcat(invariants, bubbleProcess::mstCalculateInvariants(resred,dfcoeff,noHarmonics,noHarmonics), invariants);
}
}
}
std::cout << "filters are applied" << std::endl;
//For the hue image, calculate bubbles, the invariant vector, normalize it and append to the overall vector
cv::Mat hueChannel= ImageProcess::generateChannelImage(bgr_image,0,satLower,satUpper,valLower,valUpper);
vector<bubblePoint> hueBubble = bubbleProcess::convertGrayImage2Bub(hueChannel,focalLengthPixels,180);
vector<bubblePoint> reducedHueBubble = bubbleProcess::reduceBubble(hueBubble);
DFCoefficients dfcoeffRGB = bubbleProcess::calculateDFCoefficients(reducedHueBubble,noHarmonics,noHarmonics);
if(normalize_invariants == true){
cv::normalize(bubbleProcess::mstCalculateInvariants(reducedHueBubble,dfcoeffRGB,noHarmonics,noHarmonics), normalizedDummyVector);
cv::hconcat(invariants, normalizedDummyVector, invariants);
}
else{
cv::hconcat(invariants, bubbleProcess::mstCalculateInvariants(reducedHueBubble,dfcoeffRGB,noHarmonics,noHarmonics), invariants);
}
std::cout << "hue invariants are constructed" << std::endl;
//rotation around X and Y to get same coordinates with RGB frame and database images
int rotX=-90;
int rotY=90;
int rotZ=0;
Eigen::Matrix4f yy;
double xRad = rotX*M_PI/180;
double yRad = rotY*M_PI/180;
double zRad = rotZ*M_PI/180;
yy<<cos(yRad), 0, sin(yRad), 0,
0, 1, 0, 0,
-sin(yRad), 0, cos(yRad), 0,
0, 0, 0, 1;
Eigen::Matrix4f xx;
xx<<1, 0, 0, 0,
0, cos(xRad), -sin(xRad), 0,
0, sin(xRad), cos(xRad), 0,
0, 0, 0, 1;
Eigen::Matrix4f zz;
zz<<cos(zRad),-sin(zRad), 0, 0,
sin(zRad), cos(zRad), 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
//rotation
pcl::transformPointCloud(normalCloud,normalCloud,xx*yy*zz);
//Create the bubble points out of the depth information
for(unsigned int i = 0; i < normalCloud.points.size(); i++)
{
bubblePointXYZ pt;
pt.x = normalCloud.points.at(i).x;
pt.y = normalCloud.points.at(i).y;
pt.z = normalCloud.points.at(i).z;
bubble.push_back(pt);
}
//For the depth image, calculate bubbles, the invariant vector, normalize it and append to the overall vector
vector<bubblePoint> sphBubble = bubbleProcess::convertBubXYZ2BubSpherical(bubble,maxRangeMeters);
vector<bubblePoint> sphRedBubble = bubbleProcess::reduceBubble(sphBubble);
DFCoefficients dfcoeff = bubbleProcess::calculateDFCoefficients(sphRedBubble,noHarmonics,noHarmonics);
if(normalize_invariants == true){
cv::normalize(bubbleProcess::mstCalculateInvariants(sphRedBubble, dfcoeff,noHarmonics, noHarmonics), normalizedDummyVector);
cv::hconcat(invariants, normalizedDummyVector, invariants);
}
else{
cv::hconcat(invariants, bubbleProcess::mstCalculateInvariants(sphRedBubble, dfcoeff,noHarmonics, noHarmonics), invariants);
}
}
std::cout << "depth invariants are constructed" << std::endl;
//save the invariants in to the callback struct to use in the main loop
invariants.copyTo(callbackStruct->invariants);
//rotate the robot 45 degree to obtain new Kinect data
geometry_msgs::Twist velocityCommandMsg;
velocityCommandMsg.linear.x = 0;
velocityCommandMsg.angular.z = -angular_velocity/2;
velocityCommandPublisher.publish(velocityCommandMsg);
//Send velocity command message for 45 degrees rotation to left
struct timespec tMotion1, tMotion2;
clock_gettime(CLOCK_MONOTONIC, &tMotion1);
clock_gettime(CLOCK_MONOTONIC, &tMotion2);
float elapsedMotionTime = (tMotion2.tv_sec - tMotion1.tv_sec) + (double) (tMotion2.tv_nsec - tMotion1.tv_nsec) * 1e-9;
while(elapsedMotionTime <= 2*(bubble_update_period-9)/12.4){
velocityCommandPublisher.publish(velocityCommandMsg);
clock_gettime(CLOCK_MONOTONIC, &tMotion2);
elapsedMotionTime = (tMotion2.tv_sec - tMotion1.tv_sec) + (double) (tMotion2.tv_nsec - tMotion1.tv_nsec) * 1e-9;
}
struct timespec t_stop1, t_stop2;
clock_gettime(CLOCK_MONOTONIC, &t_stop1);
clock_gettime(CLOCK_MONOTONIC, &t_stop2);
float elapsedStopTime=(t_stop2.tv_sec - t_stop1.tv_sec) + (double) (t_stop2.tv_nsec - t_stop1.tv_nsec) * 1e-9;
while(elapsedStopTime <= 1){
velocityCommandMsg.angular.z = 0;
velocityCommandPublisher.publish(velocityCommandMsg);
clock_gettime(CLOCK_MONOTONIC, &t_stop2);
elapsedStopTime=(t_stop2.tv_sec - t_stop1.tv_sec) + (double) (t_stop2.tv_nsec - t_stop1.tv_nsec) * 1e-9;
}
//Calculate the processing time
clock_gettime(CLOCK_MONOTONIC, &t2);
elapsed_time = (t2.tv_sec - t1.tv_sec) + (double) (t2.tv_nsec - t1.tv_nsec) * 1e-9;
qDebug() << "Processed in " << elapsed_time << " seconds.";
}
void ChatConnection::Connect()
{
QStatus status = ER_OK;
assert(invariants());
createMessageBus();
NotifyUser(MSG_STATUS, "Start the message bus.");
/* Start the msg bus */
if (ER_OK == status) {
status = this->busAttachment->Start();
if (ER_OK != status) {
NotifyUser(MSG_ERROR, "BusAttachment::Start failed (%s)\n", QCC_StatusText(status));
}
}
/* Register a bus listener */
if (ER_OK == status) {
// make sure the callback has been set
this->busAttachment->RegisterBusListener(*(this->busListener));
}
NotifyUser(MSG_STATUS, "Registered BusListener");
/* Connect to the local daemon */
NotifyUser(MSG_STATUS, "Connect to the local daemon.");
if (ER_OK == status) {
status = this->busAttachment->Connect();
}
if (ER_OK != status) {
NotifyUser(MSG_ERROR, "BusAttachment::Connect(%s) failed (%s)\n", this->busAttachment->GetConnectSpec().c_str(), QCC_StatusText(status));
}
if (!this->advertisedName.empty()) {
NotifyUser(MSG_STATUS, "Request name");
status = this->busAttachment->RequestName(this->advertisedName.c_str(), DBUS_NAME_FLAG_DO_NOT_QUEUE);
if (ER_OK != status) {
NotifyUser(MSG_ERROR, "RequestName(%s) failed (status=%s)\n", this->advertisedName.c_str(), QCC_StatusText(status));
status = (status == ER_OK) ? ER_FAIL : status;
}
/* Bind the session port*/
NotifyUser(MSG_STATUS, "Bind session port.");
SessionOpts opts(SessionOpts::TRAFFIC_MESSAGES, true, SessionOpts::PROXIMITY_ANY, TRANSPORT_ANY);
if (ER_OK == status) {
SessionPort sp = CHAT_PORT;
status = this->busAttachment->BindSessionPort(sp, opts, *(this->busListener));
if (ER_OK != status) {
NotifyUser(MSG_ERROR, "BindSessionPort failed (%s)\n", QCC_StatusText(status));
}
}
/* Advertise same well-known name */
if (ER_OK == status) {
status = this->busAttachment->AdvertiseName(this->advertisedName.c_str(), opts.transports);
if (status != ER_OK) {
NotifyUser(MSG_ERROR, "Failed to advertise name %s (%s)\n", this->advertisedName.c_str(), QCC_StatusText(status));
}
}
} else {
/* Discover name */
status = this->busAttachment->FindAdvertisedName(this->joinName.c_str());
if (status != ER_OK) {
NotifyUser(MSG_ERROR, "org.alljoyn.Bus.FindAdvertisedName failed (%s)\n", QCC_StatusText(status));
}
NotifyUser(MSG_STATUS, "Found Advertised Name \n");
}
NotifyUser(MSG_STATUS, "Ready...");
}
