// //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 }