search

analogdevicesinc / aditof_sdk

Analog Devices 3D ToF software suite
https://analogdevicesinc.github.io/aditof_sdk/
BSD 3-Clause "New" or "Revised" License
72 stars 49 forks source link

How to write requestFrame() callback function #809

Closed Lukzard9 closed 1 year ago

Lukzard9 commented 1 year ago

Hello, I'm using the AD-FXTOF1-EBZ camera with ROS on a raspberry pi 3 and I am having trouble writing a c++ code to use the requestFrame(Frame *, FrameUpdateCalback) method. The idea is to improve the Rpi performance since the requestFrame() is very slow without the callback parameter, it publishes images at a rate of 5-6Hz and it's not great for the navigation goal I have in mind. Reading the API I think the callback pointer should improve the performance but I am not totally sure about that.

So, is it actually possible to improve performance or are the raspberry hardware limitation too strict?

If it's possible than the code presents a problem, the callback function never gets actually called. By that I mean that the code inside the callback() function never executes and I have tested it multiple times with a barebone function printing Hello world on a standard output. No response. I am also confident it's the callback causing problems because the version of the code without it publishes correctly at the slow rate cited early.

I admit that I am not familiar with c++ but if needed I can provide other useful information about the problem

### It's possible I include some useless libraries but after various testing I might have forgot to delete some of those

#include <aditof/camera.h>
#include <aditof/depth_sensor_interface.h>
#include <aditof/frame.h>
#include <aditof/system.h>
#ifndef JS_BINDINGS
#include <glog/logging.h>
#else
#include <aditof/log_cout.h>
#endif

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#ifdef OPENCV2
#include <opencv2/contrib/contrib.hpp>
#endif

#include "../../../opencv/aditof_opencv.h"

//added ros lib
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <iostream>
#include <ctime>

using namespace aditof;

class MyClass {       
  private:             
    static image_transport::Publisher pub;
    static ros::Publisher camera_info_pubisher;
    static std::shared_ptr<Camera> camera;

  public:
    static void MyClass::setParameters(image_transport::Publisher param,  ros::Publisher ci_pub, std::shared_ptr<Camera> cam){
        pub = param;
        camera_info_pubisher = ci_pub;
        camera = cam;
    }

    static image_transport::Publisher MyClass::getImagePublisher(){
        return MyClass::pub;
    }

    static ros::Publisher MyClass::getInfoPublisher(){
        return camera_info_pubisher;
    }

    static std::shared_ptr<Camera> MyClass::getCamera(){
        return camera;
    }
};

//initialize static variables
image_transport::Publisher MyClass::pub;
ros::Publisher MyClass::camera_info_pubisher;
std::shared_ptr<Camera> MyClass::camera;

void callback(aditof::Status status, aditof::Frame* frame) {
    ros::Time tStamp = ros::Time::now();
    /* Convert from frame to depth mat */
    cv::Mat mat;
    status = fromFrameToDepthMat(*frame, mat);
    if (status != Status::OK) {
        std::cout << "Error converting frame to depth mat";
    }

    /* Convert from raw values to values that opencv can understand */
    mat.convertTo(mat, CV_8U, distance_scale);

    /* Apply a rainbow color map to the mat to better visualize the
    * depth data */
    applyColorMap(mat, mat, cv::COLORMAP_RAINBOW);

    /* Display the image */
    sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "mono16", mat).toImageMsg();
    image_transport::Publisher pubisher = MyClass::getImagePublisher();
    pubisher.publish(msg);
    cameraInfoMsg->FrameDataToMsg(MyCLass::getCamera(), &frame, tStamp);
    cameraInfoMsg->publishMsg(MyClass::getInfoPublisher());
}

