D435 and T265 on Raspberry 4(C++)

[altb71]

Hi all
Using D435 and T265 on Raspi4 works in principle (T265 has to be plugged later).
Running the "multicam" example with D435 and T265 (via xtra powerded USB hub) works at the beginning and shows data / images of both cams. After approx 20 sec T265 hangs. Killing the app/ replugging T265 / restarting multicam works but is not feasible solution (system should run on a copter) . I find a solution for python, but I am not able to convert it to C++.
Any help?
Thanks Ruprecht

[dorodnic]

Hi @muco42
Do you see this behaviour in realsense-viewer or rs-capture tools?

[altb71]

Both: realsense-viewer connects both, after a short time T265 hangs. the same behaviour with the examples (e.g. multicam)

[csr-kick]

@dorodnic @MartyG-RealSense
There are probably a dozen different issues listed on this github over the last two months about this problem with zero response from Intel. The T265 just fails after a random period of time (from seconds to minutes), and the only way to get it back is to restart the software (realsense-viewer, or any of the provided examples) and sometimes plugging and unplugging the sensor. Please please please help.

[MartyG-RealSense]

@csr-kick T265 cases are being handled by someone else, so I cannot take them on.

[csr-kick]

@MartyG-RealSense
That's an odd approach to have, but ok. Who is responsible? Have they been informed about these issues? Is there a plan to address them? A timeline?

Why should any company purchase a realsense device, never mind attempt to use one in a product, if support for the device will just disappear when problems are found?

[MartyG-RealSense]

@csr-kick Intel are aware of the T265 cases, but I do not have any more information to provide other than to re-state that somebody else other than myself is responsible for them.

[bobgates]

Can we get their name? I share @csr-kick's frustration. It seems like the T265 is the ugly stepchild that Intel tries to forget about...

[MartyG-RealSense]

I cannot provide names of support team members handling T265 cases.

[csr-kick]

@MartyG-RealSense
Even though you guys don't seem to care, maybe pass along this thread:

https://www.raspberrypi.org/forums/viewtopic.php?f=29&t=281665

Really sad that others have to support your devices for you, while you continue to sell them.

[jameskiki]

Hi There
I am currently working on this project with @muco42
There seem to be a few problems

• the T265 has trouble beeing recognized when connected to the RPI (the device is visible and i can request information from it)
  how ever if i try to start the pipe i get two errors
  --- Warning [default] Could not open device failed to set power state
  --- ... Unable to open device interface
  If i unplug and replug the device while this error is beeing displayed the device connects as desired

• i tried using the callbacks to get this (two cameras) to work, however i have the problam that as soon as i "connect" a CB to it, the other devices CB stops working (hower this is not always the case)

• I also tried using the poll_for_frame functions but with the same result, as soon as i connect to one of the devices, the previously connected one no longer works (callbacks no longer executed)

Are there any docs on how the Library works in the Background? I'm trying to understand where the two cameras are disturbing each other or are we missing some switch we need to flip or something like that? (i doubt this, as it seems to work sometimes)

pls tell me what informtion you need and i will try to provide it (I will be on holiday starting tomorrow, i will still try to check in every once in a while)

[MarcoAndrade26]

Is there any solution? I am having same problem

[jameskiki]

Hi @MarcoAndrade26
I pieced together the following solution, in my opinion it's rather hacky, but maybe it helps.
Please keep in mind i am rather new to all this stuff.

I was able to get the cameras to "work" i don't get any complete failures anymore, however i do have to ocasionaly reset the T265 (within the application).
I would share my repo but I'm having trouble making it public (it's on github.enterprise), i will share as soon as i can

i can't tell you why but it seems to work better if you use the "low level API" for the T265 and the "high level API" for the D435, see this issue #3434

The main difference is in the Connect/Disconnect member function

these are my individual camera classes:
T265

#include "RsT265.h"

RsT265::RsT265(){}

RsT265::RsT265(rs2::device _dev) :RsCam(_dev){}

RsT265::~RsT265(){}

void RsT265::Configure()
{
    m_pipe = *new rs2::pipeline(*m_ctx);
    m_cfg =  *new rs2::config();
    m_cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);
    m_cfg.enable_device(m_serial);
}

bool RsT265::ConnectWithCB()
{
    if (m_wd)
        delete m_wd;

    if (m_queue)
        delete m_queue;

    // start the sensor providing a callback to get the frame
    printf("Deploying callback for T265...\n");
    m_wd = new WatchDog(20, "T265_WD");
    m_queue = new rs2::frame_queue(500);

    auto cb = [this](rs2::frame f) {
        //printf("thread id: %i", std::this_thread::get_id());
        //printf("  T265\n");
        m_wd->Kick();

        if (f.get_profile().stream_type() == RS2_STREAM_POSE) {
            m_queueMut.lock();
            m_queue->enqueue(f);
            m_queueMut.unlock();
        }
        else if (f.get_profile().stream_type() == RS2_STREAM_FISHEYE) {
            // this is one of the fisheye imagers
            auto fisheye_frame = f.as<rs2::video_frame>();
        }
    };

    // open the profiles
    rs2::sensor sensor = m_dev.first<rs2::sensor>();
    std::vector<rs2::stream_profile> profs = sensor.get_stream_profiles();
    rs2::stream_profile prof;

    bool retVal = false;
    try
    {
        sensor.open(profs);
        sensor.start(cb);
        retVal = true;
    }
    catch (const rs2::error& e)
    {
        printf("ERROR: connecting CB in T265\n");
        printf(e.what() + '\n');
        m_dev.hardware_reset();
    }
    m_wd->Kick();
    return retVal;
}

