What is causing my callback functions to not function?
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(¤tOrientationAngle, 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;
}
It would help to have more information about how you are running your code:
rosrun
orroslaunch
? There are many times when ROS will suppress print statements from going to the terminal, that could be the case here.Formatting your code (use the
101010
button) would make it easier for you to get help.I'm using rosrun.