Esempio n. 1
0
 // Update is called once per frame
 void Update()
 {
     if (RBSocket.Instance.IsConnected)
     {
         PoseStamped true_pose = new PoseStamped();
         true_pose.header = header;
         true_pose.pose   = pose;
         true_pose_pub.publish(true_pose);
     }
 }
Esempio n. 2
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;
            }
        }
    }
Esempio n. 3
0
 void footCB(RBS.Messages.geometry_msgs.PoseStamped msg)
 {
     pose = msg.pose;
 }
Esempio n. 4
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;
            }
        }
    }
Esempio n. 5
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;
            }
        }
    }