Example #1
0
    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);
    }
Example #2
0
    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);
    }
Example #3
0
    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);
    }
Example #4
0
File: Robot.cs Project: cxdcxd/RRS
 private void Lidar_delegateLidarDataChanged(byte[] buffer)
 {
     publisher_lidar_1.Send(buffer);
 }
Example #5
0
File: Robot.cs Project: cxdcxd/RRS
 private void Color_camera_delegateCameraDataChanged(byte[] buffer)
 {
     publisher_camera_color.Send(buffer);
 }
Example #6
0
    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);
    }
Example #7
0
 private void Segment_camera_delegateCameraDataChanged(byte[] buffer)
 {
     //print("Segment");
     publisher_camera_segment.Send(buffer);
 }
Example #8
0
 private void Depth_camera_delegateCameraDataChanged(byte[] buffer)
 {
     //print("Depth");
     publisher_camera_depth.Send(buffer);
 }
Example #9
0
 private void Imu_delegateIMUDataChanged(byte[] buffer)
 {
     publisher_imu.Send(buffer);
 }
Example #10
0
 private void Lidar_front_delegateLidarDataChanged(byte[] buffer)
 {
     //print("Send Lidar Front");
     publisher_lidar_front.Send(buffer);
 }
Example #11
0
 private void Normal_camera_delegateCameraDataChanged(byte[] buffer)
 {
     publisher_camera_normal.Send(buffer);
 }
Example #12
0
 private void Depth_camera_delegateCameraDataChanged(byte[] buffer)
 {
     publisher_camera_depth_net2.Send(buffer);
 }
Example #13
0
 private void Color_camera_delegateCameraDataChanged(byte[] buffer)
 {
     publisher_camera_rgb_net2.Send(buffer);
     UnityEngine.Debug.Log("net image publisherd");
 }