Fix gimbal with multiple typhoon_h480

grvcTeam · Sep 12, 2017 · cf617ee · cf617ee
1 parent 3778b78
commit cf617ee
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 5 deletions.
diff --git a/px4_bringup/scripts/spawn_gzmodel.py b/px4_bringup/scripts/spawn_gzmodel.py
@@ -92,6 +92,19 @@ def main():
             if plugintag.get('name') == 'mavlink_interface':
                 porttag = plugintag.find('mavlink_udp_port')
                 porttag.text = str(udp_config["sim_port"])
+
+        # Typhoon_h480 patch - TODO use xacro instead
+        if args.model == 'typhoon_h480':
+            for plugintag in model.findall('plugin'):
+                if plugintag.get('name') == 'gimbal_controller':
+                    imutag = plugintag.find('imu')
+                    imutag.text = 'typhoon_h480_' + str(args.id) + '::camera_imu'
+            for linktag in model.findall('link'):
+                if linktag.get('name') == 'cgo3_camera_link':
+                    for sensortag in linktag.findall('sensor'):
+                        if sensortag.get('name') == 'camera_imu':
+                            sensortag.set('name', 'typhoon_h480_' + str(args.id) + '::camera_imu')
+
         tree.write(temp_sdf)
 
     else:

diff --git a/robots_description/models/typhoon_h480/model.sdf b/robots_description/models/typhoon_h480/model.sdf
@@ -767,7 +767,7 @@
       </physics>
     </joint>
 
-    <link name='typhoon_h480/imu_link'>
+    <link name='imu_link'>
       <pose frame=''>0 0 0 0 0 3.1415927</pose>
       <inertial>
         <pose frame=''>0 0 0 0 -0 0</pose>
@@ -782,8 +782,8 @@
         </inertia>
       </inertial>
     </link>
-    <joint name='typhoon_h480/imu_joint' type='revolute'>
-      <child>typhoon_h480/imu_link</child>
+    <joint name='imu_joint' type='revolute'>
+      <child>imu_link</child>
       <parent>base_link</parent>
       <axis>
         <xyz>1 0 0</xyz>
@@ -1543,7 +1543,7 @@
           <zero_position_armed>0</zero_position_armed>
           <joint_control_type>position_gztopic</joint_control_type>
           <gztopic>/gimbal_roll_cmd</gztopic>
-          <joint_name>cgo3_camera_joint</joint_name>
+          <joint_name>cgo3_horizontal_arm_joint</joint_name>
         </channel>
         <channel name="gimbal_pitch">
           <input_index>7</input_index>
@@ -1606,7 +1606,7 @@
     <static>0</static>
     <plugin name='rotors_gazebo_imu_plugin' filename='librotors_gazebo_imu_plugin.so'>
       <robotNamespace></robotNamespace>
-      <linkName>typhoon_h480/imu_link</linkName>
+      <linkName>imu_link</linkName>
       <imuTopic>/imu</imuTopic>
       <gyroscopeNoiseDensity>0.0003394</gyroscopeNoiseDensity>
       <gyroscopeRandomWalk>3.8785e-05</gyroscopeRandomWalk>