Example #1
0
    // Update is called once per frame
    void Update()
    {
        // 初期化されていなかったら初期化(unity座標系の真下にロボットがあると仮定)
        if (!initialized)
        {
            // PoseStamped (Header, Pose)
            RBS.Messages.geometry_msgs.PoseStamped msg = new RBS.Messages.geometry_msgs.PoseStamped();
            if (RBSocket.Instance.IsConnected)
            {
                robotPosePub.publish(msg);
                initialized = true;
            }
        }
        // ロボットが検出されていたら
        if (robotAnchorScript.isDetected)
        {
            ///////// ROS のメッセージ送信 /////////
            // PoseStamped (Header, Pose)
            RBS.Messages.geometry_msgs.PoseStamped msg = new RBS.Messages.geometry_msgs.PoseStamped();

            // Header
            RBS.Messages.std_msgs.Header header = new RBS.Messages.std_msgs.Header();
            header.frame_id = "unity";
            msg.header      = header;

            // Pose (position, orientation)
            RBS.Messages.geometry_msgs.Pose       pose        = new RBS.Messages.geometry_msgs.Pose();
            RBS.Messages.geometry_msgs.Point      position    = new RBS.Messages.geometry_msgs.Point();
            RBS.Messages.geometry_msgs.Quaternion orientation = new RBS.Messages.geometry_msgs.Quaternion();
            // position
            position.x    = robotAnchorScript.position.z;
            position.y    = -robotAnchorScript.position.x;
            position.z    = robotAnchorScript.position.y;
            pose.position = position;
            // orientation
            orientation.x    = robotAnchorScript.rotation.z;
            orientation.y    = -robotAnchorScript.rotation.x;
            orientation.z    = robotAnchorScript.rotation.y;
            orientation.w    = -robotAnchorScript.rotation.w;
            pose.orientation = orientation;

            msg.pose = pose;
            // ROSとの接続が確率されていたらPublish
            if (RBSocket.Instance.IsConnected)
            {
                robotPosePub.publish(msg);
                robotAnchorScript.isDetected = false;
            }
        }
    }
Example #2
0
    // パブリッシュの内容 --------------------------------------------
    void PublishRobotPose()
    {
        // DefaultTrackableEventHandlerを参照
        trueRobotPosition = ImageTarget.transform.localPosition;
        trueRobotRotation = ImageTarget.transform.localRotation;

        // GUI
        XZText.text = "X:" + trueRobotPosition.x.ToString("F4") + " Z:" + trueRobotPosition.z.ToString("F4");
        YText.text  = "Y:" + trueRobotRotation.eulerAngles.y;

        // ROSトピックのデータ
        RBS.Messages.std_msgs.Header header = new RBS.Messages.std_msgs.Header();
        header.stamp.secs  = (uint)unity_time;
        header.stamp.nsecs = (uint)unity_time;
        header.frame_id    = "/map";
        header.seq         = 10;

        RBS.Messages.geometry_msgs.Point pos = new RBS.Messages.geometry_msgs.Point
        {
            x = trueRobotPosition.z,
            y = -trueRobotPosition.x,
            z = trueRobotPosition.y
        };
        RBS.Messages.geometry_msgs.Quaternion ori = new RBS.Messages.geometry_msgs.Quaternion
        {
            x = trueRobotRotation.z,
            y = -trueRobotRotation.x,
            z = trueRobotRotation.y,
            w = -trueRobotRotation.w
        };
        RBS.Messages.geometry_msgs.Pose pose = new RBS.Messages.geometry_msgs.Pose
        {
            position    = pos,
            orientation = ori
        };

        RosConnectorGameObject.GetComponent <TruePosePublisher>().SetHeader(header);
        RosConnectorGameObject.GetComponent <TruePosePublisher>().SetPose(pose);
    }
