add script to insert controlable box model to kinect

mttamtam · Aug 26, 2015 · 321672a · 321672a
1 parent 60a94c7
commit 321672a
Show file tree

Hide file tree

Showing 5 changed files with 233 additions and 2 deletions.
diff --git a/hrpsys_gazebo_tutorials/environment_models/Cube/Cube.urdf b/hrpsys_gazebo_tutorials/environment_models/Cube/Cube.urdf
@@ -0,0 +1,53 @@
+<?xml version="1.0"?>
+<robot name="cube-robot"
+       xmlns:xi="http://www.w3.org/2001/XInclude"
+       xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+       xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+       xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+       xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+       xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+       xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+       xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+       xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+       xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">
+  <gazebo>
+    <static>true</static>
+  </gazebo>
+  <link name="nil_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 -0 0"/>
+      <geometry>
+        <mesh filename="model://Cube/meshes/nil_link_mesh.dae" scale="1 1 1" />
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 -0 0"/>
+      <geometry>
+        <mesh filename="model://Cube/meshes/nil_link_mesh.dae" scale="1 1 1" />
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1" />
+      <origin xyz="0 0 0" rpy="0 -0 0"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+  </link>
+  <gazebo reference="nil_link">
+    <mu1>0.9</mu1>
+    <mu2>0.9</mu2>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libSetVelPlugin.so" name="set_vel_plugin" >
+      <objname>Cube</objname>
+      <linkname>nil_link</linkname>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libPubTfPlugin.so" name="pub_tf_plugin" >
+      <objname>Cube</objname>
+      <linkname>nil_link</linkname>
+    </plugin>
+  </gazebo>
+</robot>
diff --git a/hrpsys_gazebo_tutorials/environment_models/Cube/meshes/nil_link_mesh.dae b/hrpsys_gazebo_tutorials/environment_models/Cube/meshes/nil_link_mesh.dae
@@ -0,0 +1,107 @@
+<?xml version="1.0"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+  <asset>
+    <contributor>
+      <author>Assimp</author>
+      <authoring_tool>Assimp Collada Exporter</authoring_tool>
+    </contributor>
+    <created>2015-08-26T12:04:52</created>
+    <modified>2015-08-26T12:04:52</modified>
+    <unit name="meter" meter="1" />
+    <up_axis>Y_UP</up_axis>
+  </asset>
+  <library_effects>
+    <effect id="m0blinn20-fx" name="m0blinn20">
+      <profile_COMMON>
+        <technique sid="common">
+          <phong>
+            <emission>
+              <color sid="emission">0   0   0   1</color>
+            </emission>
+            <ambient>
+              <color sid="ambient">0.199219   0.199219   0.199219   1</color>
+            </ambient>
+            <diffuse>
+              <color sid="diffuse">0.398438   0.398438   0.398438   1</color>
+            </diffuse>
+            <specular>
+              <color sid="specular">0   0   0   1</color>
+            </specular>
+            <shininess>
+              <float sid="shininess">10</float>
+            </shininess>
+            <reflective>
+              <color sid="reflective">0   0   0   0</color>
+            </reflective>
+            <transparent>
+              <color sid="transparent">0   0   0   1</color>
+            </transparent>
+            <transparency>
+              <float sid="transparency">0</float>
+            </transparency>
+            <index_of_refraction>
+              <float sid="index_of_refraction">1</float>
+            </index_of_refraction>
+          </phong>
+        </technique>
+      </profile_COMMON>
+    </effect>
+  </library_effects>
+  <library_materials>
+    <material id="m0blinn20" name="m0blinn20">
+      <instance_effect url="#m0blinn20-fx"/>
+    </material>
+  </library_materials>
+  <library_geometries>
+    <geometry id="meshId0" name="meshId0_name" >
+      <mesh>
+        <source id="meshId0-positions" name="meshId0-positions">
+          <float_array id="meshId0-positions-array" count="72"> -0.5 -0.5 1 0.5 -0.5 1 0.5 0.5 1 -0.5 0.5 1 0.5 -0.5 0 -0.5 -0.5 0 -0.5 0.5 0 0.5 0.5 0 -0.5 0.5 0 -0.5 0.5 1 0.5 0.5 1 0.5 0.5 0 0.5 0.5 0 0.5 0.5 1 0.5 -0.5 1 0.5 -0.5 0 0.5 -0.5 0 0.5 -0.5 1 -0.5 -0.5 1 -0.5 -0.5 0 -0.5 -0.5 0 -0.5 -0.5 1 -0.5 0.5 1 -0.5 0.5 0 </float_array>
+          <technique_common>
+            <accessor count="24" offset="0" source="#meshId0-positions-array" stride="3">
+              <param name="X" type="float" />
+              <param name="Y" type="float" />
+              <param name="Z" type="float" />
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="meshId0-normals" name="meshId0-normals">
+          <float_array id="meshId0-normals-array" count="72"> 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 1 0 0 1 0 0 1 0 0 1 0 1 0 -0 1 0 -0 1 0 -0 1 0 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 -1 0 0 -1 0 -0 </float_array>
+          <technique_common>
+            <accessor count="24" offset="0" source="#meshId0-normals-array" stride="3">
+              <param name="X" type="float" />
+              <param name="Y" type="float" />
+              <param name="Z" type="float" />
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="meshId0-vertices">
+          <input semantic="POSITION" source="#meshId0-positions" />
+          <input semantic="NORMAL" source="#meshId0-normals" />
+        </vertices>
+        <polylist count="12" material="theresonlyone">
+          <input offset="0" semantic="VERTEX" source="#meshId0-vertices" />
+          <vcount>3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
+          <p>0 1 2 2 3 0 4 5 6 6 7 4 8 9 10 10 11 8 12 13 14 14 15 12 16 17 18 18 19 16 20 21 22 22 23 20 </p>
+        </polylist>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_visual_scenes>
+    <visual_scene id="base1d_med" name="base1d_med">
+      <node id="nil_link" name="nil_link">
+        <matrix>1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1</matrix>
+        <instance_geometry url="#meshId0">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="theresonlyone" target="#m0blinn20" />
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#base1d_med" />
+  </scene>
+</COLLADA>
diff --git a/hrpsys_gazebo_tutorials/launch/gazebo_kinect_cube_interactive.launch b/hrpsys_gazebo_tutorials/launch/gazebo_kinect_cube_interactive.launch
@@ -0,0 +1,9 @@
+<launch>
+  <arg name="RUN_RVIZ" default="true" />
+
+  <node pkg="hrpsys_gazebo_tutorials" type="insert_cube_interactive_model.py" name="insert_cube_interactive_model" />
+
+  <include file="$(find hrpsys_gazebo_tutorials)/launch/gazebo_kinect_interactive.launch">
+    <arg name="RUN_RVIZ" value="$(arg RUN_RVIZ)"/>
+  </include>
+</launch>
diff --git a/hrpsys_gazebo_tutorials/scripts/insert_cube_interactive_model.py b/hrpsys_gazebo_tutorials/scripts/insert_cube_interactive_model.py
@@ -0,0 +1,61 @@
+#!/usr/bin/env python
+
+import rospy
+import math
+import jsk_interactive_marker
+from geometry_msgs.msg import *
+from jsk_interactive_marker.srv import *
+from jsk_interactive_marker.msg import *
+from gazebo_msgs.msg import *
+from gazebo_msgs.srv import *
+from jsk_rviz_plugins.msg import TransformableMarkerOperate
+from jsk_rviz_plugins.srv import RequestMarkerOperate
+from gazebo_msgs.srv import SpawnModel
+import tf
+import rospkg
+
+def disable_gravity():
+    rospy.wait_for_service('/gazebo/set_physics_properties')
+    set_physics_properties_srv = rospy.ServiceProxy('/gazebo/set_physics_properties', SetPhysicsProperties)
+    get_physics_properties_srv = rospy.ServiceProxy('/gazebo/get_physics_properties', GetPhysicsProperties)
+    current_physics_properties = get_physics_properties_srv()
+    set_physics_properties_srv(time_step=0.001,
+                               max_update_rate=1000,
+                               gravity=Vector3(x=0, y=0, z=0),
+                               ode_config=current_physics_properties.ode_config)
+
+def spawn_gazebo_cube_model():
+    try:
+        spawn_model_srv = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel)
+        urdf_file_path = rospkg.RosPack().get_path('hrpsys_gazebo_tutorials')+'/environment_models/Cube/Cube.urdf'
+        urdf_file_data = open(urdf_file_path).read()
+        spawn_model_srv(model_name='Cube', model_xml=urdf_file_data)
+    except rospy.ServiceException, e:
+        print 'spawn_model service call failed: %s'%e
+
+def insert_marker(shape_type=TransformableMarkerOperate.BOX, name='default_name', description='default_description', mesh_resource='', mesh_use_embedded_materials=False):
+    try:
+        req_marker_operate_srv(TransformableMarkerOperate(type=shape_type, action=TransformableMarkerOperate.INSERT, frame_id="gazebo_world", name=name, description=description, mesh_resource=mesh_resource, mesh_use_embedded_materials=mesh_use_embedded_materials))
+    except rospy.ServiceException, e:
+        print 'insert_marker service call failed: %s'%e
+
+def insert_cube_interactive_model():
+    global cube_pose_pub, req_marker_operate_srv, set_pose_srv
+    rospy.Subscriber('/transformable_interactive_server/pose_with_name', PoseStampedWithName, cube_marker_pose_cb)
+    cube_pose_pub = rospy.Publisher('/Cube/SetVelPlugin/PoseCommand', Pose)
+    req_marker_operate_srv = rospy.ServiceProxy('/transformable_interactive_server/request_marker_operate', RequestMarkerOperate)
+    mesh_resource_name="package://hrpsys_gazebo_tutorials/environment_models/Cube/meshes/nil_link_mesh.dae"
+    insert_marker(shape_type=TransformableMarkerOperate.MESH_RESOURCE, name='cube_marker', description='', mesh_resource=mesh_resource_name, mesh_use_embedded_materials=True)
+
+def cube_marker_pose_cb(msg):
+    if msg.name == 'cube_marker':
+        cube_pose_pub.publish(msg.pose.pose)
+
+if __name__ == '__main__':
+    rospy.init_node('insert_cube_interactive_model')
+    disable_gravity()
+    rospy.sleep(1)
+    spawn_gazebo_cube_model()
+    rospy.sleep(1)
+    insert_cube_interactive_model()
+    rospy.spin()
diff --git a/hrpsys_gazebo_tutorials/scripts/insert_kinect_interactive_model.py b/hrpsys_gazebo_tutorials/scripts/insert_kinect_interactive_model.py
@@ -18,7 +18,7 @@ def insert_marker(shape_type=TransformableMarkerOperate.BOX, name='default_name'
 
 def insert_kinect_interactive_model():
     global kinect_pose_pub, req_marker_operate_srv, set_pose_srv
-    rospy.Subscriber('/transformable_interactive_server/pose', PoseStamped, kinect_marker_pose_cb)
+    rospy.Subscriber('/transformable_interactive_server/pose_with_name', PoseStampedWithName, kinect_marker_pose_cb)
     kinect_pose_pub = rospy.Publisher('/Kinect/SetVelPlugin/PoseCommand', Pose)
     set_control_relative_pose_pub = rospy.Publisher('/transformable_interactive_server/set_control_relative_pose', Pose)
     req_marker_operate_srv = rospy.ServiceProxy('/transformable_interactive_server/request_marker_operate', RequestMarkerOperate)
@@ -31,7 +31,8 @@ def insert_kinect_interactive_model():
     set_pose_srv(pose_stamped=PoseStamped(std_msgs.msg.Header(stamp=rospy.Time.now(), frame_id="camera_link"), Pose(orientation=Quaternion(0, 0, 0, 1))))
 
 def kinect_marker_pose_cb(msg):
-    kinect_pose_pub.publish(msg.pose)
+    if msg.name == 'kinect_marker':
+        kinect_pose_pub.publish(msg.pose.pose)
 
 if __name__ == '__main__':
     rospy.init_node('insert_kinect_interactive_model')