0
$\begingroup$

Rosanswers logo

Dear Ros users, I've successfully created a simulator in gazebo. Now i want to communicate to the controller via topics. I can't seem to succeed at creating a publisher/subscriber S-function. I've included all the necessary header files, but Matlab crashes during initialization(part of the crashdump included). Even though i only call upon ros::init.

I've seen some question about this, yet no definitive answers. Has there been any earlier (succesful or not) attempt at this?

Thanks,

Erik

Segment of ros code causing the crash.

 int argc = 0;
 char** argv = NULL;
 ros::init(argc,argv,"talker");
 ros::NodeHandle n;

Crashdump:

------------------------------------------------------------------------
            Abort signal detected at Fri Jan  6 15:04:10 2012
------------------------------------------------------------------------

Configuration:
  Crash Decoding  : Disabled
  Current Visual  : 0x21 (class 4, depth 24)
  Default Encoding: UTF-8
  GNU C Library   : 2.11.1 stable
  MATLAB Root     : /usr/local/MATLAB/R2011b
  MATLAB Version  : 7.13.0.564 (R2011b)
  Operating System: Linux 2.6.32-37-generic-pae #81-Ubuntu SMP Fri Dec 2 22:24:22 UTC 2011 i686
  Processor ID    : x86 Family 6 Model 10 Stepping 5, GenuineIntel
  Virtual Machine : Java 1.6.0_17-b04 with Sun Microsystems Inc. Java HotSpot(TM) Client VM mixed mode
  Window System   : The X.Org Foundation (10706000), display :0.0

Fault Count: 1

Abnormal termination:
Abort signal

Register State (from fault):
  EAX = 00000000  EBX = 00001025
  ECX = 00001059  EDX = 00000006
  ESP = b03db450  EBP = b03db468
  ESI = 00000400  EDI = b6be0ff4

  EIP = b78d7430  EFL = 00000202

   CS = 00000073   DS = 0000007b   SS = 0000007b
   ES = 0000007b   FS = 00000000   GS = 00000033


Stack Trace (from fault):
[  0] 0xb7761255     /usr/local/MATLAB/R2011b/bin/glnx86/libmwfl.so+00156245 _ZN2fl4diag15stacktrace_base7captureERKNS0_14thread_contextEj+000169
[  1] 0xb7764175     /usr/local/MATLAB/R2011b/bin/glnx86/libmwfl.so+00168309
[  2] 0xb7764569     /usr/local/MATLAB/R2011b/bin/glnx86/libmwfl.so+00169321 _ZN2fl4diag13terminate_logEPKcRKNS0_14thread_contextEb+000181
[  3] 0xb73df93f    /usr/local/MATLAB/R2011b/bin/glnx86/libmwmcr.so+00399679 _ZN2fl4diag13terminate_logEPKcPK8ucontextb+000096
[  4] 0xb73dc996    /usr/local/MATLAB/R2011b/bin/glnx86/libmwmcr.so+00387478
[  5] 0xb73ddd35    /usr/local/MATLAB/R2011b/bin/glnx86/libmwmcr.so+00392501
[  6] 0xb73deab6    /usr/local/MATLAB/R2011b/bin/glnx86/libmwmcr.so+00395958
[  7] 0xb78d7410 __kernel_rt_sigreturn+000000
[  8] 0xb78d7430 __kernel_vsyscall+000016
[  9] 0xb6ab5651                       /lib/tls/i686/cmov/libc.so.6+00173649 gsignal+000081
[ 10] 0xb6ab8a82                       /lib/tls/i686/cmov/libc.so.6+00187010 abort+000386

Originally posted by Erik_S on ROS Answers with karma: 103 on 2012-01-06

Post score: 2


Original comments

Comment by Erik_S on 2012-01-07:
So i did not see the possibility to use these, but please correct me if i'm wrong. I'd love to know how to use the ipc-brigde in simulink.

Therefore i was trying to use the roscpp functions myself.(Not working in simulink is not option unfortunately.)