Example #3
0
    void HitTest(ARPoint arPoint)
    {
        List <ARHitTestResult> hitResults = UnityARSessionNativeInterface
                                            .GetARSessionNativeInterface()
                                            .HitTest(arPoint, ARHitTestResultType.ARHitTestResultTypeExistingPlaneUsingExtent);

        // ロボットを検出するオブジェクトと検出したオブジェクトにPrefabを重ねて表示するオブジェクト
        GameObject generateObjectAnchor = GameObject.Find("GenerateObjectAnchor");
        GameObject objectAnchorGO       = generateObjectAnchor.GetComponent <GenerateRobotAnchor>().objectAnchorGO;

        // 平面とあたっている かつ ロボットを検出している場合
        if (hitResults.Count > 0 && objectAnchorGO != null)
        {
            // 1回目のタッチ
            if (!isPlaced)
            {
                // 前回指定したゴール位置を削除
                Destroy(goalPosition);
                Destroy(goalRotation);

                goalPosition = Instantiate(goalPositionPrefab);
                goalPosition.transform.position = UnityARMatrixOps.GetPosition(hitResults[0].worldTransform);
                goalPosition.transform.rotation = UnityARMatrixOps.GetRotation(hitResults[0].worldTransform);
                this.isPlaced = true;
            }
            // 2回目のタッチ
            else
            {
                goalRotation = Instantiate(goalRotationPrefab);
                // 1回目にタッチした位置に対する、2回目にタッチした位置の方向を計算
                Vector3 p1     = goalPosition.transform.position;
                Vector3 p2     = UnityARMatrixOps.GetPosition(hitResults[0].worldTransform);
                Vector3 d      = p2 - p1;
                float   rad    = Mathf.Atan2(d.z, d.x);
                float   degree = rad * Mathf.Rad2Deg;

                // 上で計算した方向に三角を向けて配置
                goalRotation.transform.position         = p1;
                goalRotation.transform.localEulerAngles = new Vector3(0, -degree, 0);

                // Websocketがつながっている
                // Goal位置をPublishする
                if (RBSocket.Instance.IsConnected)
                {
                    //////// ロボット座標系でのゴールの位置・姿勢を計算 ////////
                    // Unity座標系で見たロボットとゴールの相対位置
                    Vector3 robo2Goal = goalPosition.transform.position - objectAnchorGO.transform.position;
                    float   dx        = robo2Goal.x;
                    float   dz        = robo2Goal.z;
                    // ロボット座標系でのゴールの位置
                    UnityEngine.Quaternion robotQua = objectAnchorGO.transform.rotation;
                    float   theta         = -robotQua.eulerAngles.y * Mathf.Deg2Rad; // 左ねじ回転なのでマイナス
                    Vector3 rGoalPosition = new Vector3(dx * Mathf.Cos(-theta) - dz * Mathf.Sin(-theta),
                                                        0,
                                                        dx * Mathf.Sin(-theta) + dz * Mathf.Cos(-theta));
                    // ロボット座標系でのゴールの姿勢
                    UnityEngine.Quaternion goalQua       = goalRotation.transform.rotation;
                    UnityEngine.Quaternion rGoalRotation = goalQua * UnityEngine.Quaternion.Inverse(robotQua);
                    // 垂直z右手座標系に変換
                    UnityEngine.Quaternion rosGoalRotation = UnityEngine.Quaternion.Euler(0,
                                                                                          0,
                                                                                          -rGoalRotation.eulerAngles.y);

                    ///////// ROS のメッセージ送信 /////////
                    // PoseStamped (Header, Pose)
                    RBS.Messages.geometry_msgs.PoseStamped msg = new RBS.Messages.geometry_msgs.PoseStamped();

                    // Header
                    RBS.Messages.std_msgs.Header header = new RBS.Messages.std_msgs.Header();
                    header.frame_id = "base_footprint";
                    msg.header      = header;
                    // msg.header.frame_id = "base_footprint";

                    // Pose
                    RBS.Messages.geometry_msgs.Pose pose = new RBS.Messages.geometry_msgs.Pose();
                    // position
                    RBS.Messages.geometry_msgs.Point position = new RBS.Messages.geometry_msgs.Point();
                    position.x    = rGoalPosition.x;
                    position.y    = rGoalPosition.z;
                    position.z    = 0;
                    pose.position = position;
                    // orientation
                    RBS.Messages.geometry_msgs.Quaternion orientation = new RBS.Messages.geometry_msgs.Quaternion();
                    orientation.x    = rosGoalRotation.x;
                    orientation.y    = rosGoalRotation.y;
                    orientation.z    = rosGoalRotation.z;
                    orientation.w    = rosGoalRotation.w;
                    pose.orientation = orientation;
                    msg.pose         = pose;

                    goalPub.publish(msg);
                }

                this.isPlaced = false;
            }
        }
    }
