Feature to transform markers on rviz

jsk_interactive_markers/jsk_interactive_marker/CMakeLists.txt

@@ -27,6 +27,7 @@ find_package(catkin REQUIRED COMPONENTS
   jsk_recognition_msgs
   ${PCL_MSGS}
   jsk_topic_tools
+  rviz
   roseus)
 find_package(orocos_kdl REQUIRED)
 find_package(TinyXML REQUIRED)
@@ -54,13 +55,28 @@ add_definitions("-g")
 include_directories(include ${catkin_INCLUDE_DIRS}
   ${orocos_kdl_INCLUDE_DIRS} ${TinyXML_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
 
+# include Qt
+find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED)
+include(${QT_USE_FILE})
+add_definitions(-DQT_NO_KEYWORDS -g)
+
+qt4_wrap_cpp(MOC_FILES
+  include/jsk_interactive_marker/rviz_plugins/transformable_marker_operator.h
+)
+
 # target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${TinyXML_LIBRARIES} ${orocos_kdl_LIBRARIES})
 link_directories(${catkin_LIBRARY_DIRS})
 link_directories(${orocos_kdl_LIBRARY_DIRS})
 link_directories(${Boost_LIBRARY_DIRS})
 
-add_library(jsk_interactive_marker src/interactive_marker_helpers.cpp)
-target_link_libraries(jsk_interactive_marker ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES} ${Boost_LIBRARIES})
+add_library(jsk_interactive_marker
+  src/interactive_marker_helpers.cpp
+  src/rviz_plugins/transformable_marker_operator.cpp
+  ${MOC_FILES}
+  ${UIC_FILES}
+)
+target_link_libraries(jsk_interactive_marker ${QT_LIBRARIES} ${catkin_LIBRARIES} ${rviz_DEFAULT_PLUGIN_LIBRARIES} ${orocos_kdl_LIBRARIES} ${Boost_LIBRARIES})
+add_dependencies(jsk_interactive_marker ${PROJECT_NAME}_generate_messages_cpp ${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS})
 
 add_executable(interactive_marker_interface src/interactive_marker_interface.cpp src/interactive_marker_utils.cpp)
 target_link_libraries(interactive_marker_interface ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES} ${Boost_LIBRARIES} jsk_interactive_marker)
@@ -149,6 +165,13 @@ if(${yaml_cpp_VERSION} VERSION_LESS "0.5.0")
 endif()
 
 
