void publishObs()
{
// camera stuff
RosMessageTypes.Sensor.Image shoulderImage = takeImage(shoulderRenderTexture, shoulderCam);
RosMessageTypes.Sensor.Image gripperImage = takeImage(gripperRenderTexture, gripperCam);
Observation state = new Observation(
GetQuaternionProprioState(),
GetAchievedGoal(), //getCurrentState(),
shoulderImage,
gripperImage);
ros.Send("state", state);
}
// ################################# Getters ############################################
// void getPosOri(ArticulationBody link)
// {
// Transform data = link.transform;
// // //print(data.eulerAngles);
// // print(data.position);
// return
// }
// void getEEinfo()
// {
// }
// ################################# Publishers #################################
RosMessageTypes.Sensor.Image takeImage(RenderTexture renderTexture, Camera cam)
{
cam.targetTexture = renderTexture;
RenderTexture currentRT = RenderTexture.active;
RenderTexture.active = renderTexture;
cam.Render();
Texture2D currentCameraTexture = new Texture2D(renderTexture.width, renderTexture.height);
currentCameraTexture.ReadPixels(new Rect(0, 0, renderTexture.width, renderTexture.height), 0, 0);
currentCameraTexture.Apply();
RenderTexture.active = currentRT;
// Get the raw byte info from the screenshot
byte[] imageBytes = currentCameraTexture.GetRawTextureData();
uint imageHeight = (uint)renderTexture.height;
uint imageWidth = (uint)renderTexture.width;
RosMessageTypes.Sensor.Image rosImage = new RosMessageTypes.Sensor.Image(new RosMessageTypes.Std.Header(), imageWidth, imageHeight, "RGBA", isBigEndian, step, imageBytes);
shoulderCam.targetTexture = null;
return(rosImage);
}
