void Listener::chatterCallback(const sensor_msgs::ImageConstPtr& msg){
//(const std_msgs::String::ConstPtr &msg) {
// ROS_INFO("I heard: [%s]", msg->data.c_str());
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
px = QPixmap::fromImage(cvtCvMat2QImage(cv_ptr->image));
logging.insertRows(0,1);
std::stringstream logging_msg;
logging_msg << "[ INFO] [" << ros::Time::now() << "]: I heard: " << "camera";
QVariant new_row(QString(logging_msg.str().c_str()));
logging.setData(logging.index(0),new_row);
Q_EMIT Update_Image(&px);
}
void Raster::refineRaster(std::size_t scaling)
{
double *new_raster_data(new double[_n_rows*_n_cols*scaling*scaling]);
for (std::size_t row(0); row<_n_rows; row++) {
for (std::size_t col(0); col<_n_cols; col++) {
const std::size_t idx(row*_n_cols+col);
for (std::size_t new_row(row*scaling); new_row<(row+1)*scaling; new_row++) {
const std::size_t idx0(new_row*_n_cols*scaling);
for (std::size_t new_col(col*scaling); new_col<(col+1)*scaling; new_col++) {
new_raster_data[idx0+new_col] = _raster_data[idx];
}
}
}
}
std::swap(_raster_data, new_raster_data);
_cell_size /= scaling;
_n_cols *= scaling;
_n_rows *= scaling;
delete [] new_raster_data;
}
void
printBlock (
sEvent *sp,
FILE *outFile
)
{
char time_str[40];
pwr_tTime event_time;
new_row( outFile);
printMsgInfo(&(sp->Mess.block.Info), outFile);
//EventText
//Status
//TargetIdx
//DetectTime
//AValue
//CtrlLimit
//Hysteres
//Unit
//DValue
//High
col_print(outFile, "\"\"");
col_print(outFile, "%s", sp->Mess.block.EventName);
col_print(outFile, "%u", sp->Mess.block.TargetId.Idx);
event_time = net_NetTimeToTime( &(sp->Mess.block.DetectTime));
time_AtoAscii(&event_time, time_eFormat_ComprDateAndTime, time_str, sizeof(time_str));
col_print(outFile, "%s", time_str);
col_print(outFile,"");
col_print(outFile,"");
col_print(outFile,"");
col_print(outFile,"");
col_print(outFile,"");
col_print(outFile,"");
}
int separa_capacity(int *card,CONSTRAINT **stack)
{
int k,l,nvar,num,index;
double *primal;
double *cvar;
PROT_LEVEL*pro,*pro0;
VARIABLE **xvar;
CONSTRAINT*con;
char *sleep;
double bd[2];
double maxc;
if( *card ==MAX_CUTS_ITER) return(0);
#ifdef STAMP
std::cout << " .. capacity cuts .. ";
#endif
cvar = (double *)malloc( Rncells * sizeof(double) );
xvar = (VARIABLE **)malloc( Rncells * sizeof(VARIABLE *) );
primal = (double *)malloc( Rncells * sizeof(double) );
if( !cvar || !xvar || !primal ){
std::cout << "There is not enough memory for CAPACITY" << std::endl;
CSPexit(EXIT_MEMO); //exit(1);
}
for(k=0;k<Rncells;k++) primal[k] = columns[k].val;
load_network(primal,'C');
num = *card;
sleep = (char *)calloc( nprot_level , sizeof(char) );
l = nprot_level;
while(l--){
pro = prot_level+l;
if( !sleep[l] ){
index = pro->sen->var->index;
/////////// free_col(index,bd);
if( protection_level(pro->sen->var,pro->sense,&nvar,xvar,cvar,primal,'C') < pro->sense * pro->sen->var->nominal + pro->level-ZERO ){
con = capacity_constraint(nvar,xvar,cvar,pro->sen->var,pro->level);
if( con && new_row(con) ){
stack[ num++] = con;
if( num==MAX_CUTS_ITER) break;
}else
remove_row( con );
}
/////////// unfree_col(index,bd);
k = l;
while(k--){
pro0 = prot_level+k;
if( pro0->sense * primal[ pro0->sen->var->index ] > pro0->sense * pro0->sen->var->nominal + pro0->level - ZERO )
sleep[k] = 1;
}
// for introducing an alternative Capacity Cut
// a cell must be choosen to be super-suppressed
index = -1;
maxc = 0.0;
for(k=0;k<nvar;k++)
if( xvar[k]->val>ZERO && cvar[k]>maxc ){
index = xvar[k]->index;
maxc = cvar[k];
}
if( index != -1 ){
free_col(index,bd);
if( protection_level(pro->sen->var,pro->sense,&nvar,xvar,cvar,NULL,'C') < pro->sense * pro->sen->var->nominal + pro->level-ZERO ){
#ifdef STAMP
for(k=0;k<nvar;k++)
if( xvar[k]->index==index ){
std::cout << "ERROR: capacity with imposible cell" << std::endl;
CSPexit(EXIT_ERROR); //exit(1);
}
#endif
con = capacity_constraint(nvar,xvar,cvar,pro->sen->var,pro->level);
if( con && new_row(con) ){
stack[ num++] = con;
if( num==MAX_CUTS_ITER) break;
}else
remove_row( con );
}
unfree_col(index,bd);
}
}
}
free( sleep );
sleep = NULL; /*PWOF*/
num -= *card;
*card += num;
unload_network();
free(primal);
primal = NULL; /*PWOF*/
free(cvar);
cvar = NULL; /*PWOF*/
free(xvar);
xvar = NULL; /*PWOF*/
ccapa += num;
#ifdef STAMP
std::cout << num << std::endl;
#endif
return( num );
}
int separa_bridge(int *card,CONSTRAINT **stack)
{
int k,l,nvar,nvar1,nvar2,num;
double *primal1,*primal2;
double *cvar1,*cvar2,*cvar;
VARIABLE *var,*var0;
VARIABLE **xvar1,**xvar2,**xvar;
CONSTRAINT*con;
double minvalue,maxvalue,value;
char *sleep;
if( *card ==MAX_CUTS_ITER) return(0);
#ifdef STAMP
std::cout << " .. bridge-less cuts .. ";
#endif
cvar = (double *)malloc( Rncells * sizeof(double) );
cvar1 = (double *)malloc( Rncells * sizeof(double) );
cvar2 = (double *)malloc( Rncells * sizeof(double) );
xvar1 = (VARIABLE **)malloc( Rncells * sizeof(VARIABLE *) );
xvar2 = (VARIABLE **)malloc( Rncells * sizeof(VARIABLE *) );
xvar = (VARIABLE **)malloc( Rncells * sizeof(VARIABLE *) );
primal1 = (double *)malloc( Rncells * sizeof(double) );
primal2 = (double *)malloc( Rncells * sizeof(double) );
if( !cvar1 || !cvar2 || !xvar || !primal1 || !primal2 ){
std::cout << "There is not enough memory for CAPACITY" << std::endl;
CSPexit(EXIT_MEMO); //exit(1);
}
for(k=0;k<Rncells;k++) primal2[k] = primal1[k] = columns[k].val;
load_network(primal1,'C');
num = *card;
sleep = (char *)calloc( nsupport , sizeof(char) );
for(k=0;k<nsupport;k++)
if( support[k]->sensitive ) sleep[k]=1;
l = nsupport;
while(l--){
var = support[l];
if( !sleep[l] ){
bounding_1();
maxvalue = protection_level(var,1,&nvar1,xvar1,cvar1,primal1,'C');
minvalue = -protection_level(var,-1,&nvar2,xvar2,cvar2,primal2, 'C');
value = bridge_lhs(nvar1,nvar2,xvar1,xvar2,cvar1,cvar2,&nvar,xvar,cvar);
if( maxvalue-minvalue < ZERO || value < var->val+ZERO-MIN_VIOLA ){
con = bridge_constraint(nvar,xvar,var->index);
if( con && new_row(con) ){
stack[ num++] = con;
if( num==MAX_CUTS_ITER) break;
}else
remove_row( con );
}
k = l;
while(k--){
var0 = support[k];
if( fabs(primal2[var0->index] - primal1[var0->index]) > var0->val-MIN_VIOLA+ZERO )
sleep[k] = 1;
}
}
}
free( sleep );
sleep = NULL; /*PWOF*/
num -= *card;
*card += num;
unload_network();
free(primal1);
primal1 = NULL; /*PWOF*/
free(primal2);
primal2 = NULL; /*PWOF*/
free(xvar);
xvar = NULL; /*PWOF*/
free(xvar1);
xvar1 = NULL; /*PWOF*/
free(xvar2);
xvar2 = NULL; /*PWOF*/
free(cvar);
cvar = NULL; /*PWOF*/
free(cvar1);
cvar1 = NULL; /*PWOF*/
free(cvar2);
cvar2 = NULL; /*PWOF*/
cbrid += num;
#ifdef STAMP
std::cout << num << std::endl;
#endif
return( num );
}