+install(FILES plugin_description.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(TARGETS jsk_interactive_marker
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+
 install(DIRECTORY launch euslisp urdf
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   PATTERN ".svn" EXCLUDE)

jsk_interactive_markers/jsk_interactive_marker/config/sample_transformable_markers_client.rviz

@@ -7,7 +7,7 @@ Panels:
         - /Global Options1
         - /Status1
       Splitter Ratio: 0.505308
-    Tree Height: 565
+    Tree Height: 104
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -27,6 +27,9 @@ Panels:
     Name: Time
     SyncMode: 0
     SyncSource: ""
+  - Class: jsk_rviz_plugin/TransformableMarkerOperatorAction
+    Name: TransformableMarkerOperatorAction
+    ServerName: /transformable_interactive_server
 Visualization Manager:
   Class: ""
   Displays:
@@ -100,31 +103,33 @@ Visualization Manager:
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.498753
-        Y: -0.574558
-        Z: -0.315591
+        X: 0.554979
+        Y: -0.888855
+        Z: 0.0868585
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 0.580398
+      Pitch: 0.290398
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.6954
+      Yaw: 0.8604
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
   Height: 846
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001d9000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002d1000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001d9000002c4fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000000f7000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000042005400720061006e00730066006f0072006d00610062006c0065004d00610072006b00650072004f00700065007200610074006f00720041006300740069006f006e0100000125000001c7000001c700ffffff000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002d1000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
     collapsed: false
   Tool Properties:
     collapsed: false
+  TransformableMarkerOperatorAction:
+    collapsed: false
   Views:
     collapsed: false
   Width: 1200
-  X: 0
-  Y: 14
+  X: 196
+  Y: 150

jsk_rviz_plugins/src/transformable_marker_operator.h → jsk_interactive_markers/jsk_interactive_marker/include/jsk_interactive_marker/rviz_plugins/transformable_marker_operator.h

@@ -5,24 +5,34 @@
 #include <ros/ros.h>
 #include <rviz/panel.h>
 #include <QtGui>
+#include <jsk_interactive_marker/SetMarkerDimensions.h>
 #include <jsk_rviz_plugins/RequestMarkerOperate.h>
 #endif
 
 class QLineEdit;
 class QPushButton;
 
-namespace jsk_rviz_plugins
+namespace rviz {
+  class VisualizationManager;
+}
+
+namespace jsk_interactive_marker
 {
   class TransformableMarkerOperatorAction: public rviz::Panel
     {
       Q_OBJECT
       public:
       TransformableMarkerOperatorAction( QWidget* parent = 0 );
 
+      virtual void onInitialize();
       virtual void load( const rviz::Config& config );
       virtual void save( rviz::Config config ) const;
 
     protected Q_SLOTS:
+      void update();
+      void updateServerName();
+      void updateFocusMarkerDimensions();
+      void updateDimensionsService();
 
       void callRequestMarkerOperateService(jsk_rviz_plugins::RequestMarkerOperate srv);
       void insertBoxService();
@@ -43,13 +53,20 @@ namespace jsk_rviz_plugins
 
       QVBoxLayout* layout;
 
+      QLineEdit* server_name_editor_;
+      QLineEdit* transform_name_editor_;
+      QLineEdit* dimension_x_editor_;
+      QLineEdit* dimension_y_editor_;
+      QLineEdit* dimension_z_editor_;
+      QLineEdit* dimension_radius_editor_;
+      QLineEdit* dimension_sm_radius_editor_;
       QLineEdit* name_editor_;
       QLineEdit* description_editor_;
       QLineEdit* frame_editor_;
       QLineEdit* id_editor_;
 
       ros::NodeHandle nh_;
     };
-}
+}  // namespace jsk_interactive_marker
 
 #endif

jsk_interactive_markers/jsk_interactive_marker/include/jsk_interactive_marker/transformable_object.h

@@ -88,7 +88,7 @@ namespace jsk_interactive_marker
     bool setY(std_msgs::Float32 y){box_y_=y.data;return true;};
     bool setZ(std_msgs::Float32 z){box_z_=z.data;return true;};
 
-    float getInteractiveMarkerScale(){return 1+std::max(std::max(box_x_, box_y_),box_z_);};
+    float getInteractiveMarkerScale(){return 1.25*std::max(std::max(box_x_, box_y_),box_z_);};
 
     float box_x_;
     float box_y_;
@@ -172,7 +172,7 @@ namespace jsk_interactive_marker
     bool setRadius(std_msgs::Float32 r){cylinder_radius_=r.data;return true;};
     bool setZ(std_msgs::Float32 z){cylinder_z_=z.data;return true;};
 
-    float getInteractiveMarkerScale(){return 1+std::max(cylinder_radius_, cylinder_z_);};
+    float getInteractiveMarkerScale(){return 1.25*std::max(cylinder_radius_, cylinder_z_);};
 
     float cylinder_radius_;
     float cylinder_z_;

jsk_interactive_markers/jsk_interactive_marker/package.xml

@@ -40,6 +40,7 @@
   <build_depend version_gte="1.0.0">jsk_recognition_msgs</build_depend>
   <build_depend>jsk_topic_tools</build_depend> 
   <build_depend>yaml-cpp</build_depend>
+  <build_depend>rviz</build_depend>
   <run_depend>message_runtime</run_depend>
   <run_depend>roseus</run_depend>
   <run_depend>tinyxml</run_depend>
@@ -64,6 +65,12 @@
   <run_depend>pr2eus_moveit</run_depend>
   <run_depend>yaml-cpp</run_depend>
   <run_depend>moveit_msgs</run_depend>
+  <run_depend>rviz</run_depend>
+
+  <export>
+    <rviz plugin="${prefix}/plugin_description.xml" />
+  </export>
+
 </package>
 
 

jsk_interactive_markers/jsk_interactive_marker/plugin_description.xml

@@ -0,0 +1,15 @@
+<library path="lib/libjsk_interactive_marker">
+
+  <!--
+    Use name jsk_rviz_plugin/XXX intentionally for backward compatibility
+    of plugins moved from jsk_rviz_plugin.
+  -->
+  <class name="jsk_rviz_plugin/TransformableMarkerOperatorAction"
+         type="jsk_interactive_marker::TransformableMarkerOperatorAction"
+         base_class_type="rviz::Panel">
+    <description>
+      A panel widget to TransformableOperator action
+    </description>
+  </class>
+
+</library>

jsk_interactive_markers/jsk_interactive_marker/scripts/transformable_markers_client.py

@@ -8,12 +8,12 @@
 from geometry_msgs.msg import Vector3
 from geometry_msgs.msg import Quaternion
 from geometry_msgs.msg import Pose
-from geometry_msgs.msg import PoseStamped
 from jsk_rviz_plugins.srv import RequestMarkerOperate
 from jsk_rviz_plugins.msg import TransformableMarkerOperate
+from jsk_interactive_marker.msg import MarkerDimensions
 from jsk_interactive_marker.msg import PoseStampedWithName
-from jsk_interactive_marker.srv import GetTransformableMarkerPose
-from jsk_interactive_marker.srv import GetTransformableMarkerPoseRequest
+from jsk_interactive_marker.srv import GetTransformableMarkerFocus
+from jsk_interactive_marker.srv import GetTransformableMarkerFocusRequest
 from jsk_interactive_marker.srv import SetTransformableMarkerPose
 from jsk_interactive_marker.srv import SetTransformableMarkerPoseRequest
 from jsk_interactive_marker.srv import SetTransformableMarkerColor
@@ -23,7 +23,6 @@
 from jsk_recognition_utils.color import labelcolormap
 from jsk_recognition_msgs.msg import BoundingBox
 from jsk_recognition_msgs.msg import BoundingBoxArray
-from std_msgs.msg import Header
 import rospy
 
 
@@ -48,12 +47,17 @@ def __init__(self):
             osp.join(self.server, 'set_pose'), SetTransformableMarkerPose)
         self.req_dim = rospy.ServiceProxy(
             osp.join(self.server, 'set_dimensions'), SetMarkerDimensions)
