Driveworks convert ROS Image to dwImageCUDA

Hi,
I’m trying to convert a ROS sensor_msgs::ImageConstPtr into dwImageCUDA to be able to feed that into the LaneNet provided in Driveworks. I’m using a DrivePX2 as platform and Driveworks 0.6.67 Release.
I’ve read through all the similar posts on these forums (most of the use more updated Driveworks versions targeting Drive AGX). I’ve managed to get the following by combining all the information from the topics I’ve seen, unfortunately I’m still getting a segfault. The idea is to convert the ros image first to cv mat and from there convert it to dwImageCPU.
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
ROS_INFO(“Received a new image”);
dwImageCPU* imageCPU = nullptr;
dwImageProperties prop;
prop.height=msg->height;
prop.width=msg->width;
prop.pxlFormat = DW_IMAGE_RGB;
prop.pxlType= DW_TYPE_UINT8;
prop.type = DW_IMAGE_CPU;
cv_bridge::CvImagePtr cvImgPtr = cv_bridge::toCvCopy(msg, “”);
dwImageCPU_create(imageCPU, &prop);
mempcpy(imageCPU->data[0], cvImgPtr->image.data, cvImgPtr->image.total() * cvImgPtr->image.elemSize());
}

On memcpy I’m getting the segfault and I really cannot see what I’m doing wrong.
The idea is that once I have the dwImageCPU I’ll use ImageStreamer to convert that to dwImageCUDA as described in here: What is the good way to subscribe a ROS image and convert to Nvidia image types.

If anyone could help me out it’d be highly appreciated as it is a pain to figure it out with the limited material that can be found online.
Looking forward to your reply!
BR
Alex

Hi
I’ve been breaking my head for a very long time about this and managed to book some progress.
The issue for the segfault was that I was initialising the imageCPU to a nullptr. I’ve switched it up to be just an object and pass the address to the dwImageCPU_create(), this resolved the issue.
I’ve went further and and converted the imageCPU to imageCUDA by following some code from image_streamer_simple. I did manage to get it run, but the laneNet inferences was never returning results. I’ve figured that this was probably caused by the fact that my image is rgb and in the example code rgba was used.
So I’ve written some code to convert rgb to rgba and tried to convert that to ImageCUDA, but I keep getting a segfault on dwImageStreamer_postCPU().
Below you can find the code, it is a ros callback that listens on an image.
I’ve added a comment ‘Crashes on next line with segfault’ to indicate the line it goes wrong. You can see that I’ve tried different memcpy commands, both give an issue.

void imageCallback(const sensor_msgs::ImageConstPtr& msg) {
ROS_INFO(“Received a new image”);
dwImageCPU imageCPU;
dwImageProperties prop;
prop.height=msg->height;
prop.width=msg->width;
prop.pxlFormat = DW_IMAGE_RGBA;
prop.pxlType= DW_TYPE_UINT8;
prop.type = DW_IMAGE_CPU;
prop.planeCount = 1;
//Convert to CVImagePtr to use the Mat object
cv_bridge::CvImagePtr cvImgPtr = cv_bridge::toCvCopy(msg, “”);

    //Prepare rgba frame, we will convert our rgb image into this
    cv::Mat rgbaFrame(cvImgPtr->image.size(), CV_8UC4, new uchar[cvImgPtr->image.total()*4]);
    //Convert rgb to rgba
    cv::cvtColor(cvImgPtr->image, rgbaFrame, CV_RGB2RGBA, 4);
    //cv::imwrite("test.jpg", rgbaFrame);

    CHECK_DW_ERROR(dwImageCPU_create(&imageCPU, (const dwImageProperties*) &prop));

    //auto s = rgbaFrame.size();
    //mempcpy(imageCPU.data[0], &rgbaFrame.data[0], s.height * s.width * 4);

    mempcpy(imageCPU.data[0], rgbaFrame.data, rgbaFrame.total() * rgbaFrame.elemSize());
    dwImageStreamerHandle_t m_streamerCPU2CUDA = DW_NULL_HANDLE;

    auto status = dwImageStreamer_initialize(&m_streamerCPU2CUDA, &prop, DW_IMAGE_CUDA, context);
    if (status != DW_SUCCESS) {
        logError("Cannot init image streamer m_streamerCPU2CUDA: %s\n", dwGetStatusName(status));
        return;
    }

    //Crashes on next line with a segfault
    status = dwImageStreamer_postCPU(&imageCPU, m_streamerCPU2CUDA);
    if (status != DW_SUCCESS) {
        std::stringstream err;
        err << "Cannot post m_rgbaCPU in m_streamerCPU2CUDA: " << dwGetStatusName(status);
        throw std::runtime_error(err.str().c_str());
    }

    // use a pointer to a dwImageCUDA to receive the converted image from the streamer. We receive
    // a pointer as the image is only "borrowed" from the streamer who has the ownership. Since the image is
    // borrowed we need to return it when we are done using it
    dwImageCUDA *rgbaCuda;
    // receive the converted CUDA image from the stream, timeout set to 1000 ms
    status = dwImageStreamer_receiveCUDA(&rgbaCuda, 1000, m_streamerCPU2CUDA);
    if (status != DW_SUCCESS) {
        std::stringstream err;
        err << "Cannot receive cuda image from cpu : " << dwGetStatusName(status);
        throw std::runtime_error(err.str().c_str());
    }


    runLaneDetectionInference(rgbaCuda);


    status = dwImageStreamer_release(&m_streamerCPU2CUDA);
    if (status != DW_SUCCESS) {
        logError("Cannot release m_streamerCPU2CUDA: %s\n", dwGetStatusName(status));
    }


}

If someone could point me into the right direction that’d be highly appreciated.
BR
Alex

Sorry for the late response.
We will need you to use your account with corporate or university email address.
Or you can also change your current account to use corporate or university email address by following the below links:
My Profile | NVIDIA Developer → “Edit Profile” → “Change email” → “CHANGE”
Sorry for any inconvenience.
Thanks