bool RsT265::Disconnect()
{
    auto sensor = m_dev.first<rs2::sensor>();
    try
    {
        sensor.stop();
        sensor.close();
        return true;
    }
    catch (const rs2::error& e)
    {
        printf("error in Disconnecting Device T265\n");
        printf(e.what() + '\n');
        m_dev.hardware_reset();
        printf("Called HW-Reset\n");
        return false;
    }
}

D435

#include "RsD435.h"

RsD435::RsD435(){}

RsD435::RsD435(rs2::device _dev):RsCam(_dev){}

RsD435::~RsD435(){}

void RsD435::Configure()
{
	m_pipe = *new rs2::pipeline(*m_ctx);
	m_cfg = *new rs2::config();
    m_cfg.enable_stream(RS2_STREAM_DEPTH);
    m_cfg.enable_device(m_serial);
}

bool RsD435::ConnectWithCB()
{
    printf("Deploying callback for D435...\n");

    if (m_wd)
        delete m_wd;

    if (m_queue)
        delete m_queue;

    m_wd = new WatchDog(220, "D435_WD");
    m_queue = new rs2::frame_queue(500);

    auto cb = [=](rs2::frame f) {
        m_wd->Kick();

        if (f.get_profile().stream_type() == RS2_STREAM_DEPTH) {
            m_queueMut.lock();
            m_queue->enqueue(f);
            m_queueMut.unlock();
        }
    };

    Configure();

    bool retVal = false;
    try
    {
        m_pipe.start(m_cfg, cb);
        retVal = true;
    }
    catch (const rs2::error& e)
    {
        printf(e.what() + '\n');
    }
    m_wd->Kick();
    return retVal;
}

And they both inherit from this class
RsCam

#include "RsCam.h"

RsCam::RsCam(){}

RsCam::RsCam(rs2::device _dev)
{
    printf("...RsCam Object instantiated...\n");
    m_dev = _dev;
    m_name = getName();
    m_serial = *getSerial();
}

RsCam::~RsCam()
{
    try
    {
        Disconnect();
    }
    catch (const std::exception&)
    {
        // ignore something went wrong, otherwise abort() will be called
    }
    printf("device instance of %s deleted\n", m_name.c_str());

    delete m_queue;
    m_ctx = nullptr; // DONOT Delete ctx object as this will brake the application (T265 can only be alloted to one context object)
}

// returns the device name
std::string RsCam::GetName()
{
    return m_name;
}

rs2::device RsCam::GetDevice()
{
    return getDevice();
}

std::string* RsCam::getSerial()
{
    std::string* sn = new std::string("???");
    if (m_dev.supports(RS2_CAMERA_INFO_SERIAL_NUMBER))
        *sn = m_dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
    return sn;
}

std::string RsCam::getName()
{
    std::string name = "Unknown Device";
    if (m_dev.supports(RS2_CAMERA_INFO_NAME))
        name = m_dev.get_info(RS2_CAMERA_INFO_NAME);
    return name;
}

rs2::device RsCam::getDevice()
{
    return m_dev;
}

bool RsCam::Connect()
{
    printf("trying to connect to: %s\n", m_name.c_str());
    bool retVal = false;
    try
    {
        m_pipe.start(m_cfg);
        retVal = true;
    }
    catch (const rs2::error& e)
    {
        printf(e.what());
        printf("\n");
        throw e;
    }
    return retVal;
}

bool RsCam::Disconnect()
{
    try
    {
        m_pipe.stop();
    }
    catch (const rs2::error& e)
    {
        printf(e.what() + '\n');
        throw e;    // no handling happens here, so throw
    }

    return true;
}

void RsCam::setContext(rs2::context *_ctx)
{
    m_ctx = _ctx;
}

bool RsCam::CheckWD()
{
    return m_wd->Check();
}

bool RsCam::pollForFrame(rs2::frame* f)
{
    std::lock_guard<std::mutex> lock(m_queueMut);
    return m_queue->poll_for_frame(f);    
}

And here the "Main()" which checks a watchdog and resets the device, if the callback has not been executed (watchdog not kicked) for some time.

