Add ball shooter yaml for customizing WAM-V

vrx_gazebo/config/wamv_config/ball_shooter_compliance/numeric.yaml

@@ -0,0 +1,5 @@
+wamv_ball_shooter:
+    num: 1
+    allowed_params:
+        position
+        orientation

vrx_gazebo/config/wamv_config/example_ball_shooter_config.yaml

@@ -0,0 +1,3 @@
+wamv_ball_shooter:
+    - position: "0.55 0.35 1.4"
+      orientation: "0 0 0" 

vrx_gazebo/launch/generate_wamv.launch

@@ -13,6 +13,9 @@
     <arg name="thruster_yaml" default="$(find vrx_gazebo)/src/vrx_gazebo_python/generator_scripts/wamv_config/example_thruster_config.yaml"/>
     <param name="thruster_yaml" value="$(arg thruster_yaml)"/>
 
+    <arg name="ball_shooter_yaml" default="$(find vrx_gazebo)/src/vrx_gazebo_python/generator_scripts/wamv_config/example_ball_shooter_config.yaml"/>
+    <param name="ball_shooter_yaml" value="$(arg ball_shooter_yaml)"/>
+
     <arg name="wamv_target"/>
     <param name="wamv_target" value="$(arg wamv_target)"/>
 
@@ -22,6 +25,8 @@
 
     <param name="thrusters_dir" value="$(find wamv_description)/urdf/thrusters"/>
 
+    <param name="ball_shooter_dir" value="$(find wamv_gazebo)/urdf/ball_shooter"/>
+
     <node name="wamv_generator" pkg="vrx_gazebo" type="generate_wamv.py" output="screen" required="true"/>
   </group>
 </launch>

vrx_gazebo/src/vrx_gazebo/compliance.py

@@ -8,6 +8,71 @@
 from vrx_gazebo.utils import get_macros
 
 
+class BallShooterCompliance:
+    def __init__(self):
+
+        rospack = rospkg.RosPack()
+        pkg_path = rospack.get_path('vrx_gazebo')
+        self.config_dir = os.path.join(pkg_path, 'config', 'wamv_config')
+
+        # open ball_shooter_compliance/bounding_boxes.yaml and all the boxes defined
+        self.boxes = find_boxes(os.path.join(self.config_dir,
+            'sensor_compliance', 'bounding_boxes.yaml'))
+        # look at all sensors in sensors directory and get the default params
+        self.ball_dir = rospy.get_param('ball_shooter_dir') + '/'
+        self.default_parameters = get_macros(self.ball_dir)
+        self.numeric = yaml.safe_load(open(os.path.join(self.config_dir,
+            'ball_shooter_compliance', 'numeric.yaml')))
+        return
+
+    def param_compliance(self, sensor_type, params={}):
+        # given an instance of ball shooter with parameters
+        # = params, is it in compliance
+        # check if ball shooter is allowed
+        params = params.copy()
+        if sensor_type not in self.default_parameters:
+            rospy.logerr('%s is not defined anywhere under %s' %
+                        (sensor_type, self.config_dir))
+        assert sensor_type in self.default_parameters,\
+            '%s is not defined anywhere under %s' % (sensor_type, self.config_dir)
+        for i in params:
+            if i not in self.numeric[sensor_type]['allowed_params']:
+                rospy.logerr('%s parameter specification of %s not permitted' % 
+                            (i, sensor_type))
+
+        # add the default params to params if not specified
+        for i in self.default_parameters[sensor_type]:
+            if i not in params:
+                params[i] = self.default_parameters[sensor_type][i]
+        if 'x' and 'y' and 'z' in params:
+            xyz = np.array([float(params['x']),
+                            float(params['y']),
+                            float(params['z'])])
+            for box in self.boxes:
+                if box.fit(xyz):
+                    return True
+            rospy.logerr('%s %s is out of bounds' % 
+                        (sensor_type, params['name']))
+            rospy.logerr('%s is at xyz=(%s, %s, %s), %s' % 
+                          (sensor_type, params['name'], 
+                           xyz[0], xyz[1], xyz[2],
+                           'must fit in at last one of the following boxes ' +
+                           'with remaining space:'))
+            for box in self.boxes:
+                rospy.logerr(' %s' % str(box))
+            return False
+        else:
+            return True
+
+    def number_compliance(self, sensor_type, n):
+        # ie: are n ball shooters allowed?
+        if n > self.numeric[sensor_type]['num']:
+            rospy.logerr('Too many ball shooters requested')
+            rospy.logerr(' maximum of %s allowed' %
+                         (self.numeric['num']))
+            return False
+        return True
+
 class SensorCompliance:
     def __init__(self):
 

