//
//Generate image message
//
void generate_image_message(sensor_msgs::Image& image_msg)
{
int size[2], data_len;
float* image_buf = simGetVisionSensorImage(camera_handle);
simGetVisionSensorResolution(camera_handle, size);
image_msg.encoding = sensor_msgs::image_encodings::RGB8;
image_msg.height = size[1];
image_msg.width = size[0];
image_msg.step = image_msg.width * 3; //image stride in bytes
data_len = image_msg.step * image_msg.height;
image_msg.data.resize(data_len);
image_msg.is_bigendian = 0;
int msg_idx, buf_idx;
for(unsigned int i = 0; i < image_msg.height; i++)
{
for(unsigned int j = 0; j < image_msg.step; j++)
{
msg_idx = (image_msg.height - i - 1) * image_msg.step + j;
buf_idx = i * image_msg.step + j;
image_msg.data[msg_idx] = (unsigned char)(image_buf[buf_idx] * 255);
}
}
simReleaseBuffer((char*)image_buf);
}
void CSubscriberData::setVisionSensorImageCallback(const sensor_msgs::Image::ConstPtr& image)
{
if (_handleGeneralCallback_before())
{
int resol[2];
int result=simGetVisionSensorResolution(auxInt1,resol);
if (result!=-1)
{
if ((image->encoding==sensor_msgs::image_encodings::RGB8)&&(image->step==image->width*3)&&(int(image->height)==resol[1])&&(int(image->width)==resol[0]))
{
float* image_buff=new float[3*resol[0]*resol[1]];
for(unsigned int i=0;i<image->height;i++)
{
int msg_idx=(image->height-i-1)*image->step;
int buf_idx=i*image->step;
for(unsigned int j=0;j<image->step;j++)
image_buff[buf_idx+j]=float(image->data[msg_idx+j])/255.0f;
}
simSetVisionSensorImage(auxInt1,image_buff);
delete[] image_buff;
}
}
else
shutDownImageSubscriber();
_handleGeneralCallback_after();
}
}
void LUA_GRAB_CALLBACK(SLuaCallBack* p)
{ // the callback function of the new Lua command
int retVal=0; // Means error
if (p->inputArgCount>1)
{ // Ok, we have at least 2 input argument
if ( (p->inputArgTypeAndSize[0*2+0]==sim_lua_arg_int)&&(p->inputArgTypeAndSize[1*2+0]==sim_lua_arg_int) )
{ // Ok, we have (at least) 2 ints as argument
if ( (countCaptureDevices()>p->inputInt[0])&&(p->inputInt[0]>=0)&&(p->inputInt[0]<4) )
{
if (startCountPerDevice[p->inputInt[0]]>0)
{
if (openCaptureDevices[p->inputInt[0]])
{
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
int t=simGetObjectType(p->inputInt[1]);
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
if (t==sim_object_visionsensor_type)
{
int r[2]={0,0};
simGetVisionSensorResolution(p->inputInt[1],r);
if ( (r[0]==captureInfo[p->inputInt[0]].mWidth)&&(r[1]==captureInfo[p->inputInt[0]].mHeight) )
{
float* buff=new float[r[0]*r[1]*3];
for (int i=0;i<r[1];i++)
{
int y0=r[0]*i;
int y1=r[0]*(r[1]-i-1);
for (int j=0;j<r[0];j++)
{ // Info is provided as BGR!! (and not RGB)
buff[3*(y0+j)+0]=float(((BYTE*)captureInfo[p->inputInt[0]].mTargetBuf)[4*(y1+j)+2])/255.0f;
buff[3*(y0+j)+1]=float(((BYTE*)captureInfo[p->inputInt[0]].mTargetBuf)[4*(y1+j)+1])/255.0f;
buff[3*(y0+j)+2]=float(((BYTE*)captureInfo[p->inputInt[0]].mTargetBuf)[4*(y1+j)+0])/255.0f;
}
}
simSetVisionSensorImage(p->inputInt[1],buff);
delete[] buff;
retVal=1;
}
else
simSetLastError(LUA_GRAB,"Resolutions do not match."); // output an error
}
else
simSetLastError(LUA_GRAB,"Invalid vision sensor handle."); // output an error
}
else
simSetLastError(LUA_GRAB,"Resolution was not set."); // output an error
}
else
simSetLastError(LUA_GRAB,"simExtCamStart was not called."); // output an error
}
else
simSetLastError(LUA_GRAB,"Wrong index."); // output an error
}
else
simSetLastError(LUA_GRAB,"Wrong argument type/size."); // output an error
}
else
simSetLastError(LUA_GRAB,"Not enough arguments."); // output an error
// Now we prepare the return value:
p->outputArgCount=1; // 1 return value
p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*0+1]=1; // Not used (table size if the return value was a table)
p->outputInt=(simInt*)simCreateBuffer(1*sizeof(retVal)); // 1 int return value
p->outputInt[0]=retVal; // This is the int value we want to return
}
