// 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; } } }
// パブリッシュの内容 -------------------------------------------- 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); }
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; } } }
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; } } }