Cannot create a socket

Development
1

Hi all,
I was trying to communicate a software called ORB-SLAM2 with Slicer using OpenIGTLink library to pass the transform information of the camera movement to Slicer. I have the following code integrated in ORB-SLAM2:

igtl::ServerSocket::Pointer serverSocket;
  serverSocket = igtl::ServerSocket::New();
  int r = serverSocket->CreateServer(18944);
   
  igtl::TransformMessage::Pointer transMsg;
  transMsg = igtl::TransformMessage::New();
  transMsg->SetDeviceName("Tracker");

   if (r < 0)
    {
    std::cerr << "Cannot create a server socket." << std::endl;
    exit(0);
    }

  igtl::Socket::Pointer socket;
  socket = serverSocket->WaitForConnection(1000);
  igtl::Matrix4x4 matrix;
        
        matrix[0][0]=Twc.m[0];
        matrix[1][0]=Twc.m[1];
        matrix[2][0]=Twc.m[2];
        matrix[3][0]=Twc.m[3];

        matrix[0][1]=Twc.m[4];
        matrix[1][1]=Twc.m[5];
        matrix[2][1]=Twc.m[6];
        matrix[3][1]=Twc.m[7];

        matrix[0][2]=Twc.m[8];
        matrix[1][2]=Twc.m[9];
        matrix[2][2]=Twc.m[10];
        matrix[3][2]=Twc.m[11];

        matrix[0][3]=Twc.m[12];
        matrix[1][3]=Twc.m[13];
        matrix[2][3]=Twc.m[14];
        matrix[3][3]=Twc.m[15];
	
	transMsg->SetMatrix(matrix);
        transMsg->Pack();
        socket->Send(transMsg->GetPackPointer(), transMsg->GetPackSize());
        socket->CloseSocket();

but when I run the code it gives me an error (segment violation). I think that the problem is in the creation of the socket, because when I comment this line “socket->Send(transMsg->GetPackPointer(), transMsg->GetPackSize());” it doesn’t give any error but It neither send the data. I would like to know if I am creating the socket in a wrong way or what could be the problem.

Thanks

2

There are several examples in OpenIGTLink library: https://github.com/openigtlink/OpenIGTLink/tree/master/Examples/Tracker.

First just build those and see if they work, then choose one and modify it to fit your needs.

In general, we integrate these kind of algorithms and interfaces in the Plus toolkit (www.plustoolkit.org). For example, we’ve just added to Plus webcam-based tracking using ArUco.

3

If :18944 is already open in another process you won’t be able to create a new socket on that port.

4

That’s a good point! Make sure one server communicates with one client (Slicer can be either a client or server).

5

I have tried with the TrackerServer example and it works (also the socket in the port 18944). I tried to adapt the code from that example to use in my program (ORB-SLAM2), but in this program the socket doesn’t work correctly.

6

Compare all the compiler and linker options of the working and non-working example.
Do you use CMake?
What operating system do you use?

7
  • Use a debugger and step through the code, figure out exactly where is the segfault.
  • Add some print statements to tell whether it receives the connection (WaitForConnection will block).
  • Use wireshark to monitor connections/traffic.
8

Yes, I am using Cmake and Ubuntu 14.04

9

Now, I have established connection between ORB-SLAM2 an Slicer using OpenIGTLink changing the socket code shown before, but when I make socket->Send(transMsg->GetPackPointer(), transMsg->GetPackSize()); I only send the first transformation correctly but this transformation doesn’t change like in trackerserver example. I’ve checked the matrix I employ (making a igtl::PrintMatrix(matrix); ) and this matrix works well (It changes in each iteration), but it seems that the part
transMsg->SetMatrix(matrix);

    transMsg->Pack();

    socket->Send(transMsg->GetPackPointer(), transMsg->GetPackSize());

does not update the message sent to Slicer. Any idea of what can be the problem?

Thanks

10

Is memory content at transMsg->GetPackPointer() is changing?

11

No, I have always the same memory content ( 0x7f1340603da0)

12

The pointer itself is irrelevant, check the memory content at that location (up to GetPackSize() number of bytes). If you don’t see any change that’s great, it means that the error is not in the communication part.

Probably it’s some trivial issue. If you send a link to the source code then I can have a look.

13

