Add type assigment in relay_field.py

start-jsk · Jun 14, 2016 · 0a185c2 · 0a185c2
1 parent 92d05b5
commit 0a185c2
Show file tree

Hide file tree

Showing 8 changed files with 37 additions and 284 deletions.
diff --git a/jsk_mbzirc_tasks/CMakeLists.txt b/jsk_mbzirc_tasks/CMakeLists.txt
@@ -4,10 +4,8 @@ project(jsk_mbzirc_tasks)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp gazebo_ros)
+find_package(catkin REQUIRED COMPONENTS roscpp)
 
-# Depend on system install of Gazebo and SDFormat
-find_package(gazebo REQUIRED)
 
 ###################################
 ## catkin specific configuration ##
@@ -16,28 +14,14 @@ catkin_package(
     CATKIN_DEPENDS
 )
 
-include_directories(include ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
+include_directories(include ${catkin_INCLUDE_DIRS})
 
 add_executable(uav_teleop_keyboard src/uav_teleop_keyboard.cpp)
 target_link_libraries(uav_teleop_keyboard ${catkin_LIBRARIES})
 
-# build plugins
-add_library(mbzirc_gazebo_uav_state_display_plugin src/mbzirc_gazebo_uav_state_display_plugin.cpp)
-target_link_libraries(mbzirc_gazebo_uav_state_display_plugin ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-
-#############
-## Install ##
-#############
-
 #############
 ## Install ##
 #############
-install(TARGETS
-  mbzirc_gazebo_uav_state_display_plugin
-  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  )
-
 install(DIRECTORY launch urdf config
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   USE_SOURCE_PERMISSIONS)

diff --git a/jsk_mbzirc_tasks/config/task3.rviz b/jsk_mbzirc_tasks/config/task3.rviz
@@ -116,7 +116,6 @@ Visualization Manager:
       Size (m): 0.01
       Style: Flat Squares
       Topic: /UAV1/scan
-      Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
@@ -128,7 +127,6 @@ Visualization Manager:
       Overlay Alpha: 0.5
       Queue Size: 2
       Transport Hint: raw
-      Unreliable: false
       Value: true
       Visibility:
         Grid: true
@@ -200,7 +198,7 @@ Visualization Manager:
       Max Color: 255; 255; 255
       Max Intensity: 999999
       Min Color: 0; 0; 0
-      Min Intensity: 0
+      Min Intensity: -6.80389e-27
       Name: UAV2 LaserScan
       Position Transformer: XYZ
       Queue Size: 10
@@ -209,7 +207,6 @@ Visualization Manager:
       Size (m): 0.01
       Style: Flat Squares
       Topic: /UAV2/scan
-      Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
@@ -221,10 +218,10 @@ Visualization Manager:
       Overlay Alpha: 0.5
       Queue Size: 2
       Transport Hint: raw
-      Unreliable: false
       Value: true
       Visibility:
         Grid: true
+        OverlayText: true
         Remaining Time: true
         UAV1 Camera: true
         UAV1 LaserScan: true
@@ -301,7 +298,6 @@ Visualization Manager:
       Size (m): 0.01
       Style: Flat Squares
       Topic: /UAV3/scan
-      Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
@@ -313,10 +309,10 @@ Visualization Manager:
       Overlay Alpha: 0.5
       Queue Size: 2
       Transport Hint: raw
-      Unreliable: false
       Value: true
       Visibility:
         Grid: true
+        OverlayText: true
         Remaining Time: true
         UAV1 Camera: true
         UAV1 LaserScan: true
@@ -358,12 +354,12 @@ Visualization Manager:
       Topic: /UAV1/ground_truth_overlay
       Value: true
       font: DejaVu Sans Mono
-      height: 60
+      height: 30
       left: 10
       line width: 2
       text size: 20
       top: 10
-      width: 330
+      width: 250
     - Background Alpha: 0.8
       Background Color: 0; 0; 0
       Class: jsk_rviz_plugin/OverlayText
@@ -376,12 +372,12 @@ Visualization Manager:
       Topic: /UAV2/ground_truth_overlay
       Value: true
       font: DejaVu Sans Mono
-      height: 60
+      height: 30
       left: 10
       line width: 2
       text size: 20
       top: 80
-      width: 330
+      width: 250
     - Background Alpha: 0.8
       Background Color: 0; 0; 0
       Class: jsk_rviz_plugin/OverlayText
@@ -394,12 +390,12 @@ Visualization Manager:
       Topic: /UAV3/ground_truth_overlay
       Value: true
       font: DejaVu Sans Mono
-      height: 60
+      height: 30
       left: 10
       line width: 2
       text size: 20
       top: 150
-      width: 330
+      width: 250
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

diff --git a/jsk_mbzirc_tasks/include/jsk_mbzirc_tasks/mbzirc_gazebo_uav_state_display_plugin.h b/jsk_mbzirc_tasks/include/jsk_mbzirc_tasks/mbzirc_gazebo_uav_state_display_plugin.h
diff --git a/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_1.launch b/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_1.launch
@@ -35,19 +35,19 @@
     <arg name="z" value="$(arg z)"/>
   </include>
 
-  <!-- workaround until https://github.com/ros/ros_comm/pull/728 get released --> 
+  <!-- workaround until https://github.com/ros/ros_comm/pull/728 get released -->
   <!-- display time in rviz -->
   <node pkg="jsk_mbzirc_tasks" name="remain_time_text_to_overlay" type="relay_field_728.py"
         args="/remaining_time /remaining_time_overlay jsk_rviz_plugins/OverlayText
-              'text: m.data' --wait-for-start" />
+              'text: m.data' --wait-for-start" /> 
   <!-- display uav state in rviz -->
   <node pkg="jsk_mbzirc_tasks" name="uav_state_text_to_overlay" type="relay_field_728.py"
-        args="/string_state /state_overlay jsk_rviz_plugins/OverlayText
-              'text: m.data' --wait-for-start" />
+        args="/state /state_overlay jsk_rviz_plugins/OverlayText
+              'text: [m.pose.pose.position.z, str]' --wait-for-start" />
   <!-- display  state in rviz -->
-  <node pkg="jsk_mbzirc_tasks" name="uav_ground_truth_text_to_overlay" type="relay_field_728.py"
-        args="/string_ground_truth /ground_truth_overlay jsk_rviz_plugins/OverlayText
-              'text: m.data' --wait-for-start" />
+  <node pkg="jsk_mbzirc_tasks" name="uav_ground_truth_text_to_overlay" type="relay_field_728.py" output="screen"
+        args="/ground_truth/state /ground_truth_overlay jsk_rviz_plugins/OverlayText
+              'text: [m.pose.pose.position.z, str]' --wait-for-start" />
 
   <group unless="$(arg headless)" >
     <node pkg="rviz" type="rviz" name="rviz" args="-d $(find jsk_mbzirc_tasks)/config/task1.rviz"/>

diff --git a/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_3.launch b/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_3.launch
@@ -27,12 +27,12 @@
     </include>
     <!-- display uav state in rviz -->
     <node pkg="jsk_mbzirc_tasks" name="uav_state_text_to_overlay" type="relay_field_728.py"
-          args="/$(arg uav1_name)/string_state /$(arg uav1_name)/state_overlay jsk_rviz_plugins/OverlayText
-                'text: m.data' --wait-for-start" />
+          args="/$(arg uav1_name)/state /$(arg uav1_name)/state_overlay jsk_rviz_plugins/OverlayText
+                'text: [m.pose.pose.position.z, str]' --wait-for-start" />
     <!-- display  state in rviz -->
     <node pkg="jsk_mbzirc_tasks" name="uav_ground_truth_text_to_overlay" type="relay_field_728.py"
-          args="/$(arg uav1_name)/string_ground_truth /$(arg uav1_name)/ground_truth_overlay jsk_rviz_plugins/OverlayText
-                'text: m.data' --wait-for-start" />
+          args="/$(arg uav1_name)/ground_truth/state /$(arg uav1_name)/ground_truth_overlay jsk_rviz_plugins/OverlayText
+                'text: [m.pose.pose.position.z, str]' --wait-for-start" />
   </group>
   <group ns="$(arg uav2_name)">
     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
@@ -45,12 +45,12 @@
     </include>
     <!-- display uav state in rviz -->
     <node pkg="jsk_mbzirc_tasks" name="uav_state_text_to_overlay" type="relay_field_728.py"
-          args="/$(arg uav2_name)/string_state /$(arg uav2_name)/state_overlay jsk_rviz_plugins/OverlayText
-                'text: m.data' --wait-for-start" />
+          args="/$(arg uav2_name)/state /$(arg uav2_name)/state_overlay jsk_rviz_plugins/OverlayText
+                'text: [m.pose.pose.position.z, str]' --wait-for-start" />
     <!-- display  state in rviz -->
     <node pkg="jsk_mbzirc_tasks" name="uav_ground_truth_text_to_overlay" type="relay_field_728.py"
-          args="/$(arg uav2_name)/string_ground_truth /$(arg uav2_name)/ground_truth_overlay jsk_rviz_plugins/OverlayText
-                'text: m.data' --wait-for-start" />
+          args="/$(arg uav2_name)/ground_truth/state /$(arg uav2_name)/ground_truth_overlay jsk_rviz_plugins/OverlayText
+                'text: [m.pose.pose.position.z, str]' --wait-for-start" />
   </group>
   <group ns="$(arg uav3_name)">
     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
@@ -63,12 +63,14 @@
     </include>
     <!-- display uav state in rviz -->
     <node pkg="jsk_mbzirc_tasks" name="uav_state_text_to_overlay" type="relay_field_728.py"
-          args="/$(arg uav3_name)/string_state /$(arg uav3_name)/state_overlay jsk_rviz_plugins/OverlayText
-                'text: m.data' --wait-for-start" />
+          args="/$(arg uav3_name)/state /$(arg uav3_name)/state_overlay jsk_rviz_plugins/OverlayText
+                'text: [m.pose.pose.position.z, str]' --wait-for-start" />
+
     <!-- display  state in rviz -->
     <node pkg="jsk_mbzirc_tasks" name="uav_ground_truth_text_to_overlay" type="relay_field_728.py"
-          args="/$(arg uav3_name)/string_ground_truth /$(arg uav3_name)/ground_truth_overlay jsk_rviz_plugins/OverlayText
-                'text: m.data' --wait-for-start" />
+          args="/$(arg uav3_name)/ground_truth/state /$(arg uav3_name)/ground_truth_overlay jsk_rviz_plugins/OverlayText
+                'text: [m.pose.pose.position.z, str]' --wait-for-start" />
+
   </group>
 
   <!-- workaround until https://github.com/ros/ros_comm/pull/728 get released --> 

diff --git a/jsk_mbzirc_tasks/script/relay_field_728.py b/jsk_mbzirc_tasks/script/relay_field_728.py
@@ -28,6 +28,7 @@
 import rostopic
 import genpy
 import std_msgs
+import __builtin__
 
 __author__ = '[email protected] (Kentaro Wada)'
 
@@ -38,6 +39,9 @@ def _eval_in_dict_impl(dict_, globals_, locals_):
         type_ = type(v)
         if type_ is dict:
             res[k] = _eval_in_dict_impl(v, globals_, locals_)
+        elif (type_ is list):
+            val = eval(v[0], globals_, locals_)
+            res[k] = getattr(__builtin__,v[1])(val)
         elif (type_ is str) or (type_ is unicode):
             try:
                 res[k] = eval(v, globals_, locals_)