Merge pull request #248 from usnistgov/version1.5_staging

Version 1.5

.gitignore

@@ -51,4 +51,7 @@ log/
 /ariac_human/agent/.gradle/*
 /ariac_human/agent/bin
 /ariac_human/agent/build
-/ariac_human/agent/java_rosbridge.msgs
+/ariac_human/agent/java_rosbridge.msgs
+
+/automated_evaluation/nist_team.yaml
+

ariac_gazebo/ariac_gazebo/environment_startup.py

@@ -80,17 +80,17 @@ def __init__(self):
 
         self.declare_parameter('robot_description', '',
                                ParameterDescriptor(description='Ariac Robots description'))
-
         self.declare_parameter('trial_config_path', '',
                                ParameterDescriptor(description='Path of the current trial\'s configuration yaml file'))
         self.declare_parameter('user_config_path', '',
                                ParameterDescriptor(description='Path of the user\'s configuration yaml file'))
 
-
         self.trial_config = self.read_yaml(
             self.get_parameter('trial_config_path').get_parameter_value().string_value)
+
         self.user_config = self.read_yaml(
             self.get_parameter('user_config_path').get_parameter_value().string_value)
+
 
 
         # Conveyor
@@ -126,11 +126,11 @@ def __init__(self):
             Trial, '/ariac/trial_config', latching_qos)
 
         # Create a subscriber for debugging purposes
-        self.trial_config_sub = self.create_subscription(
-            Trial,
-            '/ariac/trial_config',
-            self.trial_config_callback,
-            10)
+        # self.trial_config_sub = self.create_subscription(
+        #     Trial,
+        #     '/ariac/trial_config',
+        #     self.trial_config_callback,
+        #     10)
 
         # Create service client to spawn objects into gazebo
         self.spawn_client = self.create_client(SpawnEntity, '/spawn_entity')
@@ -144,22 +144,25 @@ def __init__(self):
         self.tf_listener = TransformListener(self.tf_buffer, self)
 
 
-    def trial_config_callback(self, msg: Trial):
-        """Simple callback to print the trial name
+    # def trial_config_callback(self, msg: Trial):
+    #     """Simple callback to print the trial name
 
-        Args:
-            msg (Trial): The trial config message
-        """
-        pass
-        # self.get_logger().info('Trial name: "%s"' % msg.trial_name)
+    #     Args:
+    #         msg (Trial): The trial config message
+    #     """
+    #     pass
+    #     # self.get_logger().info('Trial name: "%s"' % msg.trial_name)
 
 
     def parse_trial_file(self):
+        '''
+        Parse the trial configuration file and publish it
+        '''        
         config_file_name = self.get_parameter(
             'trial_config_path').get_parameter_value().string_value
 
         config_file_name = config_file_name.rsplit('/', 1)[1]
-        # self.get_logger().info(f'CONFIG FILE: {config_file_name}')
+
         message = Trial()
 
         # If time limit is not specified, use default of -1

ariac_gazebo/launch/ariac.launch.py

@@ -15,6 +15,7 @@
 
 from ament_index_python.packages import get_package_share_directory, PackageNotFoundError
 
+
 def launch_setup(context, *args, **kwargs):
     # Set the path to this package.
     pkg_share = FindPackageShare(package='ariac_gazebo').find('ariac_gazebo')
@@ -36,7 +37,8 @@ def launch_setup(context, *args, **kwargs):
     trial_config_path = os.path.join(pkg_share, 'config', 'trials', trial_name + ".yaml")
 
     if not os.path.exists(trial_config_path):
-        rclpy.logging.get_logger('Launch File').fatal(f"Trial configuration '{trial_name}' not found in {pkg_share}/config/trials/")
+        rclpy.logging.get_logger('Launch File').fatal(
+            f"Trial configuration '{trial_name}' not found in {pkg_share}/config/trials/")
         exit()
 
     try:
@@ -45,12 +47,12 @@ def launch_setup(context, *args, **kwargs):
         rclpy.logging.get_logger('Launch File').fatal("Competitor package not found")
         exit()
 
-
     sensor_config = LaunchConfiguration("sensor_config").perform(context)
     user_config_path = os.path.join(competitor_pkg_share, 'config', sensor_config + ".yaml")
 
     if not os.path.exists(user_config_path):
-        rclpy.logging.get_logger('Launch File').fatal(f"Sensor configuration '{sensor_config}.yaml' not found in {competitor_pkg_share}/config/")
+        rclpy.logging.get_logger('Launch File').fatal(
+            f"Sensor configuration '{sensor_config}.yaml' not found in {competitor_pkg_share}/config/")
         exit()
 
     # Gazebo node
@@ -179,7 +181,7 @@ def launch_setup(context, *args, **kwargs):
             'human_behavior': human_behavior,
         }.items(),
         condition=IfCondition(trial_has_human_challenge)
-    )    
+    )
     nodes_to_start = [
         gazebo,
         sensor_tf_broadcaster,
@@ -202,7 +204,9 @@ def generate_launch_description():
     )
 
     declared_arguments.append(
-        DeclareLaunchArgument("competitor_pkg", default_value="test_competitor", description="name of competitor package"))
+        DeclareLaunchArgument(
+            "competitor_pkg", default_value="test_competitor",
+            description="name of competitor package"))
 
     declared_arguments.append(
         DeclareLaunchArgument("sensor_config", default_value="sensors", description="name of user configuration file")

ariac_gazebo/models/pump/model.sdf

@@ -124,12 +124,17 @@
       </visual>
 
       <collision name="collision">
-        <pose>0 0 0.06 0 0 0</pose>
+        <!-- <pose>0 0 0.06 0 0 0</pose> -->
+
         <geometry>
-          <box>
+          <!-- <box>
             <size>0.12 0.12 0.12</size>
-          </box>
+          </box> -->
+          <mesh>
+            <uri>model://pump/meshes/pump_collision.stl</uri>
+          </mesh>
         </geometry>
+        <max_contacts>10</max_contacts>
 
         <surface>
           <contact>

ariac_plugins/package.xml

@@ -16,27 +16,22 @@
   <build_depend>gazebo_dev</build_depend>
   <build_depend>gazebo_msgs</build_depend>
   <build_depend>sensor_msgs</build_depend>
-  <build_depend>
-    controller_manager_msgs</build_depend>
+  <build_depend>controller_manager_msgs</build_depend>
   <build_depend>gazebo_ros</build_depend>
   <build_depend>rclcpp</build_depend>
 
   <exec_depend>gazebo_dev</exec_depend>
-  <exec_depend>
-    gazebo_msgs</exec_depend>
+  <exec_depend>gazebo_msgs</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>controller_manager_msgs</exec_depend>
-  <exec_depend>
-    gazebo_ros</exec_depend>
+  <exec_depend>gazebo_ros</exec_depend>
   <exec_depend>rclcpp</exec_depend>
 
-  <test_depend>
-    ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
   <export>
-    <build_type>
-      ament_cmake</build_type>
+    <build_type>ament_cmake</build_type>
   </export>
 </package>