add ui/oculus

ros/src/socket/packages/oculus_socket/launch/oculus_sender.launch

@@ -1,3 +1,3 @@
 <launch>
-	 <node pkg="udp" name="udp_sender" type="udp_sender" args="$(arg ip)" output="screen"/>
+	 <node pkg="oculus_socket" name="oculus_sender" type="oculus_sender" args="$(arg ip)" output="screen"/>
 </launch>

ros/src/socket/packages/oculus_socket/nodes/oculus_sender/oculus_sender.cpp

@@ -36,7 +36,7 @@ geometry_msgs::Point prev;
 
 struct timeval t;
 
-static void ndt_pose_Callback(const geometry_msgs::PoseStamped::ConstPtr& input)
+static void current_pose_Callback(const geometry_msgs::PoseStamped::ConstPtr& input)
 {
 
   tf::Quaternion q(input->pose.orientation.x, input->pose.orientation.y, input->pose.orientation.z, input->pose.orientation.w);
@@ -52,9 +52,8 @@ static void ndt_pose_Callback(const geometry_msgs::PoseStamped::ConstPtr& input)
   sendto(sock1, buf1, sizeof(buf1), 0, (struct sockaddr *)&addr1, sizeof(addr1));
 }
 
-static void car_pose_Callback(const visualization_msgs::MarkerArray::ConstPtr& input)
+static void obj_pose_Callback(const visualization_msgs::MarkerArray::ConstPtr& input)
 {
-
   for (std::vector<visualization_msgs::Marker>::const_iterator item = input->markers.begin(); item != input->markers.end(); item++){
 
     tf::Quaternion q(item->pose.orientation.x, item->pose.orientation.y, item->pose.orientation.z, item->pose.orientation.w);
@@ -64,8 +63,16 @@ static void car_pose_Callback(const visualization_msgs::MarkerArray::ConstPtr& i
 
     ROS_INFO("x=[%.6f] y=[%.6f] z=[%.6f]", item->pose.position.x, item->pose.position.y, item->pose.position.z);
     ROS_INFO("roll=[%f] pitch=[%f] yaw=[%f], PI=[%f]", roll/PI*180, pitch/PI*180, yaw/PI*180, PI);
-  gettimeofday(&t,NULL);
-  sprintf(buf1,"%ld%ld OTH %d %f %f %f %f %f %f", t.tv_sec, t.tv_usec , 0, item->pose.position.x, item->pose.position.y, item->pose.position.z, roll/PI*180, pitch/PI*180, yaw/PI*180);
+    gettimeofday(&t,NULL);
+    if(item->color.b == 1.0f){
+      sprintf(buf1,"%ld%ld OTH %d %f %f %f %f %f %f", t.tv_sec, t.tv_usec , 0, item->pose.position.x, item->pose.position.y, item->pose.position.z, roll/PI*180, pitch/PI*180, yaw/PI*180);
+    }
+    else if(item->color.g == 1.0f){
+      sprintf(buf1,"%ld%ld PED %d %f %f %f %f %f %f", t.tv_sec, t.tv_usec , 0, item->pose.position.x, item->pose.position.y, item->pose.position.z, roll/PI*180, pitch/PI*180, yaw/PI*180);
+    }
+    else {
+      sprintf(buf1,"%ld%ld OBJ %d %f %f %f %f %f %f", t.tv_sec, t.tv_usec , 0, item->pose.position.x, item->pose.position.y, item->pose.position.z, roll/PI*180, pitch/PI*180, yaw/PI*180);
+    }
 
     sendto(sock1, buf1, sizeof(buf1), 0, (struct sockaddr *)&addr1, sizeof(addr1));
   }
@@ -99,9 +106,10 @@ int main (int argc, char **argv)
   ros::init(argc,argv,"udp_sender");
   ros::NodeHandle n;
 
-  ros::Subscriber ndt_pose_sub = n.subscribe("ndt_pose",1000,ndt_pose_Callback);
+  ros::Subscriber current_pose_sub = n.subscribe("current_pose",1000,current_pose_Callback);
   //  ros::Subscriber vscan_sub = n.subscribe("vscan_points", 100, vscan_Callback);
-  ros::Subscriber car_pose_sub = n.subscribe("car_pose",1000,car_pose_Callback);
+  ros::Subscriber obj_car_sub = n.subscribe("obj_car/obj_pose",1000,obj_pose_Callback);
+  ros::Subscriber obj_person_sub = n.subscribe("obj_person/obj_pose",1000,obj_pose_Callback);
 
 
   sock1 = socket(AF_INET, SOCK_DGRAM, 0);

ui/oculus/ofw/Makefile

@@ -0,0 +1,13 @@
+# Attempt to load a config.make file.
+# If none is found, project defaults in config.project.make will be used.
+ifneq ($(wildcard config.make),)
+	include config.make
+endif
+
+# make sure the the OF_ROOT location is defined
+ifndef OF_ROOT
+    OF_ROOT=../../..
+endif
+
+# call the project makefile!
+include $(OF_ROOT)/libs/openFrameworksCompiled/project/makefileCommon/compile.project.mk

ui/oculus/ofw/README.txt

@@ -0,0 +1,42 @@
+＊環境構築
+ -Linux
+　・Ubuntu 14.04
+
+　-Oculus
+　・Oculus DK2
+　・SDK ovr_sdk_linux_0.4.4
+
+　-openFrameworks
+　・バージョン of_v0.8.4_linux64_release
+
+　・手順
+	1.openFrameworks  のサイト(http://openframeworks.jp/)より code::blocks(64 bit) をダウンロード
+	2.IDE setup guide の手順に従いセットアップ
+	3.openFrameworks の addons より ofxOculusDK2 (https://github.com/sphaero/ofxOculusDK2/tree/linux_0.4.4)をダウンロード
+	4.of_v0.8.4_linux64_release の addons ディレクトリ以下に ofxOculusDK2 を置く
+	5.of_v0.8.4_linux64_release の apps ディレクトリ以下の projectGenerator よりプロジェクトを作成する。(プロジェクト名称は任意)
+	6.作成したプロジェクトの bin/data ディレクトリ, src ディレクトリ, Makefile, addons.make, config.make を このディレクトリ(Autoware/ui/oculus/ofw/) のものに置き換える。
+
+＊使用方法
+
+　・ROS側
+	1.oculus_sender の起動(runtime_maneger の Oculus_Rift ボタン)
+
+	↓ runtime_maneger を使用しない場合 (現時点ではこちらのみ？) ↓
+	2.端末上で以下のコマンド入力 Oculus PC の ipアドレス を引数にとる
+	  $ roslaunch oculus oculus_sender.launch ip:= "xxx.xxx.xxx.xxx"
+
+　・ofw側
+	1.ソースコード内、各mapのディレクトリパスの変更
+	　-pointcloud_map.hの変更
+	　　　#define PCD_DIR を 読み込みたいディレクトリのパスに設定
+	　　　#define STATIC_DIR を 読み込みたいディレクトリのパスに設定
+	　-testApp.hの変更
+	　　　#define VECTOR を 読み込みたいディレクトリのパスに設定
+	2.プロジェクトの起動
+
+　・Oculus SDK 0.4.4 
+	1.ofw の実行の前に Oculus SDK 内で
+	　　　　% ./oculusd 
+	　を実行する必要がある。
+

ui/oculus/ofw/addons.make

@@ -0,0 +1,3 @@
+ofxNetwork
+ofxAssimpModelLoader
+ofxOculusDK2

ui/oculus/ofw/config.make

@@ -0,0 +1,142 @@
+################################################################################
+# CONFIGURE PROJECT MAKEFILE (optional)
+#   This file is where we make project specific configurations.
+################################################################################
+
+################################################################################
+# OF ROOT
+#   The location of your root openFrameworks installation
+#       (default) OF_ROOT = ../../.. 
+################################################################################
+# OF_ROOT = ../../..
+
+################################################################################
+# PROJECT ROOT
+#   The location of the project - a starting place for searching for files
+#       (default) PROJECT_ROOT = . (this directory)
+#    
+################################################################################
+# PROJECT_ROOT = .
+
+################################################################################
+# PROJECT SPECIFIC CHECKS
+#   This is a project defined section to create internal makefile flags to 
+#   conditionally enable or disable the addition of various features within 
+#   this makefile.  For instance, if you want to make changes based on whether
+#   GTK is installed, one might test that here and create a variable to check. 
+################################################################################
+# None
+
+################################################################################
+# PROJECT EXTERNAL SOURCE PATHS
+#   These are fully qualified paths that are not within the PROJECT_ROOT folder.
+#   Like source folders in the PROJECT_ROOT, these paths are subject to 
+#   exlclusion via the PROJECT_EXLCUSIONS list.
+#
+#     (default) PROJECT_EXTERNAL_SOURCE_PATHS = (blank) 
+#
+#   Note: Leave a leading space when adding list items with the += operator
+################################################################################
+# PROJECT_EXTERNAL_SOURCE_PATHS = 
+
+################################################################################
+# PROJECT EXCLUSIONS
+#   These makefiles assume that all folders in your current project directory 
+#   and any listed in the PROJECT_EXTERNAL_SOURCH_PATHS are are valid locations
+#   to look for source code. The any folders or files that match any of the 
+#   items in the PROJECT_EXCLUSIONS list below will be ignored.
+#
+#   Each item in the PROJECT_EXCLUSIONS list will be treated as a complete 
+#   string unless teh user adds a wildcard (%) operator to match subdirectories.
+#   GNU make only allows one wildcard for matching.  The second wildcard (%) is
+#   treated literally.
+#
+#      (default) PROJECT_EXCLUSIONS = (blank)
+#
+#		Will automatically exclude the following:
+#
+#			$(PROJECT_ROOT)/bin%
+#			$(PROJECT_ROOT)/obj%
+#			$(PROJECT_ROOT)/%.xcodeproj
+#
+#   Note: Leave a leading space when adding list items with the += operator
+################################################################################
+# PROJECT_EXCLUSIONS =
+
+################################################################################
+# PROJECT LINKER FLAGS
+#	These flags will be sent to the linker when compiling the executable.
+#
+#		(default) PROJECT_LDFLAGS = -Wl,-rpath=./libs
+#
+#   Note: Leave a leading space when adding list items with the += operator
+#
+# Currently, shared libraries that are needed are copied to the 
+# $(PROJECT_ROOT)/bin/libs directory.  The following LDFLAGS tell the linker to
+# add a runtime path to search for those shared libraries, since they aren't 
+# incorporated directly into the final executable application binary.
+################################################################################
+# PROJECT_LDFLAGS=-Wl,-rpath=./libs
+PROJECT_LDFLAGS=-lrt
+
+################################################################################
+# PROJECT DEFINES
+#   Create a space-delimited list of DEFINES. The list will be converted into 
+#   CFLAGS with the "-D" flag later in the makefile.
+#
+#		(default) PROJECT_DEFINES = (blank)
+#
+#   Note: Leave a leading space when adding list items with the += operator
+################################################################################
+# PROJECT_DEFINES = 
+
+################################################################################
+# PROJECT CFLAGS
+#   This is a list of fully qualified CFLAGS required when compiling for this 
+#   project.  These CFLAGS will be used IN ADDITION TO the PLATFORM_CFLAGS 
+#   defined in your platform specific core configuration files. These flags are
+#   presented to the compiler BEFORE the PROJECT_OPTIMIZATION_CFLAGS below. 
+#
+#		(default) PROJECT_CFLAGS = (blank)
+#
+#   Note: Before adding PROJECT_CFLAGS, note that the PLATFORM_CFLAGS defined in 
+#   your platform specific configuration file will be applied by default and 
+#   further flags here may not be needed.
+#
+#   Note: Leave a leading space when adding list items with the += operator
+################################################################################
+# PROJECT_CFLAGS = 
+
+################################################################################
+# PROJECT OPTIMIZATION CFLAGS
+#   These are lists of CFLAGS that are target-specific.  While any flags could 
+#   be conditionally added, they are usually limited to optimization flags. 
+#   These flags are added BEFORE the PROJECT_CFLAGS.
+#
+#   PROJECT_OPTIMIZATION_CFLAGS_RELEASE flags are only applied to RELEASE targets.
+#
+#		(default) PROJECT_OPTIMIZATION_CFLAGS_RELEASE = (blank)
+#
+#   PROJECT_OPTIMIZATION_CFLAGS_DEBUG flags are only applied to DEBUG targets.
+#
+#		(default) PROJECT_OPTIMIZATION_CFLAGS_DEBUG = (blank)
+#
+#   Note: Before adding PROJECT_OPTIMIZATION_CFLAGS, please note that the 
+#   PLATFORM_OPTIMIZATION_CFLAGS defined in your platform specific configuration 
+#   file will be applied by default and further optimization flags here may not 
+#   be needed.
+#
+#   Note: Leave a leading space when adding list items with the += operator
+################################################################################
+# PROJECT_OPTIMIZATION_CFLAGS_RELEASE = 
+# PROJECT_OPTIMIZATION_CFLAGS_DEBUG = 
+
+################################################################################
+# PROJECT COMPILERS
+#   Custom compilers can be set for CC and CXX
+#		(default) PROJECT_CXX = (blank)
+#		(default) PROJECT_CC = (blank)
+#   Note: Leave a leading space when adding list items with the += operator
+################################################################################
+# PROJECT_CXX = 
+# PROJECT_CC =