Skip to content

Commit

Permalink
Merge pull request #90 from robotology-playground/fix/yarpRead
Browse files Browse the repository at this point in the history 
Fix/yarp read
  • Loading branch information
@francesco-romano
francesco-romano committed Apr 8, 2015
2 parents 3123960 + ac92538 commit 0c51bd0
Show file tree
Hide file tree
Showing 3 changed files with 82 additions and 73 deletions.
33 changes: 17 additions & 16 deletions libraries/WBCLibrary.mdl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@ Library {
MdlSubVersion 0
SavedCharacterEncoding "US-ASCII"
LibraryType "BlockLibrary"
LockLinksToLibrary on
ScopeRefreshTime 0.035000
OverrideScopeRefreshTime on
DisableAllScopes off
Expand All @@ -20,9 +21,9 @@ Library {
ModifiedByFormat "%<Auto>"
LastModifiedBy "makaveli"
ModifiedDateFormat "%<Auto>"
LastModifiedDate "Thu Apr 02 11:06:34 2015"
RTWModifiedTimeStamp 349873594
ModelVersionFormat "1.%<AutoIncrement:241>"
LastModifiedDate "Wed Apr 08 11:18:43 2015"
RTWModifiedTimeStamp 350392723
ModelVersionFormat "1.%<AutoIncrement:243>"
ConfigurationManager "none"
SampleTimeColors off
SampleTimeAnnotations off
Expand Down Expand Up @@ -796,7 +797,7 @@ Library {
}
System {
Name "WBCLibrary"
Location [15, 63, 1273, 714]
Location [9, 59, 1267, 710]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
Expand Down Expand Up @@ -2686,8 +2687,8 @@ Library {
MaskVariables "from=@1;to=@2;num=@3;blocking=@4;timestamp=@5;autoconnect=@6;"
MaskTunableValueString "on,on,on,on,on,on"
MaskCallbackString "|||||autoconnect_val = get_param(gcb, 'Autoconnect');\nif(strcmp(autoconnect_val, 'on'))\n "
" set_param(gcb, 'MaskVisibilities',{'on';'off';'on';'on';'on';'on'});\nelse\n set_param(gcb, 'MaskVisibilities',"
"{'off';'on';'on';'on';'on';'on'});\nend"
" set_param(gcb, 'MaskVisibilities',{'on';'off';'on';'on';'on';'on'});\n set_param(gcb, 'to', '''...''');\nelse\n"
" set_param(gcb, 'MaskVisibilities',{'off';'on';'on';'on';'on';'on'});\nend"
MaskEnableString "on,on,on,on,on,on"
MaskVisibilityString "on,off,on,on,on,on"
MaskToolTipString "on,on,on,on,on,on"
Expand Down Expand Up @@ -13247,7 +13248,7 @@ Library {
Ports [0, 5]
Position [25, 116, 95, 184]
ZOrder -5
LibraryVersion "1.240"
LibraryVersion "1.242"
SourceBlock "WBCLibrary/Utilities/yarp read"
SourceType "yarp read"
from "['/' robotName '/left_arm/analog:o/torques']"
Expand All @@ -13264,7 +13265,7 @@ Library {
Ports [0, 5]
Position [25, 201, 95, 269]
ZOrder -5
LibraryVersion "1.240"
LibraryVersion "1.242"
SourceBlock "WBCLibrary/Utilities/yarp read"
SourceType "yarp read"
from "['/' robotName '/right_arm/analog:o/torques']"
Expand All @@ -13281,7 +13282,7 @@ Library {
Ports [0, 6]
Position [25, 286, 95, 354]
ZOrder -5
LibraryVersion "1.240"
LibraryVersion "1.242"
SourceBlock "WBCLibrary/Utilities/yarp read"
SourceType "yarp read"
from "['/' robotName '/left_leg/analog:o/torques']"
Expand All @@ -13298,7 +13299,7 @@ Library {
Ports [0, 6]
Position [25, 371, 95, 439]
ZOrder -5
LibraryVersion "1.240"
LibraryVersion "1.242"
SourceBlock "WBCLibrary/Utilities/yarp read"
SourceType "yarp read"
from "['/' robotName '/right_leg/analog:o/torques']"
Expand All @@ -13315,7 +13316,7 @@ Library {
Ports [0, 3]
Position [25, 26, 95, 94]
ZOrder -5
LibraryVersion "1.240"
LibraryVersion "1.242"
SourceBlock "WBCLibrary/Utilities/yarp read"
SourceType "yarp read"
from "['/' robotName '/torso/analog:o/torques']"
Expand Down Expand Up @@ -15823,7 +15824,7 @@ Library {
ZOrder 41
BackgroundColor "[0.732000, 0.463600, 0.000000]"
ShowName off
LibraryVersion "1.240"
LibraryVersion "1.242"
SourceBlock "WBCLibrary/wholeBodyStates/Joint Velocities"
SourceType "Joint Velocities"
robotName "robotName"
Expand All @@ -15837,7 +15838,7 @@ Library {
Position [65, 93, 140, 137]
ZOrder -1
BackgroundColor "[0.047000, 0.909000, 0.803000]"
LibraryVersion "1.240"
LibraryVersion "1.242"
SourceBlock "WBCLibrary/wholeBodyModel/Jacobians/Left Foot Jacobian"
SourceType "Left Foot Jacobian"
robotName "robotName"
Expand All @@ -15862,7 +15863,7 @@ Library {
Position [70, 28, 140, 72]
ZOrder -2
BackgroundColor "[0.000000, 1.000000, 0.533000]"
LibraryVersion "1.240"
LibraryVersion "1.242"
SourceBlock "WBCLibrary/wholeBodyModel/Jacobians/Right Foot Jacobian"
SourceType "Right Foot Jacobian"
robotName "robotName"
Expand All @@ -15876,7 +15877,7 @@ Library {
Position [180, 68, 290, 112]
ZOrder -14
BackgroundColor "[0.753334, 1.000000, 0.112472]"
LibraryVersion "1.240"
LibraryVersion "1.242"
SourceBlock "WBCLibrary/Utilities/baseVelocity"
SourceType "baseVelocity"
}
Expand Down Expand Up @@ -15994,7 +15995,7 @@ Library {
Position [20, 63, 90, 107]
ZOrder 1
BackgroundColor "[0.375000, 0.187500, 0.000000]"
LibraryVersion "1.240"
LibraryVersion "1.242"
SourceBlock "WBCLibrary/wholeBodyStates/Joint Angles"
SourceType "Joint Angles"
robotName "robotName"
Expand Down
Binary file modified libraries/WBCLibrary.slx
View file
Binary file not shown.
122 changes: 65 additions & 57 deletions libraries/yRead/src/yarpRead.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@
#include "simstruc.h"

#define IS_PARAM_DOUBLE(pVal) (mxIsNumeric(pVal) && !mxIsLogical(pVal) &&\
!mxIsEmpty(pVal) && !mxIsSparse(pVal) && !mxIsComplex(pVal) && mxIsDouble(pVal))
!mxIsEmpty(pVal) && !mxIsSparse(pVal) && !mxIsComplex(pVal) && mxIsDouble(pVal))


// Function: MDL_CHECK_PARAMETERS
Expand Down Expand Up @@ -104,8 +104,8 @@ static void mdlInitializeSizes(SimStruct *S)
ssSetOutputPortWidth (S, i, 1);
ssSetOutputPortDataType(S, i, SS_DOUBLE);
}


ssSetNumSampleTimes(S, 1);

// Reserve place for C++ object
Expand All @@ -120,35 +120,36 @@ static void mdlInitializeSizes(SimStruct *S)
SS_OPTION_WORKS_WITH_CODE_REUSE |
SS_OPTION_EXCEPTION_FREE_CODE |
SS_OPTION_ALLOW_INPUT_SCALAR_EXPANSION |
SS_OPTION_USE_TLC_WITH_ACCELERATOR);
SS_OPTION_USE_TLC_WITH_ACCELERATOR |
SS_OPTION_CALL_TERMINATE_ON_EXIT);

}

#if defined(MATLAB_MEX_FILE)
# define MDL_SET_INPUT_PORT_WIDTH
static void mdlSetInputPortWidth(SimStruct *S, int_T port,
int_T inputPortWidth)
{
ssSetInputPortWidth(S,port,inputPortWidth);
}
static void mdlSetInputPortWidth(SimStruct *S, int_T port,
int_T inputPortWidth)
{
ssSetInputPortWidth(S,port,inputPortWidth);
}
# define MDL_SET_OUTPUT_PORT_WIDTH
static void mdlSetOutputPortWidth(SimStruct *S, int_T port,
int_T outputPortWidth)
int_T outputPortWidth)
{
ssSetInputPortWidth(S,port,outputPortWidth);
}


# define MDL_SET_DEFAULT_PORT_DIMENSION_INFO
/* Function: mdlSetDefaultPortDimensionInfo ===========================================
* Abstract:
* In case no ports were specified, the default is an input port of width 2
* and an output port of width 1.
*/
static void mdlSetDefaultPortDimensionInfo(SimStruct *S)
{
ssSetInputPortWidth(S, 0, 1);
}
/* Function: mdlSetDefaultPortDimensionInfo ===========================================
* Abstract:
* In case no ports were specified, the default is an input port of width 2
* and an output port of width 1.
*/
static void mdlSetDefaultPortDimensionInfo(SimStruct *S)
{
ssSetInputPortWidth(S, 0, 1);
}
#endif

// Function: mdlInitializeSampleTimes =========================================
Expand Down Expand Up @@ -185,60 +186,67 @@ static void mdlStart(SimStruct *S)
cout<<"YARP is running!!\n"<<endl;

int_T buflen, status;
char *String;
char *buffer;

buflen = mxGetN((ssGetSFcnParam(S, PARAM_IDX_1)))*sizeof(mxChar)+1;
String = static_cast<char*>(mxMalloc(buflen));
status = mxGetString((ssGetSFcnParam(S, PARAM_IDX_1)),String,buflen);
buflen = (1 + mxGetN(ssGetSFcnParam(S, PARAM_IDX_1))) * sizeof(mxChar);
buffer = static_cast<char*>(mxMalloc(buflen));
status = mxGetString((ssGetSFcnParam(S, PARAM_IDX_1)), buffer, buflen);
if (status) {
ssSetErrorStatus(S,"Cannot retrieve string from parameter 1!! \n");
ssSetErrorStatus(S,"Cannot retrieve string from parameter 1 (from)");
return;
}

char *port_name = String; //FROM port name

buflen = mxGetN((ssGetSFcnParam(S, PARAM_IDX_2)))*sizeof(mxChar)+1;
String = static_cast<char*>(mxMalloc(buflen));
status = mxGetString((ssGetSFcnParam(S, PARAM_IDX_2)),String,buflen);
char *port_name = buffer; //FROM port name

buflen = (1 + mxGetN(ssGetSFcnParam(S, PARAM_IDX_2))) * sizeof(mxChar);
buffer = static_cast<char*>(mxMalloc(buflen));
status = mxGetString((ssGetSFcnParam(S, PARAM_IDX_2)), buffer, buflen);
if (status) {
ssSetErrorStatus(S,"Cannot retrieve string from parameter 2!! \n");
ssSetErrorStatus(S,"Cannot retrieve string from parameter 2 (to)");
return;
}

char *toPort_name = String;
char *toPort_name = buffer;

// ######## CHECKING INPUT PARAMETERS ############

BufferedPort<Vector> *toPort;
toPort = new BufferedPort<Vector>;
toPort->open(toPort_name);
//allocate memory using matlab memory management
toPort = new BufferedPort<Vector>();
ssGetPWork(S)[0] = toPort;

if (!toPort || !toPort->open(toPort_name)) {
ssSetErrorStatus(S,"Error while opening yarp port");
return;
}
ConstString toPortName = toPort->getName();
cout<<"[From] Port name will be: "<<port_name<<endl;
cout<<"[To] Port name will be: "<<toPort->getName()<<endl;
cout<<"[To] Port name will be: "<<toPortName<<endl;

ssGetPWork(S)[0] = toPort;

int_T blocking = mxGetScalar(ssGetSFcnParam(S,PARAM_IDX_4));
cout << "Blocking? : " << blocking << endl;
ssGetIWork(S)[0] = blocking;

int_T timestamp = mxGetScalar(ssGetSFcnParam(S,PARAM_IDX_5));
cout << "Timestamp? : " << timestamp << endl;
ssGetIWork(S)[1] = timestamp;

int_T autoconnect = mxGetScalar(ssGetSFcnParam(S,PARAM_IDX_6));
cout << "Autoconnect? : " << autoconnect << endl;
ssGetIWork(S)[2] = autoconnect;

if (autoconnect) {
fprintf(stderr, "Connecting '%s' to '%s' \n", port_name, toPortName.c_str());
fprintf(stderr, "Connecting '%s' to '%s'\n", port_name, toPortName.c_str());
if(!Network::connect(port_name, toPortName)) {
printf("WBI-Toolbox failed connecting %s to %s \n", port_name, toPortName.c_str());
printf("WBI-Toolbox failed connecting %s to %s\n", port_name, toPortName.c_str());
ssSetErrorStatus(S,"ERROR connecting ports!");
return;
}
}

// fprintf(stderr,"Result %d\n", port_name, toPortName.c_str(), Network::connect(port_name,toPortName));
mxFree(toPort_name);
mxFree(port_name);

// fprintf(stderr,"Result %d\n", port_name, toPortName.c_str(), Network::connect(port_name,toPortName));
}

// Function: mdlOutputs =======================================================
Expand All @@ -255,13 +263,13 @@ static void mdlOutputs(SimStruct *S, int_T tid)
if (TIMESTAMP) {
yarp::os::Stamp timestamp;
toPort->getEnvelope(timestamp);

int timestamp_index = SIZE_READING_PORT;
real_T *pY1 = (real_T *) ssGetOutputPortSignal(S, timestamp_index);
pY1[0] = (real_T)(timestamp.getCount());
pY1[1] = (real_T)(timestamp.getTime());
}

for (int i = 0; i < SIZE_READING_PORT; i++)
{
real_T *pY = (real_T *)ssGetOutputPortSignal(S,i);
Expand All @@ -274,23 +282,23 @@ static void mdlOutputs(SimStruct *S, int_T tid)
}
}
}
// else {
// fprintf(stderr,"Cannot read port %s\n", toPort->getName().c_str());
// }
// else {
// fprintf(stderr,"Cannot read port %s\n", toPort->getName().c_str());
// }
}

static void mdlTerminate(SimStruct *S)
{
// IF YOU FORGET TO DESTROY OBJECTS OR DEALLOCATE MEMORY, MATLAB WILL CRASH.
// Retrieve and destroy C++ object
BufferedPort<Vector> *toPort = static_cast<BufferedPort<Vector>*>(ssGetPWork(S)[0]);
toPort->close();

if(ssGetPWork(S) != NULL){
ssSetPWorkValue(S,0,NULL);
}

Network::fini();
if (ssGetPWork(S)) { //This is not created in compilation
BufferedPort<Vector> *toPort = static_cast<BufferedPort<Vector>*>(ssGetPWork(S)[0]);
if (toPort) {
toPort->close();
delete toPort; //remove port
}
Network::fini();
}
fprintf(stderr,"Everything was closed correctly\n");
}

Expand Down

0 comments on commit 0c51bd0

Please sign in to comment.