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..."); }