Create unique gripper model per parent vehicle model

mbzirc_ign/hooks/resource_paths.dsv.in

@@ -1,5 +1,6 @@
 prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/worlds
 prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/models
+prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/models/tmp
 prepend-non-duplicate;IGN_GAZEBO_SYSTEM_PLUGIN_PATH;@CMAKE_INSTALL_PREFIX@/lib
 
 prepend-non-duplicate;LD_LIBRARY_PATH;@CMAKE_INSTALL_PREFIX@/lib

mbzirc_ign/hooks/resource_paths.sh

@@ -1,5 +1,6 @@
 ament_prepend_unique_value IGN_GAZEBO_RESOURCE_PATH "$AMENT_CURRENT_PREFIX/share/mbzirc_ign/worlds"
 ament_prepend_unique_value IGN_GAZEBO_RESOURCE_PATH "$AMENT_CURRENT_PREFIX/share/mbzirc_ign/models"
+ament_prepend_unique_value IGN_GAZEBO_RESOURCE_PATH "$AMENT_CURRENT_PREFIX/share/mbzirc_ign/models/tmp"
 ament_prepend_unique_value IGN_GAZEBO_SYSTEM_PLUGIN_PATH "$AMENT_CURRENT_PREFIX/lib"
 
 ament_prepend_unique_value LD_LIBRARY_PATH "$AMENT_CURRENT_PREFIX/lib"

mbzirc_ign/models/mbzirc_hexrotor/model.sdf.erb

@@ -11,10 +11,10 @@ end
 
 $attitude_gain = '4 4 2'
 $motor_constant = '1.269e-05'
-if $gripper_model == 'mbzirc_oberon7_gripper'
+if $gripper_model.include?('mbzirc_oberon7_gripper')
   $attitude_gain = '25 25 2'
   $motor_constant = '1.98e-05'
-elsif $gripper_model == 'mbzirc_suction_gripper'
+elsif $gripper_model.include?('mbzirc_suction_gripper')
   $attitude_gain = '25 25 2'
   $motor_constant = '1.40e-05'
 end

mbzirc_ign/models/mbzirc_quadrotor/model.sdf.erb

@@ -11,10 +11,10 @@ end
 
 $attitude_gain = '2 3 0.15'
 $motor_constant = '8.54858e-06'
-if $gripper_model == 'mbzirc_oberon7_gripper'
+if $gripper_model.include?('mbzirc_oberon7_gripper')
   $attitude_gain = '10 10 0.15'
   $motor_constant = '3.95e-05'
-elsif $gripper_model == 'mbzirc_suction_gripper'
+elsif $gripper_model.include?('mbzirc_suction_gripper')
   $attitude_gain = '10 10 0.15'
   $motor_constant = '2.52e-05'
 end

mbzirc_ign/src/SuctionGripper.cc

@@ -154,15 +154,15 @@ void SuctionGripperPlugin::Configure(const Entity &_entity,
     this->dataPtr->node.Subscribe(prefix + "/contact_sensor_12", callback_12);
 
 
-    this->dataPtr->contactPublisherCenter = 
+    this->dataPtr->contactPublisherCenter =
       this->dataPtr->node.Advertise<msgs::Boolean>(prefix + "/contacts/center");
-    this->dataPtr->contactPublisherLeft = 
+    this->dataPtr->contactPublisherLeft =
       this->dataPtr->node.Advertise<msgs::Boolean>(prefix + "/contacts/left");
-    this->dataPtr->contactPublisherRight = 
+    this->dataPtr->contactPublisherRight =
       this->dataPtr->node.Advertise<msgs::Boolean>(prefix + "/contacts/right");
-    this->dataPtr->contactPublisherTop = 
+    this->dataPtr->contactPublisherTop =
       this->dataPtr->node.Advertise<msgs::Boolean>(prefix + "/contacts/top");
-    this->dataPtr->contactPublisherBottom = 
+    this->dataPtr->contactPublisherBottom =
       this->dataPtr->node.Advertise<msgs::Boolean>(prefix + "/contacts/bottom");
   }
   else
