{ this->camera = std::dynamic_pointer_castsensors::DepthCameraSensor(sensors::SensorManager::Instance()->GetSensor(this->world->GetName() + "::" + this->model->GetScopedName()+ "::kinect")); if(!this->camera) std::cout<<"kinect not found!"<camera->SetUpdateRate(30); }

void MyPlugin::PublishStates()
{
    /*
     * after run 100 times, collect the camera image 
     * then publish the observation
     */
    if(runCNT % 100 == 0)
    {
        this->runCNT = 1;
        this->world->SetPaused(true);

        if(!this->camera->IsActive())
            ROS_ERROR("kinect camera is not active!!! ");
        // publish observation
        const unsigned char* img = this->camera->ImageData();
        my_msgs::ObsData obs;
        // static unsigned char * mem = new unsigned char [640*480*3];
        // memcpy(mem, img, 640*480*3);
        // obs.data.resize(640*480*3);
        // for(unsigned int i=0;i<640*480*3;i++)
        //  obs.data[i] = double(img[i]);
        obs.data.insert(obs.data.end(), img, img+320*240*3);
        obs.shape.resize(2);
        obs.shape[0] = 320;
        obs.shape[1] = 240;
        this->pubObsData.publish(obs);
        // camera test
        // this->camera->SaveFrame("/home/1.jpg");
    }
    else
        this->runCNT ++;
}