I have checked the GetPackSize and it´s also the same always. The code I am using is this(I have added the OpenIGTLink connection to pass the Twc.m information to Slicer):

/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/

#include "Viewer.h"
#include <pangolin/pangolin.h>

#include <mutex>

//includes added
#include <iostream>
#include <math.h>
#include <cstdlib>

#include "igtlOSUtil.h"
#include "igtlTransformMessage.h"
#include "igtlServerSocket.h"


namespace ORB_SLAM2
{

Viewer::Viewer(System* pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Tracking *pTracking, const string &strSettingPath):
    mpSystem(pSystem), mpFrameDrawer(pFrameDrawer),mpMapDrawer(pMapDrawer), mpTracker(pTracking),
    mbFinishRequested(false), mbFinished(true), mbStopped(true), mbStopRequested(false)
{
    cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);

    float fps = fSettings["Camera.fps"];
    if(fps<1)
        fps=30;
    mT = 1e3/fps;

    mImageWidth = fSettings["Camera.width"];
    mImageHeight = fSettings["Camera.height"];
    if(mImageWidth<1 || mImageHeight<1)
    {
        mImageWidth = 640;
        mImageHeight = 480;
    }

    mViewpointX = fSettings["Viewer.ViewpointX"];
    mViewpointY = fSettings["Viewer.ViewpointY"];
    mViewpointZ = fSettings["Viewer.ViewpointZ"];
    mViewpointF = fSettings["Viewer.ViewpointF"];
}