Comment by Erik_S on 2012-01-07:
I've used IPC brigde in a commmon M-file. However you can't use mexglx functions in s-files, which are written in c(++) (i do not know how, haven't come across it).

The architecture/lay-out of these s-files makes it very hard to use the ipc-bridge files, for you can't create you own threads in them

Comment by mjcarroll on 2012-01-06:
Are you using the MATLAB/ROS IPC bridge? https://alliance.seas.upenn.edu/~meam620/wiki/index.php?n=Roslab.IpcBridge

Comment by Arkapravo on 2012-01-06:
@Erik_S WOW ! no solution here, but I follow your project in eagerness

$\endgroup$

3 Answers 3

0
$\begingroup$

Rosanswers logo

The problem seemingly lies in the fact that MATLAB 2011b has his own version of libboost, version 1.44.0. ROS makes use of version libboost 1.40.0, matlab compiles against its own (1.44.0) libboost libraries, therefore causing errors.

The program works fine in MATLAB 2011a which uses libboost 1.40.0.


Originally posted by Erik_S with karma: 103 on 2012-01-09

This answer was ACCEPTED on the original site

Post score: 3


Original comments

Comment by rado0x54 on 2012-01-16:
Still, thanks a lot!

Comment by Erik_S on 2012-01-16:
in ubuntu cd /bin/glnx86 ls -l libboo* then it should show you all the libboost entries. 2011a is comes with 1.40 and 2011b comes with 1.44.. EDIT just noticed you already found out.

Comment by rado0x54 on 2012-01-15:
@Erik_S Hey, how did you find out what MATLAB ships with what boost version? Thanks!

$\endgroup$
0
$\begingroup$

Rosanswers logo

Got it working with MATLAB 2011b. We had to recompile ros with boost Version 1_44. Thanks for the tip Erik_S!

I actually just took the 1_44 headers from the website and used the Matlab binaries for ros compilation. worked nicely.

image description


Originally posted by rado0x54 with karma: 191 on 2012-01-13

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by rado0x54 on 2012-01-16:
Yeah, everything works nicely, even subscriptions or Timer Callbacks. I don't completely understand why callback definitions should not be allowed. Just start an Asyncspinner with a few threads (otherwise the CallbackQueue will never be processes). I can post a short example tomorrow.

Comment by Erik_S on 2012-01-16:
Did something simular.Are you also running an S-function subscriber? I've succeeded in the publishing part. However subscribing seems to always involve starting a callback function, which matlab does not allow. EDIT - NVM it does allow static voids. Ill keep you posted

Comment by Enrique Fernandez on 2013-03-27:
Hi rado0x54 Are you using a shared C++ library? I'm trying to create a ROS C++ node that uses (and links) to a shared C++ MATLAB R2012b Library compiled with mcc, but I'm having issues.Are there any more specific instructions for compiling ROS fuerte with MATLAB's boost headers and binaries?Thanks!

Comment by ROSLearner on 2013-11-17:
Hi...I am trying to interface Simulink with ROS from Ubuntu 12.04. I am following your post but I am not sure how did you compile your test_s_fun.cpp using the mex command in matlab. I always get an error which says 'ros/ros.h' is not a file or directory. How did you include ROS libraries?? I tried adding the include path under File->SetPath in matlab. But this doesn't work either....It will be very helpful if you can provide the basic steps to achieve this.

$\endgroup$
0
$\begingroup$

Rosanswers logo

Here my dummy s-function. Note, this was modified from the c-function template. I'm new to the whole s-functions and it would probably be nicer to switch to the cpp template. You will have to comment everything that has to do with telekyb::Timer or telekyb::Time. This is not necessary for the ros stuff to work.

Btw, ROS_INFO Macro does not output to MATLAB (should work in rxconsole), but cout does.

#define S_FUNCTION_NAME  test_s_fun
#define S_FUNCTION_LEVEL 2


#include "simstruc.h"


//#include <string>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>

// Telekyb Timer
#include <telekyb_base/Time.hpp>
using namespace telekyb;



/*====================*
 * S-function methods *
 *====================*/

/* Function: mdlInitializeSizes ===============================================
 * Abstract:
 *    The sizes information is used by Simulink to determine the S-function
 *    block's characteristics (number of inputs, outputs, states, etc.).
 */
static void mdlInitializeSizes(SimStruct *S)
{
    /* See sfuntmpl_doc.c for more details on the macros below */

    ssSetNumSFcnParams(S, 0);  /* Number of expected parameters */
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
        /* Return if number of expected != number of actual parameters */
        return;
    }

    ssSetNumContStates(S, 0);
    ssSetNumDiscStates(S, 0);

    if (!ssSetNumInputPorts(S, 1)) return;
    ssSetInputPortWidth(S, 0, 1);
    ssSetInputPortRequiredContiguous(S, 0, true); /*direct input signal access*/
    /*
     * Set direct feedthrough flag (1=yes, 0=no).
     * A port has direct feedthrough if the input is used in either
     * the mdlOutputs or mdlGetTimeOfNextVarHit functions.
     * See matlabroot/simulink/src/sfuntmpl_directfeed.txt.
     */
    ssSetInputPortDirectFeedThrough(S, 0, 1);

    if (!ssSetNumOutputPorts(S, 1)) return;
    ssSetOutputPortWidth(S, 0, 1);

    ssSetNumSampleTimes(S, 1);
    ssSetNumRWork(S, 0);
    ssSetNumIWork(S, 0);
    ssSetNumPWork(S, 0);
    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs(S, 0);

    /* Specify the sim state compliance to be same as a built-in block */
    ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);

    ssSetOptions(S, 0);
}



/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *    This function is used to specify the sample time(s) for your
 *    S-function. You must register the same number of sample times as
 *    specified in ssSetNumSampleTimes.
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);

}



