Publisher doesn't respond when more processing is required

asked 2021-02-08 14:08:44 -0500

Simao_Araujo gravatar image

Hi,

I have a ROS Node that is responsible for reading data from memory to build a PointCloud2 message and publish it to a topic. If I comment out the X lines and keep the Y lines instead the Publisher works successfully and I am able to visualize the PointCloud2 data in RVIZ. Nevertheless, I need to check if the data that I am reading is valid which is done by keeping the X lines X and commenting out the Y lines. With this approach, the publisher stops working.

So, the publisher works successfully if the X lines are commented and the Y lines are not commented. I have also tried to use an if statement to verify if the data is valid instead of rewriting the same PointCloud position until the data is valid.

NOTE: complete code in attachment.

Questions:

  • What may be causing the problem?
  • How can I solve this issue?
  • Is there a better way to implement this?

Code snippet:

// Build a Double-sampling Gray TDC frame
    while(row < TDC_ROWS){
        while(column < TDC_COLUMNS){
            // Read Double-sampling Gray TDC values
            ds_gray_tdc = *((volatile unsigned *)(saxil_baseaddr + 0xC));
            ds_gray_tdc_value = (ds_gray_tdc & 0b11111111111111111111111111111110) >> 1;  /* X LINE */
            ds_gray_tdc_valid = (ds_gray_tdc & 0b00000000000000000000000000000001);       /* X LINE */

            // Calculate distance in meters; d = v*t -> v = speed of light in m/ps
            //distance = (ds_gray_tdc*0.000299792458);          /* Y LINE */
            distance = (ds_gray_tdc_value*0.000299792458);      /* X LINE */

            pcl_frame.points[row*TDC_COLUMNS + column].x = row;
            pcl_frame.points[row*TDC_COLUMNS + column].y = column;
            pcl_frame.points[row*TDC_COLUMNS + column].z = distance;

            //column++;                                    /* Y LINE */
            column = column + ds_gray_tdc_valid;           /* X LINE */
        }
        column = 0;
        row++;
    }
    row = 0;

Best regards, Simão Araújo

edit retag flag offensive close merge delete