vrx_gazebo/src/vrx_gazebo/configure_wamv.py

@@ -4,6 +4,7 @@
 
 from vrx_gazebo.compliance import SensorCompliance
 from vrx_gazebo.compliance import ThrusterCompliance
+from vrx_gazebo.compliance import BallShooterCompliance
 
 from vrx_gazebo.utils import create_xacro_file
 from vrx_gazebo.utils import add_gazebo_thruster_config
@@ -15,6 +16,7 @@ def main():
     # Check if yaml files were given
     received_thruster_yaml = len(rospy.get_param('thruster_yaml')) > 0
     received_sensor_yaml = len(rospy.get_param('sensor_yaml')) > 0
+    received_ball_yaml = len(rospy.get_param('ball_shooter_yaml')) > 0
 
     # Setup thruster xacro
     if received_thruster_yaml:
@@ -24,6 +26,10 @@ def main():
     if received_sensor_yaml:
         sensor_compliant = create_sensor_xacro()
 
+    # Setup ball shooter xacro
+    if received_ball_yaml:
+        ball_compliant = create_ball_shooter_xacro()
+
     # Setup command to generate WAM-V urdf file
     wamv_target = rospy.get_param('wamv_target')
     wamv_gazebo = rospy.get_param('wamv_gazebo')
@@ -44,12 +50,20 @@ def main():
         create_urdf_command += (" yaml_sensor_generation:=true "
                                 "sensor_xacro_file:=" + sensor_xacro_target)
 
+    if received_ball_yaml:
+        ball_yaml = rospy.get_param('ball_shooter_yaml')
+        ball_xacro_target = os.path.splitext(ball_yaml)[0] + '.xacro'
+        create_urdf_command += (" yaml_ball_shooter_generation:=true "
+                                "ball_shooter_xacro_file:=" + ball_xacro_target)
+
     # Create urdf and print to console
+    print('CHECK HERE')
+    print(create_urdf_command)
     os.system(create_urdf_command)
-    if not (thruster_compliant and sensor_compliant):
-        rospy.logerr('\nThis sensor/thruster configuration is NOT compliant ' +
-                     'with the (current) VRX constraints. A urdf file will ' +
-                     'be created, but please note that the above errors ' +
+    if not (thruster_compliant and sensor_compliant and ball_compliant):
+        rospy.logerr('\nThis sensor/thruster/ball shooter configuration is NOT ' +
+                     'compliant with the (current) VRX constraints. A urdf file ' +
+                     'will be created, but please note that the above errors ' +
                      'must be fixed for this to be a valid configuration ' +
                      'for the VRX competition.\n')
 
@@ -152,3 +166,39 @@ def create_sensor_xacro():
                              num_test=sensor_num_test,
                              param_test=sensor_param_test)
 
+
+def create_ball_shooter_xacro():
+    """
+    Purpose: Create a ball shooter xacro file using the given
+             rosparameters
+    """
+    # Get yaml files for ball shooter pose
+    ball_shooter_yaml = rospy.get_param('ball_shooter_yaml')
+    # rospy.loginfo('\nUsing %s as the ball shooter configuration yaml file\n'
+                  # ball_shooter_yaml)
+
+    # Set ball shooter xacro target 
+    ball_xacro_target = os.path.splitext(ball_shooter_yaml)[0] + '.xacro'
+
+    # Things to start/open the macro
+    ball_boiler_plate_top = ('<?xml version="1.0"?>\n'
+                             '<robot '
+                             'xmlns:xacro="http://ros.org/wiki/xacro" '
+                             'name="wam-v-ball-shooter">\n' +
+                             '  <xacro:macro name="yaml_ball_shooter">\n')
+
+    # Things to close the macro 
+    ball_boiler_plate_bot = '  </xacro:macro>\n</robot>'
+
+    # Check if valid number of ball shooters valid ball shooter parameters
+    comp = BallShooterCompliance()
+    ball_num_test = comp.number_compliance
+    ball_param_test = comp.param_compliance
+
+    # Create ball shooter xacro with ball shooter macros
+    return create_xacro_file(yaml_file=ball_shooter_yaml,
+                             xacro_target=ball_xacro_target,
+                             boiler_plate_top=ball_boiler_plate_top,
+                             boiler_plate_bot=ball_boiler_plate_bot,
+                             num_test=ball_num_test,
+                             param_test=ball_param_test)