Example #4
0
    void HitTest(ARPoint arPoint)
    {
        List <ARHitTestResult> hitResults = UnityARSessionNativeInterface
                                            .GetARSessionNativeInterface()
                                            .HitTest(arPoint, ARHitTestResultType.ARHitTestResultTypeExistingPlaneUsingExtent);

        // ロボットを検出するオブジェクトと検出したオブジェクトにPrefabを重ねて表示するオブジェクト
        GameObject generateRobotAnchor = GameObject.Find("GenerateRobotAnchor");
        GameObject robotAnchorGO       = generateRobotAnchor.GetComponent <GenerateRobotAnchor>().objectAnchorGO;

        // 平面とあたっている かつ ロボットを検出している場合
        if (hitResults.Count > 0 && robotAnchorGO != null)
        {
            // 1回目のタッチ
            if (!isPlaced)
            {
                // 前回指定したゴール位置を削除
                Destroy(goalPosition);
                Destroy(goalRotation);

                goalPosition = Instantiate(goalPositionPrefab);
                goalPosition.transform.position = UnityARMatrixOps.GetPosition(hitResults[0].worldTransform);
                goalPosition.transform.rotation = UnityARMatrixOps.GetRotation(hitResults[0].worldTransform);
                this.isPlaced = true;
            }
            // 2回目のタッチ
            else
            {
                goalRotation = Instantiate(goalRotationPrefab);
                // 1回目にタッチした位置に対する、2回目にタッチした位置の方向を計算
                Vector3    goalVec = goalPosition.transform.position;
                Vector3    hitVec  = UnityARMatrixOps.GetPosition(hitResults[0].worldTransform);
                Quaternion goalQua = Quaternion.LookRotation(hitVec - goalVec);
                goalRotation.transform.position = goalVec;
                goalRotation.transform.rotation = goalQua;
                // y軸成分以外の回転を除去
                Vector3 goalEuler = goalQua.eulerAngles;
                goalQua = Quaternion.Euler(0, goalEuler.y, 0);

                // Websocketがつながっている
                // Goal位置をPublishする
                if (RBSocket.Instance.IsConnected)
                {
                    ///////// ROS のメッセージ送信 /////////
                    // PoseStamped (Header, Pose)
                    RBS.Messages.geometry_msgs.PoseStamped msg = new RBS.Messages.geometry_msgs.PoseStamped();

                    // Header
                    RBS.Messages.std_msgs.Header header = new RBS.Messages.std_msgs.Header();
                    //header.frame_id = "base_footprint";
                    header.frame_id = "unity";
                    msg.header      = header;

                    // Pose
                    RBS.Messages.geometry_msgs.Pose pose = new RBS.Messages.geometry_msgs.Pose();
                    // position
                    RBS.Messages.geometry_msgs.Point position = new RBS.Messages.geometry_msgs.Point();
                    position.x    = goalVec.z;
                    position.y    = -goalVec.x;
                    position.z    = 0;
                    pose.position = position;
                    // orientation
                    RBS.Messages.geometry_msgs.Quaternion orientation = new RBS.Messages.geometry_msgs.Quaternion();
                    orientation.x    = goalQua.z;
                    orientation.y    = -goalQua.x;
                    orientation.z    = goalQua.y;
                    orientation.w    = -goalQua.w;
                    pose.orientation = orientation;
                    msg.pose         = pose;

                    goalPub.publish(msg);
                }

                this.isPlaced = false;
            }
        }
    }