How to write requestFrame() callback function
Lukzard9 opened this issue · 2 comments
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;
}
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: #809.
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.
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