#define MDL_INITIALIZE_CONDITIONS   /* Change to #undef to remove function */
#if defined(MDL_INITIALIZE_CONDITIONS)
  /* Function: mdlInitializeConditions ========================================
   * Abstract:
   *    In this function, you should initialize the continuous and discrete
   *    states for your S-function block.  The initial states are placed
   *    in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S).
   *    You can also perform any other initialization activities that your
   *    S-function may require. Note, this routine will be called at the
   *    start of simulation and if it is present in an enabled subsystem
   *    configured to reset states, it will be call when the enabled subsystem
   *    restarts execution to reset the states.
   */
  static void mdlInitializeConditions(SimStruct *S)
  {
  }
#endif /* MDL_INITIALIZE_CONDITIONS */


void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
    ROS_INFO("I heard: [%s]", msg->data.c_str());
}

double doubleData;

void doubleCallback(const std_msgs::Float64::ConstPtr& msg)
{
   std::cout << "Received Double value! " << msg->data << std::endl;
   doubleData = msg->data;
}

ros::Time start;

void timerCallback(const ros::TimerEvent& event)
{
    //ROS_INFO("Got ROS Timer CB!");
    std::cout << "Got ROS Timer CB! Time: " << (ros::Time::now() - start).toSec() << std::endl;
}


ros::Subscriber sub;
ros::AsyncSpinner* spinner;
ros::Timer timer;

ros::Subscriber doubleSub;
ros::Publisher doublePub;

Timer rtTimer;
Time sleepTime;


#define MDL_START  /* Change to #undef to remove function */
#if defined(MDL_START) 
  /* Function: mdlStart =======================================================
   * Abstract:
   *    This function is called once at start of model execution. If you
   *    have states that should be initialized once, this is the place
   *    to do it.
   */
  static void mdlStart(SimStruct *S)
  {
    rtTimer.reset();
    sleepTime = Time(0.001);
    
    setenv("ROS_MASTER_URI", "http://localhost:11311", 1);
    doubleData = -1.0;
            
            
    int argc = 0;
    char* argv[0];
    ros::init(argc, argv, "MatLabTest", ros::init_options::NoSigintHandler);
    
    //std::cout << "Test" << std::endl;
    //std::cerr << "Testerr" << std::endl;
    
    ros::NodeHandle n;
    
    spinner = new ros::AsyncSpinner(2);
    spinner->start();
    
    
    start = ros::Time::now();
    
    doublePub = n.advertise<std_msgs::Float64>("testDoublePub", 10);
    
    sub = n.subscribe("chatter", 1000, chatterCallback);
    doubleSub = n.subscribe("testDoubleSub", 1000, doubleCallback);
    //timer = n.createTimer(ros::Duration(0.5), timerCallback);
    
  }
#endif /*  MDL_START */



/* Function: mdlOutputs =======================================================
 * Abstract:
 *    In this function, you compute the outputs of your S-function
 *    block.
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{    
    const real_T *u = (const real_T*) ssGetInputPortSignal(S,0);
    
    std_msgs::Float64 msg;
    msg.data = u[0];
    doublePub.publish(msg);
    
    real_T       *y = (real_T*)ssGetOutputPortSignal(S,0);
    
   // y[0] = 2*u[0];
    y[0] = doubleData;
    
    
    Time passed = rtTimer.getElapsed();
    Time actualSleepTime = sleepTime - passed;
    actualSleepTime.sleep();
    rtTimer.reset();
    //std::cout << "Slept: " << actualSleepTime.dSecToString() << std::endl;
}



#define MDL_UPDATE  /* Change to #undef to remove function */
#if defined(MDL_UPDATE)
  /* Function: mdlUpdate ======================================================
   * Abstract:
   *    This function is called once for every major integration time step.
   *    Discrete states are typically updated here, but this function is useful
   *    for performing any tasks that should only take place once per
   *    integration step.
   */
  static void mdlUpdate(SimStruct *S, int_T tid)
  {
  }
#endif /* MDL_UPDATE */



#define MDL_DERIVATIVES  /* Change to #undef to remove function */
#if defined(MDL_DERIVATIVES)
  /* Function: mdlDerivatives =================================================
   * Abstract:
   *    In this function, you compute the S-function block's derivatives.
   *    The derivatives are placed in the derivative vector, ssGetdX(S).
   */
  static void mdlDerivatives(SimStruct *S)
  {
  }
#endif /* MDL_DERIVATIVES */



/* Function: mdlTerminate =====================================================
 * Abstract:
 *    In this function, you should perform any actions that are necessary
 *    at the termination of a simulation.  For example, if memory was
 *    allocated in mdlStart, this is the place to free it.
 */
static void mdlTerminate(SimStruct *S)
{
    delete spinner;
}


/*======================================================*
 * See sfuntmpl_doc.c for the optional S-function methods *
 *======================================================*/

/*=============================*
 * Required S-function trailer *
 *=============================*/

#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif

Originally posted by rado0x54 with karma: 191 on 2012-01-16

This answer was NOT ACCEPTED on the original site

Post score: 2

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.