int main(int argc, char *argv[]) {    
    #ifndef JS_BINDINGS 
        google::InitGoogleLogging(argv[0]);
        FLAGS_alsologtostderr = 1;
    #endif

    Status status = Status::OK;

    System system;

    std::vector<std::shared_ptr<Camera>> cameras;
    system.getCameraList(cameras);
    if (cameras.empty()) {
        LOG(WARNING) << "No cameras found!";
        return 0;
    }

    auto camera = cameras.front();
    status = camera->initialize();
    if (status != Status::OK) {
        LOG(ERROR) << "Could not initialize camera!";
        return 0;
    }

    std::vector<std::string> frameTypes;
    camera->getAvailableFrameTypes(frameTypes);
    if (frameTypes.empty()) {
        LOG(ERROR) << "No frame type available!";
        return 0;
    }

    status = camera->setFrameType(frameTypes.front());
    if (status != Status::OK) {
        LOG(ERROR) << "Could not set camera frame type!";
        return 0;
    }

    std::vector<std::string> modes;
    camera->getAvailableModes(modes);
    if (modes.empty()) {
        LOG(ERROR) << "No camera modes available!";
        return 0;
    }

    status = camera->setMode(modes[1]);
    if (status != Status::OK) {
        LOG(ERROR) << "Could not set camera mode!";
        return 0;
    }

    aditof::Frame frame;

    const int smallSignalThreshold = 50;
    camera->setControl("noise_reduction_threshold",
                       std::to_string(smallSignalThreshold));

    ros::init(argc, argv, "image_publisher");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    image_transport::Publisher pub = it.advertise("camera/image", 1);
    MyClass::setParameters(pub, l);
    ros::Rate loop_rate(10);

    //camerainfo message and publisher
    ros::Time timeStamp = ros::Time::now();
    ros::Publisher camera_info_pubisher =
        nh.advertise<sensor_msgs::CameraInfo>("camera_info", 5);
    ROS_ASSERT_MSG(camera_info_pubisher,
                   "creating camera_info_pubisher failed");

    AditofSensorMsg *camera_info_msg = MessageFactory::create(
        camera, &frame, MessageType::sensor_msgs_CameraInfo, timeStamp);
    ROS_ASSERT_MSG(camera_info_msg, "camera_info_msg message creation failed");
    CameraInfoMsg *cameraInfoMsg =
        dynamic_cast<CameraInfoMsg *>(camera_info_msg);
    ROS_ASSERT_MSG(cameraInfoMsg,
                   "downcast from AditofSensorMsg to CameraInfoMsg failed");

    //publisher loop
    while (nh.ok()) {

        /* Request frame from camera */
        status = camera->requestFrame(&frame, callback);
        if (status != Status::OK) {
            LOG(ERROR) << "Could not request frame!";
            return 0;
        }

        ros::spinOnce();
        loop_rate.sleep();

    }

    delete camera_info_msg;
    return 0;
}
rbudai98 commented 1 year ago