void Viewer::Run()
{
  //parte anadida
  igtl::TransformMessage::Pointer transMsg;
  transMsg = igtl::TransformMessage::New();
  transMsg->SetDeviceName("TrackerORB");

  igtl::ServerSocket::Pointer serverSocket;
  serverSocket = igtl::ServerSocket::New();
  int r = serverSocket->CreateServer(18944);
   
  

   if (r < 0)
    {
    std::cerr << "Cannot create a server socket." << std::endl;
    exit(0);
    }

  igtl::Socket::Pointer socket1;
  //socket1= igtl::Socket::New();
  igtl::Matrix4x4 matrix;

//final parte anadida
    mbFinished = false;
    mbStopped = false;

    pangolin::CreateWindowAndBind("ORB-SLAM2: Map Viewer",1024,768);

    // 3D Mouse handler requires depth testing to be enabled
    glEnable(GL_DEPTH_TEST);

    // Issue specific OpenGl we might need
    glEnable (GL_BLEND);
    glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::CreatePanel("menu").SetBounds(0.0,1.0,0.0,pangolin::Attach::Pix(175));
    pangolin::Var<bool> menuFollowCamera("menu.Follow Camera",true,true);
    pangolin::Var<bool> menuShowPoints("menu.Show Points",true,true);
    pangolin::Var<bool> menuShowKeyFrames("menu.Show KeyFrames",true,true);
    pangolin::Var<bool> menuShowGraph("menu.Show Graph",true,true);
    pangolin::Var<bool> menuLocalizationMode("menu.Localization Mode",false,true);
    pangolin::Var<bool> menuReset("menu.Reset",false,false);

    // Define Camera Render Object (for view / scene browsing)
    pangolin::OpenGlRenderState s_cam(
                pangolin::ProjectionMatrix(1024,768,mViewpointF,mViewpointF,512,389,0.1,1000),
                pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0)
                );

    // Add named OpenGL viewport to window and provide 3D Handler
    pangolin::View& d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f/768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));

    pangolin::OpenGlMatrix Twc;
    Twc.SetIdentity();

    cv::namedWindow("ORB-SLAM2: Current Frame");

    bool bFollow = true;
    bool bLocalizationMode = false;

    while(1)
    {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        mpMapDrawer->GetCurrentOpenGLCameraMatrix(Twc);
	socket1 = serverSocket->WaitForConnection(1);

if (socket1.IsNotNull()) // if client connected
      {
	std::cerr << "A client is connected." << std::endl;

        
	matrix[0][0]=Twc.m[0];
        matrix[1][0]=Twc.m[1];
        matrix[2][0]=Twc.m[2];
        matrix[3][0]=0;//Twc.m[3];

        matrix[0][1]=Twc.m[4];
        matrix[1][1]=Twc.m[5];
        matrix[2][1]=Twc.m[6];
        matrix[3][1]=0;//Twc.m[7];

        matrix[0][2]=Twc.m[8];
        matrix[1][2]=Twc.m[9];
        matrix[2][2]=Twc.m[10];
        matrix[3][2]=0;//Twc.m[11];

        matrix[0][3]=Twc.m[12];
        matrix[1][3]=Twc.m[13];
        matrix[2][3]=Twc.m[14];
        matrix[3][3]=1;//Twc.m[15];


	transMsg->SetMatrix(matrix);

	igtl::PrintMatrix(matrix);

        transMsg->Pack();

        socket1->Send(transMsg->GetPackPointer(), transMsg->GetPackSize());
	igtl::Sleep(1);

	std::cerr << "Size of pack: " <<  transMsg->GetPackSize()<< std::endl;
	
	}
        if(menuFollowCamera && bFollow)
        {
            s_cam.Follow(Twc);
        }
        else if(menuFollowCamera && !bFollow)
        {
            s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(mViewpointX,mViewpointY,mViewpointZ, 0,0,0,0.0,-1.0, 0.0));
            s_cam.Follow(Twc);
            bFollow = true;
        }
        else if(!menuFollowCamera && bFollow)
        {
            bFollow = false;
        }

        if(menuLocalizationMode && !bLocalizationMode)
        {
            mpSystem->ActivateLocalizationMode();
            bLocalizationMode = true;
        }
        else if(!menuLocalizationMode && bLocalizationMode)
        {
            mpSystem->DeactivateLocalizationMode();
            bLocalizationMode = false;
        }

        d_cam.Activate(s_cam);
        glClearColor(1.0f,1.0f,1.0f,1.0f);
        mpMapDrawer->DrawCurrentCamera(Twc);
        if(menuShowKeyFrames || menuShowGraph)
            mpMapDrawer->DrawKeyFrames(menuShowKeyFrames,menuShowGraph);
        if(menuShowPoints)
            mpMapDrawer->DrawMapPoints();

        pangolin::FinishFrame();

        cv::Mat im = mpFrameDrawer->DrawFrame();
        cv::imshow("ORB-SLAM2: Current Frame",im);
        cv::waitKey(mT);

        if(menuReset)
        {
            menuShowGraph = true;
            menuShowKeyFrames = true;
            menuShowPoints = true;
            menuLocalizationMode = false;
            if(bLocalizationMode)
                mpSystem->DeactivateLocalizationMode();
            bLocalizationMode = false;
            bFollow = true;
            menuFollowCamera = true;
            mpSystem->Reset();
            menuReset = false;
        }

        if(Stop())
        {
            while(isStopped())
            {
                std::this_thread::sleep_for(std::chrono::microseconds(3000));
            }
        }

        if(CheckFinish())
	{//parte anadida
	   // socket1->CloseSocket();
            break;
	}
    }

    SetFinish();
}

void Viewer::RequestFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    mbFinishRequested = true;
}

bool Viewer::CheckFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    return mbFinishRequested;
}

void Viewer::SetFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    mbFinished = true;
}

bool Viewer::isFinished()
{
    unique_lock<mutex> lock(mMutexFinish);
    return mbFinished;
}

void Viewer::RequestStop()
{
    unique_lock<mutex> lock(mMutexStop);
    if(!mbStopped)
        mbStopRequested = true;
}

bool Viewer::isStopped()
{
    unique_lock<mutex> lock(mMutexStop);
    return mbStopped;
}

bool Viewer::Stop()
{
    unique_lock<mutex> lock(mMutexStop);
    unique_lock<mutex> lock2(mMutexFinish);

    if(mbFinishRequested)
        return false;
    else if(mbStopRequested)
    {
        mbStopped = true;
        mbStopRequested = false;
        return true;
    }

    return false;

}

void Viewer::Release()
{
    unique_lock<mutex> lock(mMutexStop);
    mbStopped = false;
}

}
14

Without timestamp updates, Slicer may reject messages as duplicates. Keep timestamp up-to-date, as it is shown in OpenIGTLink examples.

  igtl::TimeStamp::Pointer ts = igtl::TimeStamp::New();
  ...

  ts->GetTime();
  transMsg->SetTimeStamp(ts);
15

Now it works, thank you very much. I do what you say and also I created a new transmsg for each iteration