@@ -205,7 +205,7 @@ void SuctionGripperPlugin::PreUpdate(const UpdateInfo &_info,
     this->dataPtr->contactPublisherRight.Publish(contact);
     this->dataPtr->contactPublisherTop.Publish(contact);
     this->dataPtr->contactPublisherBottom.Publish(contact);
-  } 
+  }
   else
   {
     contact.set_data(this->dataPtr->contacts[1][1] != kNullEntity);
@@ -228,13 +228,13 @@ void SuctionGripperPlugin::PreUpdate(const UpdateInfo &_info,
   if (!this->dataPtr->jointCreated && this->dataPtr->suctionOn)
   {
     // check that two sensors are making contact with the same object
-    auto checkContacts = 
+    auto checkContacts =
       [&](std::pair<int, int> idx0, std::pair<int, int> idx1) -> bool {
         auto contact0 = this->dataPtr->contacts[idx0.first][idx0.second];
         auto contact1 = this->dataPtr->contacts[idx1.first][idx1.second];
-        return  
-          (contact0 != kNullEntity && 
-           contact1 != kNullEntity && 
+        return
+          (contact0 != kNullEntity &&
+           contact1 != kNullEntity &&
            contact0 == contact1);
       };
 
@@ -243,7 +243,7 @@ void SuctionGripperPlugin::PreUpdate(const UpdateInfo &_info,
       (checkContacts({1, 1}, {1, 0}) ||  // Center + left
        checkContacts({1, 1}, {1, 2}) ||  // Center + right
        checkContacts({1, 1}, {0, 1}) ||  // Center + top
-       checkContacts({1, 1}, {2, 1}));   // Center + bottom 
+       checkContacts({1, 1}, {2, 1}));   // Center + bottom
 
     if (contactMade)
     {
@@ -255,7 +255,7 @@ void SuctionGripperPlugin::PreUpdate(const UpdateInfo &_info,
       this->dataPtr->pendingJointCreation = true;
       this->dataPtr->childItem = this->dataPtr->contacts[1][0];
     }
-    else if (checkContacts({0, 1}, {2, 1}))  // top + bottom 
+    else if (checkContacts({0, 1}, {2, 1}))  // top + bottom
     {
       this->dataPtr->pendingJointCreation = true;
       this->dataPtr->childItem = this->dataPtr->contacts[0][1];

mbzirc_ign/src/mbzirc_ign/model.py

@@ -25,6 +25,8 @@
 
 import mbzirc_ign.bridges
 import mbzirc_ign.payload_bridges
+import pathlib
+import shutil
 
 import yaml
 
@@ -324,6 +326,9 @@ def generate(self):
             get_package_share_directory('mbzirc_ign'),
             'models', self.model_type, 'model.sdf.erb')
 
+        model_dir = os.path.join(get_package_share_directory('mbzirc_ign'), 'models')
+        model_tmp_dir = os.path.join(model_dir, 'tmp')
+
         command = ['erb']
         command.append(f'name={self.model_name}')
 
@@ -342,7 +347,7 @@ def generate(self):
                 raise RuntimeError('Battery Capacity is zero, was flight_time set?')
             command.append(f'capacity={self.battery_capacity}')
             if self.has_valid_gripper():
-                command.append(f'gripper={self.gripper}')
+                command.append(f'gripper={self.gripper}_{self.model_name}')
 
         if self.model_type in USVS or self.model_type == 'static_arm':
             command.append(f'wavefieldSize={self.wavefield_size}')
@@ -364,7 +369,7 @@ def generate(self):
                 arm_command = ['erb']
 
                 if self.gripper:
-                    arm_command.append(f'gripper={self.gripper}')
+                    arm_command.append(f'gripper={self.gripper}_{self.model_name}')
 
                 # arm payloads
                 for (slot, payload) in self.arm_payload.items():
@@ -387,18 +392,20 @@ def generate(self):
                 # print(arm_command, str_output)
 
         if self.has_valid_gripper():
-            gripper_model_file = os.path.join(
-                get_package_share_directory('mbzirc_ign'), 'models',
-                self.gripper, 'model.sdf.erb')
-            gripper_model_output_file = os.path.join(
-                get_package_share_directory('mbzirc_ign'), 'models',
-                self.gripper, 'model.sdf')
+            gripper_model_file = os.path.join(model_dir, self.gripper, 'model.sdf.erb')
+            gripper_model_output_file = os.path.join(model_tmp_dir,
+                                                     self.gripper + "_" + self.model_name,
+                                                     'model.sdf')
             gripper_command = ['erb']
             topic_prefix = f'{self.model_name}'
             if (self.is_USV()):
                 topic_prefix += '/arm'
             gripper_command.append(f'topic_prefix={topic_prefix}')
             gripper_command.append(gripper_model_file)
+            output_dir = os.path.dirname(gripper_model_output_file)
+            pathlib.Path(output_dir).mkdir(parents=True, exist_ok=True)
+            shutil.copyfile(os.path.join(os.path.dirname(gripper_model_file), 'model.config'),
+                            os.path.join(output_dir, 'model.config'))
             process = subprocess.Popen(gripper_command, stdout=subprocess.PIPE)
             stdout = process.communicate()[0]
             str_output = codecs.getdecoder('unicode_escape')(stdout)[0]