Hi! Thank you for reaching out here regarding the issue, However the hardware stup may be a limitation for the node publication frequency, there is another issue regarding this approach. Namely the callback function is not linked to the requestFrame functions (here: https://github.com/analogdevicesinc/aditof_sdk/issues/809#:~:text=camera%2D%3ErequestFrame(%26frame%2C-,callback,-)%3B%0A%20%20%20%20%20%20%20%20if%20(status%20!%3D%20Status). An alternative way to implement it would be to allocate a constant frame memory space, where you update the data with the following way (https://github.dev/analogdevicesinc/aditof_sdk/blob/4351028dd919525b0c5dccb2b7d1cf44360ca558/examples/first-frame/main.cpp#L95-L96) and try to call the callback function manually right after obtaining the frame, and passing on the pointer towards the frame. This should solve the callback problem.

Regarding the performance, since the Raspberry Pi 3 is a low category processing unit, it might not be the best choice for ros framework implementation, but this is just a suggestion. After everything is running could you run an htop in a separate terminal and copy here the results to see the main consumers of the processor.

Lukzard9 commented 1 year ago

I don't have much say on the hardware I can use since I am working on a thesis assigned by my professor, so I am unluckily stuck with the slow raspberry pi. Regarding your first link I am not sure I understood what you are referencing since the #809 Link redirects me at the top of this issue.

After looking at the htop results (snippet below) I think I will stop trying to let the callback function work, and I'll keep using the version of the code without it that publishes at 5-6Hz. I haven't published the code I am talking about but it's basically the same, it doesn't have the class and it executes the callback code inside the while loop. Also I don't pass the function pointer to the requestFrame method.

Just for clarity I'll write that I have tried this approach because the requestFrame is the bottleneck inside my code and I assumed that the callback could speed it up with multiple threads, all of that based on the API that states If cb parameter is not given this operation will be blocking. If a callback is provided this operation will be unblocking and once the data for the frame is ready, an internal thread will call the specified callback.

I am not sure if I am supposed to open another issue but I have noticed that while the code is running the camera gets incredibily hot dropping performance over time, is it normal?

  1  [##############################################################100.0%]   Tasks: 80, 88 thr; 2 running
  2  [                                                                0.0%]   Load average: 0.89 0.25 0.18
  3  [**************************************************************100.0%]   Uptime: 01:21:22
  4  [                                                                0.0%]
  Mem[|||||||||||||||||||#####***********************************232M/872M]
  Swp[                                                            0K/2.00G]
  PID USER      PRI  NI  VIRT   RES   SHR S CPU% MEM%   TIME+  Command
 6244 pi         20   0  122M 39012 19272 R 150.  4.4  0:32.26 /home/pi/workspace/github/aditof_sdk/build/catkin_ws/devel/lib/aditof_roscpp/aditof_camera_no 6268 pi         20   0  4292  2932  2268 R 150.  0.3  0:00.09 htop -C
    1 root       20   0 33836  8284  6520 S  0.0  0.9  0:06.86 /sbin/init splash
  115 root       20   0 40756 10328  9300 S  0.0  1.2  0:21.16 /lib/systemd/systemd-journald
  150 root       20   0 18720  3800  2900 S  0.0  0.4  0:05.22 /lib/systemd/systemd-udevd
 2249 systemd-t  20   0 22384  2892  2308 S  0.0  0.3  0:00.01 /lib/systemd/systemd-timesyncd
 2174 systemd-t  20   0 22384  2892  2308 S  0.0  0.3  0:00.50 /lib/systemd/systemd-timesyncd
 2553 root       20   0 54648  7976  7012 S  0.0  0.9  0:00.00 /usr/sbin/ModemManager --filter-policy=strict
 2613 root       20   0 54648  7976  7012 S  0.0  0.9  0:00.02 /usr/sbin/ModemManager --filter-policy=strict
 2295 root       20   0 54648  7976  7012 S  0.0  0.9  0:00.38 /usr/sbin/ModemManager --filter-policy=strict
 2298 root       20   0 13012  5636  5024 S  0.0  0.6  0:00.33 /lib/systemd/systemd-logind
 2395 root       20   0 25512  3588  2432 S  0.0  0.4  0:00.27 /usr/sbin/rsyslogd -n -iNONE
 2396 root       20   0 25512  3588  2432 S  0.0  0.4  0:00.24 /usr/sbin/rsyslogd -n -iNONE
 2397 root       20   0 25512  3588  2432 S  0.0  0.4  0:00.58 /usr/sbin/rsyslogd -n -iNONE
 2299 root       20   0 25512  3588  2432 S  0.0  0.4  0:01.14 /usr/sbin/rsyslogd -n -iNONE
 2301 root       20   0 12584  7572  7156 S  0.0  0.8  0:00.45 /home/pi/workspace/github/aditof_sdk/build/apps/server/aditof-server
 2308 messagebu  20   0  6916  3780  3028 S  0.0  0.4  0:01.01 /usr/bin/dbus-daemon --system --address=systemd: --nofork --nopidfile --systemd-activation -- 2314 root       20   0 10712  3708  3336 S  0.0  0.4  0:00.09 /sbin/wpa_supplicant -u -s -O /run/wpa_supplicant
 2318 nobody     20   0  4320  2144  1972 S  0.0  0.2  0:00.07 /usr/sbin/thd --triggers /etc/triggerhappy/triggers.d/ --socket /run/thd.socket --user nobody 2328 root       20   0  3776  2204  2028 S  0.0  0.2  0:00.03 /usr/sbin/cron -f
 2337 avahi      20   0  5900  2552  2300 S  0.0  0.3  0:01.34 avahi-daemon: running [raspberrypi.local]
 2547 root       20   0 63184  9076  7432 S  0.0  1.0  0:00.00 /usr/lib/udisks2/udisksd
 2612 root       20   0 63184  9076  7432 S  0.0  1.0  0:00.02 /usr/lib/udisks2/udisksd
 2700 root       20   0 63184  9076  7432 S  0.0  1.0  0:00.00 /usr/lib/udisks2/udisksd
 2815 root       20   0 63184  9076  7432 S  0.0  1.0  0:00.00 /usr/lib/udisks2/udisksd
 2345 root       20   0 63184  9076  7432 S  0.0  1.0  0:00.44 /usr/lib/udisks2/udisksd
 2363 root       20   0 27656    80     0 S  0.0  0.0  0:00.11 /usr/sbin/rngd -r /dev/hwrng
 2364 root       20   0 27656    80     0 S  0.0  0.0  0:00.00 /usr/sbin/rngd -r /dev/hwrng
 2365 root       20   0 27656    80     0 S  0.0  0.0  0:00.00 /usr/sbin/rngd -r /dev/hwrng
 2362 root       20   0 27656    80     0 S  0.0  0.0  0:00.13 /usr/sbin/rngd -r /dev/hwrng