シリアル番号を文字列で受け取るように変更。テスト時の座標系の切り替えと、Jointテストを追加

gpsnmeajp · Nov 22, 2020 · 5665afa · 5665afa
1 parent e3b5961
commit 5665afa
Show file tree

Hide file tree

Showing 4 changed files with 60 additions and 24 deletions.
diff --git a/vmt_driver/CommunicationManager.cpp b/vmt_driver/CommunicationManager.cpp
@@ -30,7 +30,7 @@ SOFTWARE.
 
 namespace VMTDriver {
 	//別スレッド
-	void OSCReceiver::SetPose(bool roomToDriver,int idx, int enable, double x, double y, double z, double qx, double qy, double qz, double qw, double timeoffset, int root_sn)
+	void OSCReceiver::SetPose(bool roomToDriver,int idx, int enable, double x, double y, double z, double qx, double qy, double qz, double qw, double timeoffset, const char* root_sn)
 	{
 		DriverPose_t pose{ 0 };
 		pose.poseTimeOffset = timeoffset;
@@ -61,7 +61,7 @@ namespace VMTDriver {
 		pose.qRotation.z = qz;
 		pose.qRotation.w = qw;
 
-		if (root_sn == 0) {
+		if (root_sn == nullptr) {
 			//ワールド・ドライバ変換行列を設定
 			Eigen::Translation3d pos(RoomToDriverAffin.translation());
 			Eigen::Quaterniond rot(RoomToDriverAffin.rotation());
@@ -91,12 +91,6 @@ namespace VMTDriver {
 			IVRProperties* props = VRPropertiesRaw();
 			CVRPropertyHelpers* helper = VRProperties();
 
-			// デバイスを特定するシリアルナンバー
-			char sn[16];
-			sprintf(sn, "LHR-%8x", root_sn);
-			// TODO: ↑この書式文字列は特定のパターンで動作しない可能性がある。可能であればOSC受信時点で文字列に変えたい
-
-
 			bool deviceFound = false;
 
 			for (int i = 0; i < 64; i++) {
@@ -106,9 +100,9 @@ namespace VMTDriver {
 				PropertyContainerHandle_t h = props->TrackedDeviceToPropertyContainer(i);
 				string SerialNumber = helper->GetStringProperty(h, ETrackedDeviceProperty::Prop_SerialNumber_String);
 
-				if (SerialNumber.compare(sn) != 0) continue;
+				if (SerialNumber.compare(root_sn) != 0) continue;
 
-				pose.result = p->eTrackingResult;
+				pose.result = (ETrackingResult)(p->eTrackingResult);
 
 				if (p->eTrackingResult == ETrackingResult::TrackingResult_Running_OK) {
 					float* m = (float*)p->mDeviceToAbsoluteTracking.m;
@@ -238,19 +232,21 @@ namespace VMTDriver {
 			}
 			else if (adr == "/VMT/Joint/Unity")
 			{
-				int root_sn, idx, enable;
+				int idx, enable;
 				float timeoffset;
 				float x, y, z, qx, qy, qz, qw;
+				const char* root_sn = nullptr;
 				osc::ReceivedMessageArgumentStream args = m.ArgumentStream();
 				args >> idx >> enable >> timeoffset >> x >> y >> z >> qx >> qy >> qz >> qw >> root_sn >> osc::EndMessage;
 
 				SetPose(false, idx, enable, x, y, -z, qx, qy, -qz, -qw, timeoffset, root_sn);
 			}
 			else if (adr == "/VMT/Joint/Driver")
 			{
-				int root_sn, idx, enable;
+				int idx, enable;
 				float timeoffset;
 				float x, y, z, qx, qy, qz, qw;
+				const char* root_sn = nullptr;
 				osc::ReceivedMessageArgumentStream args = m.ArgumentStream();
 				args >> idx >> enable >> timeoffset >> x >> y >> z >> qx >> qy >> qz >> qw >> root_sn >> osc::EndMessage;
 

diff --git a/vmt_driver/CommunicationManager.h b/vmt_driver/CommunicationManager.h
@@ -25,11 +25,11 @@ SOFTWARE.
 #include "dllmain.h"
 
 namespace VMTDriver {
-	const string Version = "VMT_003";
+	const string Version = "VMT_004";
 
 	class OSCReceiver : public osc::OscPacketListener {
 	private:
-		void SetPose(bool roomToDriver, int idx, int enable, double x, double y, double z, double qx, double qy, double qz, double qw, double timeoffset, int root_sn = 0);
+		void SetPose(bool roomToDriver, int idx, int enable, double x, double y, double z, double qx, double qy, double qz, double qw, double timeoffset, const char* root_sn = nullptr);
 		virtual void ProcessMessage(const osc::ReceivedMessage& m, const IpEndpointName& remoteEndpoint);
 	public:
 		static void OSCReceiver::SendLog(int stat, string msg);

diff --git a/vmt_manager/MainWindow.xaml b/vmt_manager/MainWindow.xaml
@@ -4,8 +4,8 @@
         xmlns:d="http://schemas.microsoft.com/expression/blend/2008"
         xmlns:mc="http://schemas.openxmlformats.org/markup-compatibility/2006"
         xmlns:local="clr-namespace:vmt_manager"
-		Width="320"
-		Height="240"
+		Width="350"
+		Height="260"
         mc:Ignorable="d"
         Title="VMT Manager" Loaded="Window_Loaded" Closed="Window_Closed" ResizeMode="NoResize">
 	<TabControl>
@@ -54,14 +54,21 @@
 						<CheckBox Content="Temporary" DockPanel.Dock="Top" Name="SetRoomMatrixTemporaryCheckBox"></CheckBox>
 						<Button Content="Set RoomMatrix" DockPanel.Dock="Top" Name="SetRoomMatrixButtonName" Click="SetRoomMatrixButton" Margin="1"/>
 						<Button Content="Reset RoomMatrix" DockPanel.Dock="Top" Click="ResetRoomMatrixButton" Margin="1"/>
+						<TextBlock Text="Position Coordinate" DockPanel.Dock="Top" Margin="1"/>
+						<ComboBox DockPanel.Dock="Top" Name="CoordinateCombo" SelectedIndex="0">
+							<ComboBoxItem>Driver Coordinate</ComboBoxItem>
+							<ComboBoxItem>Unity Coordinate</ComboBoxItem>
+						</ComboBox>
 					</DockPanel>
 					<DockPanel Grid.Column="1" Margin="2" LastChildFill="False">
 						<TextBlock Text="VMT_0 Room Position" DockPanel.Dock="Top" Margin="1"/>
 						<TextBox Name="CheckPositionTextBox" IsEnabled="False" IsReadOnly="True" Text="-,-,-" DockPanel.Dock="Top" Margin="1"/>
 						<TextBlock Text="VMT_0 Raw Position" DockPanel.Dock="Top" Margin="1"/>
 						<TextBox Name="CheckPositionRawTextBox" IsEnabled="False" IsReadOnly="True" Text="-,-,-" DockPanel.Dock="Top" Margin="1"/>
 						<Button Content="Check VMT__0 Position &#xA;(Try set RoomPos=[1,1,1] )" DockPanel.Dock="Top" Click="CheckPositionButton" Margin="1"/>
-						<TextBlock DockPanel.Dock="Top"/>
+						<TextBlock Text="VMT_0 Joint Serial No" DockPanel.Dock="Top" Margin="1"/>
+						<TextBox Name="JointSerialNoTextBox" Text="LHR-xxxxxxxxx" DockPanel.Dock="Top" Margin="1"/>
+						<Button Content="Check VMT__0 Position &#xA;(Try set JointPos=[0.1,0.1,0.1] )" DockPanel.Dock="Top" Click="CheckJointPositionButton" Margin="1"/>
 					</DockPanel>
 				</Grid>
 			</DockPanel>
@@ -137,11 +144,11 @@
 						<Button Content="Input Reset"  Margin="1" Click="InputResetButton" DockPanel.Dock="Top"/>
 					</DockPanel>
 				</Grid>
-				<Button Content="Haptic Test"  Margin="1" Click="HapticTestButton" DockPanel.Dock="Bottom"/>
-				<DockPanel DockPanel.Dock="Bottom" Margin="1">
+				<DockPanel DockPanel.Dock="Top" Margin="1">
 					<TextBlock VerticalAlignment="Center" Text="Haptic:" DockPanel.Dock="Left" />
 					<TextBox Name="InputVMTHapticTextBox" Text="-" IsReadOnly="True" IsEnabled="False" />
 				</DockPanel>
+				<Button Content="Haptic Test"  Margin="1" Click="HapticTestButton" DockPanel.Dock="Top"/>
 			</DockPanel>
 		</TabItem>
 	</TabControl>

diff --git a/vmt_manager/MainWindow.xaml.cs b/vmt_manager/MainWindow.xaml.cs
@@ -58,7 +58,7 @@ namespace vmt_manager
     /// </summary>
 public partial class MainWindow : Window
     {
-        const string Version = "VMT_003";
+        const string Version = "VMT_004";
         private DispatcherTimer dispatcherTimer;
         Random rnd;
         string title = "";
@@ -271,8 +271,14 @@ private void GenericTimer(object sender, EventArgs e)
 
             var t1 = util.GetTransformBySerialNumber("VMT_0");
             if (t1 != null) {
-                string roomPos = String.Format("{0:0.00}, {1:0.00}, {2:0.00}", t1.position.X, t1.position.Y, t1.position.Z);
-                CheckPositionTextBox.Text = roomPos;
+                string roomPos = string.Format("{0:0.00}, {1:0.00}, {2:0.00}", t1.position.X, t1.position.Y, t1.position.Z);
+
+                //Unity座標系ではZが反転する
+                if (CoordinateCombo.SelectedIndex == 1) {
+                    roomPos = string.Format("{0:0.00}, {1:0.00}, {2:0.00}", t1.position.X, t1.position.Y, -t1.position.Z);
+                }
+
+                    CheckPositionTextBox.Text = roomPos;
                 if (roomPos == "1.00, 1.00, 1.00") {
                     CheckPositionTextBox.Background = new SolidColorBrush(Color.FromRgb(0, 255, 0));
                 }
@@ -282,7 +288,7 @@ private void GenericTimer(object sender, EventArgs e)
             }
             var t2 = util.GetTransformBySerialNumberRaw("VMT_0");
             if (t2 != null) {
-                CheckPositionRawTextBox.Text = String.Format("{0:0.00}, {1:0.00}, {2:0.00}", t2.position.X, t2.position.Y, t2.position.Z);
+                CheckPositionRawTextBox.Text = string.Format("{0:0.00}, {1:0.00}, {2:0.00}", t2.position.X, t2.position.Y, t2.position.Z);
             }
 
             if (aliveCnt > 90)
@@ -354,10 +360,37 @@ private void ResetRoomMatrixButton(object sender, RoutedEventArgs e)
         }
         private void CheckPositionButton(object sender, RoutedEventArgs e)
         {
-            osc.Send(new OscMessage("/VMT/Room/Driver",
+            if (CoordinateCombo.SelectedIndex == 1)
+            {
+                osc.Send(new OscMessage("/VMT/Room/Unity",
                 0, 1, 0f,
                 1f, 1f, 1f,
                 0f, 0f, 0f, 1f));
+            }
+            else {
+                osc.Send(new OscMessage("/VMT/Room/Driver",
+                0, 1, 0f,
+                1f, 1f, 1f,
+                0f, 0f, 0f, 1f));
+            }
+        }
+        private void CheckJointPositionButton(object sender, RoutedEventArgs e)
+        {
+            if (CoordinateCombo.SelectedIndex == 1)
+            {
+                osc.Send(new OscMessage("/VMT/Joint/Unity",
+                    0, 1, 0f,
+                    0.1f, 0.1f, 0.1f,
+                    0f, 0f, 0f, 1f,
+                    JointSerialNoTextBox.Text));
+            }
+            else {
+                osc.Send(new OscMessage("/VMT/Joint/Driver",
+                    0, 1, 0f,
+                    0.1f, 0.1f, 0.1f,
+                    0f, 0f, 0f, 1f,
+                    JointSerialNoTextBox.Text));
+            }
         }
 
         private void InstallButton(object sender, RoutedEventArgs e)