void sendGroundtruth()
{
RRSTransform t = new RRSTransform();
t.position = new RRS.Tools.Protobuf.SVector3();
t.orientation = new RRS.Tools.Protobuf.SVector4();
Vector3 convertp = new Vector3();
convertp = Helper.Unity2Ros(transform.position);
t.position.x = convertp.x;
t.position.y = convertp.y;
t.position.z = convertp.z;
var convertr = Helper.Unity2Ros(transform.rotation);
t.orientation.x = convertr.x;
t.orientation.y = convertr.y;
t.orientation.z = convertr.z;
t.orientation.w = convertr.w;
MemoryStream ms = new MemoryStream();
Serializer.Serialize <RRSTransform>(ms, t);
byte[] data = ms.ToArray();
publisher_groundtruth.Send(data);
}
void sendJointTF()
{
RRSTF tf_msg = new RRSTF();
tf_msg.names = new string[1];
tf_msg.parents = new string[1];
tf_msg.transforms = new RRSTransform[1];
tf_msg.names[0] = Links.base_link.ToString();
tf_msg.parents[0] = Links.odom.ToString();
tf_msg.transforms[0] = getRRSTransform(transform, Links.base_link);
MemoryStream ms = new MemoryStream();
Serializer.Serialize <RRSTF>(ms, tf_msg);
byte[] data = ms.ToArray();
////print("publish tf");
publisher_tf.Send(data);
}
void sendOdometry()
{
RRSOdom t = new RRSOdom();
t.position = new RRS.Tools.Protobuf.SVector3();
t.orientation = new RRS.Tools.Protobuf.SVector4();
var convertp = Helper.Unity2Ros(transform.position);
t.position.x = convertp.x;
t.position.y = convertp.y;
t.position.z = convertp.z;
var convertr = Helper.Unity2Ros(transform.rotation);
t.orientation.x = convertr.x;
t.orientation.y = convertr.y;
t.orientation.z = convertr.z;
t.orientation.w = convertr.w;
t.linear_speed = new RRS.Tools.Protobuf.SVector3();
//var convertlv = Helper.Unity2Ros(imu.linVel);
//t.linear_speed.x = convertlv.x;
//t.linear_speed.y = convertlv.y;
//t.linear_speed.z = convertlv.z;
t.angular_speed = new RRS.Tools.Protobuf.SVector3();
//var convertav = Helper.Unity2Ros(imu.angVel);
//t.angular_speed.x = convertav.x;
//t.angular_speed.y = convertav.y;
//t.angular_speed.z = convertav.z;
MemoryStream ms = new MemoryStream();
Serializer.Serialize <RRSOdom>(ms, t);
byte[] data = ms.ToArray();
publisher_odometry.Send(data);
}
private void Lidar_delegateLidarDataChanged(byte[] buffer)
{
publisher_lidar_1.Send(buffer);
}
private void Color_camera_delegateCameraDataChanged(byte[] buffer)
{
publisher_camera_color.Send(buffer);
}
private void Color_camera_delegateCameraDataChanged(byte[] buffer)
{
publisher_camera_color.Send(buffer);
//Sending Camera Info
RRSCameraInfo info_msg = new RRSCameraInfo();
//width
//800
//height
//600
//[narrow_stereo]
//Camera matrix
//520.907761 0.000000 398.668872
//0.000000 520.193056 298.836832
//0.000000 0.000000 1.000000
//Distortion
//0.006218 -0.004135 0.000217 -0.000706 0.000000
//Rectification
//1.000000 0.000000 0.000000
//0.000000 1.000000 0.000000
//0.000000 0.000000 1.000000
//Projection
//522.240356 0.000000 397.936985 0.000000
//0.000000 521.915222 299.013784 0.000000
//0.000000 0.000000 1.000000 0.000000
info_msg.width = 800;
info_msg.height = 600;
info_msg.P = new float[12];
info_msg.P[0] = 522.240356f;
info_msg.P[1] = 0.000000f;
info_msg.P[2] = 397.936985f;
info_msg.P[3] = 0.000000f;
info_msg.P[4] = 0.000000f;
info_msg.P[5] = 521.915222f;
info_msg.P[6] = 299.013784f;
info_msg.P[7] = 0.000000f;
info_msg.P[8] = 0.000000f;
info_msg.P[9] = 0.000000f;
info_msg.P[10] = 1.000000f;
info_msg.P[11] = 0.000000f;
info_msg.R = new float[9];
info_msg.R[0] = 1.000000f;
info_msg.R[1] = 0.000000f;
info_msg.R[2] = 0.000000f;
info_msg.R[3] = 0.000000f;
info_msg.R[4] = 1.000000f;
info_msg.R[5] = 0.000000f;
info_msg.R[6] = 0.000000f;
info_msg.R[7] = 0.000000f;
info_msg.R[8] = 1.000000f;
info_msg.K = new float[9];
info_msg.K[0] = 520.907761f;
info_msg.K[1] = 0.000000f;
info_msg.K[2] = 398.668872f;
info_msg.K[3] = 0.000000f;
info_msg.K[4] = 520.193056f;
info_msg.K[5] = 298.836832f;
info_msg.K[6] = 0.000000f;
info_msg.K[7] = 0.000000f;
info_msg.K[8] = 1.000000f;
info_msg.D = new float[5];
info_msg.D[0] = 0.006218f;
info_msg.D[1] = -0.004135f;
info_msg.D[2] = 0.000217f;
info_msg.D[3] = -0.000706f;
info_msg.D[4] = 0.000000f;
info_msg.distortion_model = "plumb_bob";
MemoryStream ms = new MemoryStream();
ms = new MemoryStream();
Serializer.Serialize <RRSCameraInfo>(ms, info_msg);
byte[] data = ms.ToArray();
publisher_camera_info.Send(data);
}
private void Segment_camera_delegateCameraDataChanged(byte[] buffer)
{
//print("Segment");
publisher_camera_segment.Send(buffer);
}
private void Depth_camera_delegateCameraDataChanged(byte[] buffer)
{
//print("Depth");
publisher_camera_depth.Send(buffer);
}
private void Imu_delegateIMUDataChanged(byte[] buffer)
{
publisher_imu.Send(buffer);
}
private void Lidar_front_delegateLidarDataChanged(byte[] buffer)
{
//print("Send Lidar Front");
publisher_lidar_front.Send(buffer);
}
private void Normal_camera_delegateCameraDataChanged(byte[] buffer)
{
publisher_camera_normal.Send(buffer);
}
private void Depth_camera_delegateCameraDataChanged(byte[] buffer)
{
publisher_camera_depth_net2.Send(buffer);
}
private void Color_camera_delegateCameraDataChanged(byte[] buffer)
{
publisher_camera_rgb_net2.Send(buffer);
UnityEngine.Debug.Log("net image publisherd");
}