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"); }