int CamController::Main()
{
    Timer t = Timer();

#ifdef RPI
    Communicator com;
#endif // RPI

    try
    {
        SetupConnectedDevices();
    }
    catch (const rs2::error& e)
    {
        logger->printf("Exception in setting up devices...\n");
        logger->printf(e.what() + '\n');
    }

    SetDevChangeCB();

    logger->printf("Readied %i devices...\n", m_devs.size());
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    for (RsCam* cam : m_knownDevs)
    {
        cam->ConnectWithCB();
        // ToDo: inform state via UART
    }
    int cntTracker = 0;
    int cntDepth = 0;
    int cntExpT265 = 0;
    int cntExpD435 = 0;

    bool newFrame = false;

    while (true)
    {
	newFrame = false;
        // Check all devices functioning
        m_knownDevsMut.lock();
        for (RsCam* cam : m_knownDevs)
        {
            // Check if Watchdogs are still running
            if (cam->CheckWD()) {
                logger->printf("Watchdog for %s expired!!! \n", cam->GetName().c_str());

                if (cam->GetName().find("T265") != std::string::npos)
                {
                    cntExpT265++;
                    // ToDo: inform via UART
                }
                else 
                {
                    cntExpD435++;
                    // ToDo: inform via UART
                };

                try
                {
                    cam->Disconnect();
                    std::this_thread::sleep_for(std::chrono::milliseconds(10));
                    cam->ConnectWithCB();
                }
                catch (const rs2::error& e)
                {
                    logger->print("Error in CamCont main\n");
                    logger->printf(e.what() + '\n');
                    throw e; // no handling happens here, so throw
                }

            }

            // get frames from queues
            rs2::frame fr;
            while (cam->pollForFrame(&fr))
            {
                auto prof = fr.get_profile();

                switch (prof.stream_type())
                {
                case RS2_STREAM_POSE:
                    cntTracker++;
		    newFrame = true;
#ifdef RPI
                    com.UpdatePose(&fr);
#endif // RPI
                    break;

                    //case RS2_STREAM_COLOR:
                case RS2_STREAM_DEPTH:
                    cntDepth++;
		    newFrame = true;
                    {
                        auto depth_fr = fr.as<rs2::frameset>().get_depth_frame();
                        auto res = Computer::ComputeROIs(&depth_fr, 5, 64, 50);

#ifdef RPI
                        com.UpdateDepth(&fr);
                        com.UpdateRoiDepthAngle(res->dist, res->alpha);
#endif // RPI
                    }
                    break;
                default:
                    break;
                }

            }
        }
        m_knownDevsMut.unlock();

	if(newFrame)
	{
        logger->printf("%s  frames total T265: %i     and D435: %i   resets T265: %i  resets D435: %i\n",t.Elapsed().c_str(), cntTracker, cntDepth, cntExpT265, cntExpD435);
	}

#ifdef RPI
        com.checkRequestAndReact();
#endif // RPI

        //std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    return 0;
}

If you have any questions don't hesitate to ask, i'll try my best to answer

[RealSenseSupport]

Hi,

Will you be needing further help with this? If we don’t hear from you in 7 days, this issue will be closed.

Thanks

[altb71]

Yes! Von: RealSenseSupport <notifications@github.com> Gesendet: Samstag, 5. Dezember 2020 03:47 An: IntelRealSense/librealsense <librealsense@noreply.github.com> Cc: Altenburger Ruprecht (altb) <altb@zhaw.ch>; Mention <mention@noreply.github.com> Betreff: Re: [IntelRealSense/librealsense] D435 and T265 on Raspberry 4(C++) (#6707) Hi, Will you be needing further help with this? If we don’t hear from you in 7 days, this issue will be closed. Thanks — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub<#6707 (comment)>, or unsubscribe<https://github.com/notifications/unsubscribe-auth/ABPOXB7Z7FUJLGAF2PXL2PLSTGNJZANCNFSM4OJM4JNQ>.

[csr-kick]

Hi,

Will you be needing further help with this? If we don’t hear from you in 7 days, this issue will be closed.

Thanks

Yes we still need help. No one ever actually responded to our multiple threads about these issues, and the problems still exist. Then you went off and stopped supporting the T265 while you work on the next gen. No idea why you think companies will trust you on that product when you have not earned it on this one.

[altb71]

Yes! Von: csr-kick <notifications@github.com> Gesendet: Donnerstag, 10. Dezember 2020 00:22 An: IntelRealSense/librealsense <librealsense@noreply.github.com> Cc: Altenburger Ruprecht (altb) <altb@zhaw.ch>; Mention <mention@noreply.github.com> Betreff: Re: [IntelRealSense/librealsense] D435 and T265 on Raspberry 4(C++) (#6707) Hi, Will you be needing further help with this? If we don’t hear from you in 7 days, this issue will be closed. Thanks Yes we still need help. No one ever actually responded to our multiple threads about these issues, and the problems still exist. Then you went off and stopped supporting the T265 while you work on the next gen. No idea why you think companies will trust you on that product when you have not earned it on this one. — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub<#6707 (comment)>, or unsubscribe<https://github.com/notifications/unsubscribe-auth/ABPOXB5VVODDSAJGD3O2ODTSUABCDANCNFSM4OJM4JNQ>.

[jameskiki]

They canceled support for the t265? I can't find any press release or something along those lines, can you post a link? Seeing as how they are handling this problem i wouldn't be that suprised. This is extremly disappointing...
I've seen opensource projects by one or two guys handled better than this :(

[csr-kick]

Why is this issue now closed when the problem still exists? It needs to stay open to inform current and future potential users that these issues are not addressed, rather than be buried out of inconvenience.