+        self.req_focus = rospy.ServiceProxy(
+            osp.join(self.server, 'get_focus'),
+            GetTransformableMarkerFocus)
         rospy.wait_for_service(self.req_marker.resolved_name)
         rospy.wait_for_service(self.req_color.resolved_name)
         rospy.wait_for_service(self.req_pose.resolved_name)
         rospy.wait_for_service(self.req_dim.resolved_name)
+        rospy.wait_for_service(self.req_focus.resolved_name)
 
         self.object_poses = {}
+        self.object_dimensions = {}
 
         # TODO: support other than box: ex. torus, cylinder, mesh
 
@@ -73,26 +77,24 @@ def __init__(self):
             pos = box.get('position', [0, 0, 0])
             ori = box.get('orientation', [0, 0, 0, 1])
             self.set_pose(box['name'], box['frame_id'], pos, ori)
-            self.object_poses[box['name']] = PoseStamped(
-                header=Header(frame_id=box['frame_id']),
-                pose=Pose(
-                    position=Vector3(*pos),
-                    orientation=Quaternion(*ori),
-                )
+            self.object_poses[box['name']] = Pose(
+                position=Vector3(*pos),
+                orientation=Quaternion(*ori),
             )
+            self.object_dimensions[box['name']] = Vector3(*dim)
             rospy.loginfo("Inserted transformable box '{}'.".format(name))
 
+        self.sub_dimensions = rospy.Subscriber(
+            osp.join(self.server, 'marker_dimensions'),
+            MarkerDimensions,
+            self._dimensions_change_callback)
         self.sub_pose = rospy.Subscriber(
             osp.join(self.server, 'pose_with_name'),
             PoseStampedWithName,
             self._pose_change_callback)
 
         do_auto_save = rospy.get_param('~config_auto_save', True)
         if do_auto_save:
-            self.req_get_pose = rospy.ServiceProxy(
-                osp.join(self.server, 'get_pose'),
-                GetTransformableMarkerPose)
-            rospy.wait_for_service(self.req_get_pose.resolved_name)
             self.timer_save = rospy.Timer(
                 rospy.Duration(1), self._save_callback)
 
@@ -141,50 +143,59 @@ def set_pose(self, name, frame_id, position, orientation):
         req.pose_stamped.pose.orientation.w = orientation[3]
         self.req_pose(req)
 
-    def get_pose(self, name):
-        req = GetTransformableMarkerPoseRequest(target_name=name)
-        res = self.req_get_pose(req)
-        return res.pose_stamped
+    def get_focus_marker(self):
+        req = GetTransformableMarkerFocusRequest()
+        res = self.req_focus(req)
+        return res.target_name
+
+    def _dimensions_change_callback(self, msg):
+        name = self.get_focus_marker()
+        self.object_dimensions[name] = msg
 
     def _pose_change_callback(self, msg):
-        self.object_poses[msg.name] = msg.pose
+        self.object_poses[msg.name] = msg.pose.pose
 
     def _pub_bboxes_callback(self, event):
         bbox_array_msg = BoundingBoxArray()
         bbox_array_msg.header.frame_id = self.config['boxes'][0]['frame_id']
         bbox_array_msg.header.stamp = event.current_real
         for box in self.config['boxes']:
             bbox_msg = BoundingBox()
-            pose_stamped = self.object_poses[box['name']]
+            pose = self.object_poses[box['name']]
             bbox_msg.header.frame_id = bbox_array_msg.header.frame_id
             bbox_msg.header.stamp = bbox_array_msg.header.stamp
-            bbox_msg.pose = pose_stamped.pose
-            # TODO: support transformed bbox dimensions like pose
-            dimensions = box['dimensions']
-            bbox_msg.dimensions.x = dimensions[0]
-            bbox_msg.dimensions.y = dimensions[1]
-            bbox_msg.dimensions.z = dimensions[2]
+            bbox_msg.pose = pose
+            dimensions = self.object_dimensions[box['name']]
+            bbox_msg.dimensions.x = dimensions.x
+            bbox_msg.dimensions.y = dimensions.y
+            bbox_msg.dimensions.z = dimensions.z
             bbox_array_msg.boxes.append(bbox_msg)
         self.pub_bboxes.publish(bbox_array_msg)
 
     def _save_callback(self, event):
         for i, box in enumerate(self.config['boxes']):
             box_cfg = self.config['boxes'][i]
             box_cfg['name'] = box['name']
-            pose_stamped = self.object_poses[box['name']]
+            pose = self.object_poses[box['name']]
+            dimensions = self.object_dimensions[box['name']]
             # TODO: support transformed bbox dimensions like pose
             box_cfg.update({
-                'frame_id': pose_stamped.header.frame_id,
+                'frame_id': box['frame_id'],
+                'dimensions': [
+                    dimensions.x,
+                    dimensions.y,
+                    dimensions.z,
+                ],
                 'position': [
-                    pose_stamped.pose.position.x,
-                    pose_stamped.pose.position.y,
-                    pose_stamped.pose.position.z,
+                    pose.position.x,
+                    pose.position.y,
+                    pose.position.z,
                 ],
                 'orientation': [
-                    pose_stamped.pose.orientation.x,
-                    pose_stamped.pose.orientation.y,
-                    pose_stamped.pose.orientation.z,
-                    pose_stamped.pose.orientation.w,
+                    pose.orientation.x,
+                    pose.orientation.y,
+                    pose.orientation.z,
+                    pose.orientation.w,
                 ],
             })
         yaml.dump(self.config, open(self.config_file, 'w'))