void Update()
{
if (RBSocket.Instance.IsConnected)
{
RBS.Messages.geometry_msgs.Pose pose = new RBS.Messages.geometry_msgs.Pose();
pose.position.x = cubeObject.transform.position.x;
pose.position.y = cubeObject.transform.position.y;
pose.position.z = cubeObject.transform.position.z;
pub.Publish(pose);
}
}
// 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 footCB(RBS.Messages.geometry_msgs.PoseStamped msg)
{
pose = msg.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;
}
}
}
public void SetPose(Pose data)
{
pose = data;
}