ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

What is causing my callback functions to not function?

asked 2017-06-08 13:17:48 -0500

bhampshire1 gravatar image

updated 2017-06-08 14:57:03 -0500

I'm trying to collect image data and save in to a file however, fwrite and cout are giving garbage values when I check the values from my "buffer". In order find out if this from the result of conversion data lost, I added command line outputs to let me know when my callbacks are being called. After adding the terminal outputs, it has become apparent there is an issue with my callback functions because the whole code would evaluate without doing any of the couts in my callbacks. Is there a structure issue that I'm missing? I running with rosrun.

    //Global Variables
float currentOrientationAngle;

std::vector<uint8_t> rgbImage(480*680*3);

std::vector<uint8_t> depthImage(480*640);

//ROS Callback Function
void odometryCallback(const nav_msgs::Odometry& msg)
{
    std::cout << "Odometry Callback" << std::endl;
    float currentOrientationPosition = msg.pose.pose.orientation.z;
    currentOrientationAngle = 180 + (currentOrientationPosition * 180);
    std::cout << currentOrientationAngle << std::endl;
}

//ROS Callback Function
void depthImageCallback(const sensor_msgs::Image& msg)
{
    std::cout<< "Depth Callback" << std::endl;
    depthImage = msg.data;
    std::cout << depthImage[50] << std::endl;

}

//ROS Callback Function
void imageCallback(const sensor_msgs::Image& msg)
{
    std::cout<< "Image Callback" << std::endl;
        rgbImage = msg.data;
    std::cout << rgbImage[50] << std::endl;
}

int main(int argc, char **argv)
{
     std::cout << "Starting....." << std::endl;

    FILE * imageLogFile = fopen("imageDataLog.txt", "wb+");
    FILE * depthLogFile = fopen("depthDataLog.txt", "wb+"); 
        FILE * orientationLogFile = fopen("orientationDataLog.txt","wb+");

        //ROS Variables
        ros::init(argc, argv, "rosLogger");
        ros::NodeHandle n;
        ros::Subscriber odometrySub = n.subscribe("/odometry/filtered", 60, odometryCallback);
        ros::Subscriber depthImageSub = n.subscribe("/camera/depth_registered/image_raw", 60, depthImageCallback);
        ros::Subscriber imageSub = n.subscribe("/camera/rgb/image_color", 60, imageCallback);
    ros::spinOnce();

    int frameCount = 1;

    //Record from ROS nodes
    while (frameCount <= 5)
    {
        /**
        * Format is:
        * int64_t: Orientation
        * int32_t: depthSize
        * int32_t: imageSize
        * depthSize * unsigned char: depth_compress_buf
        * imageSize * unsigned char: encodedImage->data.ptr
        */
        int imageSize = rgbImage.size();
        int depthSize = depthImage.size();

        std::cout << std::endl << imageSize << std::endl;
        uint8_t imageBuffer[imageSize];
        uint8_t depthBuffer[depthSize];

        //Buffer frames
        for(int i = 0; i < imageSize; i++) 
        {
            imageBuffer[i] = rgbImage[i];
            //std::cout << static_cast<unsigned>(imageBuffer[i]) << std::endl;
            //sleep(1);
        }

        for(int i = 0; i < depthSize; i++)
        {
            depthBuffer[i] = depthImage[i]; //depthImage.data[i];
        }


        fwrite(&currentOrientationAngle, sizeof(float),1,orientationLogFile);

        //fwrite(&depthSize, sizeof(int32_t), 1, logFile);
        //fwrite(&imageSize, sizeof(int32_t), 1, logFile);
        fwrite(depthBuffer, sizeof(uint8_t),sizeof(depthBuffer),depthLogFile);

        fwrite(imageBuffer, sizeof(uint8_t),sizeof(imageBuffer),imageLogFile);

        frameCount++;
        ros::spinOnce();
    }

    fclose(orientationLogFile);
    fclose(depthLogFile);
    fclose(imageLogFile);

    std::cout << "Done!" << std::endl;

    return 1;
}
edit retag flag offensive close merge delete

Comments

It would help to have more information about how you are running your code: rosrun or roslaunch? There are many times when ROS will suppress print statements from going to the terminal, that could be the case here.

Thomas D gravatar image Thomas D  ( 2017-06-08 13:51:31 -0500 )edit

Formatting your code (use the 101010 button) would make it easier for you to get help.

Thomas D gravatar image Thomas D  ( 2017-06-08 13:51:56 -0500 )edit

I'm using rosrun.

bhampshire1 gravatar image bhampshire1  ( 2017-06-08 14:58:00 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2017-06-08 15:16:48 -0500

Airuno2L gravatar image

Maybe there is a chance that your while loop is finishing before any messages come in to trigger the callbacks?

A quick way to test this would to be put a 1 second sleep in the while loop to see if any of the callbacks get called.

(for what it's worth, it looks like a lot of what you have in your while loop should actually be in callbacks)

edit flag offensive delete link more

Comments

I think I should try shifting my buffer array to my callbacks. I was trying to avoid too many globals. I'll try setting the loop rate. However, the images are coming in at 60 Hz.

bhampshire1 gravatar image bhampshire1  ( 2017-06-08 15:23:28 -0500 )edit

I shifted the for loops to their respective callbacks and set the loop rate to 5. The callbacks are being called.

bhampshire1 gravatar image bhampshire1  ( 2017-06-09 22:44:47 -0500 )edit

To avoid globals, you can make everything a class and have those globals as member variables instead.

Airuno2L gravatar image Airuno2L  ( 2017-06-13 08:20:25 -0500 )edit

Question Tools

Stats

Asked: 2017-06-08 13:17:48 -0500

Seen: 404 times

Last updated: Jun 08 '17