Debug pc commands ros feedback

.github/workflows/build-and-test.sh

@@ -51,5 +51,6 @@ colcon build --packages-up-to buoy_tests --event-handlers console_direct+
 source $COLCON_WS/install/setup.bash
 
 # Test all buoy packages
-colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+
+#colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+
+launch_test install/buoy_tests/share/buoy_tests/launch/pc_commands_ros_feedback.launch.py
 colcon test-result

buoy_description/CMakeLists.txt

@@ -6,7 +6,7 @@ find_package(ament_cmake REQUIRED)
 set(BUOY_BASE_MODEL_PATH "/models/mbari_wec_base")
 file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}${BUOY_BASE_MODEL_PATH})
 add_custom_command(
-  OUTPUT model_gen_cmd
+  OUTPUT base_model_gen_cmd
   COMMAND python3 -m em
     ${CMAKE_CURRENT_SOURCE_DIR}${BUOY_BASE_MODEL_PATH}/model.sdf.em >
     ${CMAKE_CURRENT_BINARY_DIR}${BUOY_BASE_MODEL_PATH}/model.sdf
@@ -16,6 +16,17 @@ file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}${BUOY_MODEL_PATH})
 set(BUOY_ROS_MODEL_PATH "/models/mbari_wec_ros")
 file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}${BUOY_ROS_MODEL_PATH})
 
+add_custom_target(base_model_gen_target ALL
+  DEPENDS base_model_gen_cmd
+)
+
+add_custom_command(
+  OUTPUT model_gen_cmd
+  COMMAND python3 -m em
+    ${CMAKE_CURRENT_SOURCE_DIR}${BUOY_MODEL_PATH}/model.sdf.em >
+    ${CMAKE_CURRENT_BINARY_DIR}${BUOY_MODEL_PATH}/model.sdf
+)
+
 add_custom_target(model_gen_target ALL
   DEPENDS model_gen_cmd
 )

buoy_description/models/mbari_wec/model.sdf → buoy_description/models/mbari_wec/model.sdf.em

@@ -1,4 +1,11 @@
 <?xml version="1.0" ?>
+@{
+try:
+    scale_factor
+except NameError:
+    scale_factor = 1.0  # not defined so default
+}@
+
 <sdf version="1.8">
   <model name="MBARI_WEC">
 
@@ -11,6 +18,7 @@
       <PistonArea>1.375</PistonArea>
       <HydMotorDisp>0.30</HydMotorDisp>
       <RotorInertia>1</RotorInertia>
+      <ScaleFactor>@(scale_factor)</ScaleFactor>
     </plugin>
 
     <!-- Upper Polytropic Spring plugin -->

buoy_description/models/mbari_wec_base/model.sdf.em

@@ -27,13 +27,23 @@ tether_top_length = 2.5
 
 num_tether_bottom_links = 5
 
-# Heave cone
-heave_total_mass = 817
-trefoil_mass = 10
+# TrefoilDoors
+trefoil_pose = {'open': 1.047, 'closed': 0.0}
+try:
+    door_state
+except NameError:
+    door_state = 'closed'  # not defined so default
+
+if 'open' in door_state:
+    mu_zz = 3000.0
 
 # Heave cone
 heave_total_mass = 817
-trefoil_mass = 10
+trefoil_mass = 20
+try:
+    mu_zz
+except NameError:
+    mu_zz = 10000.0  # not defined so default
 
 ###################
 # Computed values #
@@ -449,18 +459,11 @@ pto_scale = pto_inner_radius / pto_stl_inner_radius
       </axis>
     </joint>
 
-    <joint name="TrefoilDoors" type="revolute">
+    <joint name="TrefoilDoors" type="fixed">
       <parent>HeaveCone</parent>
       <child>Trefoil</child>
       <provide_feedback>1</provide_feedback>
-      <pose>0.0 0.0 0.0 0 0 0</pose>
-      <axis>
-        <limit>
-          <lower>0.0</lower>
-          <upper>1.047</upper>
-        </limit>
-        <xyz>0.0 0.0 1.0</xyz>
-      </axis>
+      <pose>0.0 0.0 0.0 0 0 @(trefoil_pose[door_state])</pose>
     </joint>
 
   </model>

buoy_gazebo/CMakeLists.txt

@@ -143,6 +143,10 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
 )
 
+# Install batch running script
+install(PROGRAMS scripts/mbari_wec_batch DESTINATION lib/${PROJECT_NAME}/)
+install(FILES scripts/example_sim_params.yaml DESTINATION share/${PROJECT_NAME})
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

buoy_gazebo/launch/mbari_wec.launch.py

@@ -61,12 +61,16 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(
             os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py'),
         ),
-        launch_arguments={'gz_args': PathJoinSubstitution([
-            pkg_buoy_gazebo,
-            'worlds',
-            LaunchConfiguration('world_file')
-        ]),
-            'debugger': LaunchConfiguration('debugger')}.items(),
+        launch_arguments={'gz_args': [
+            LaunchConfiguration('gz_args'), PathJoinSubstitution([' ']),
+            PathJoinSubstitution([
+                pkg_buoy_gazebo,
+                'worlds',
+                LaunchConfiguration('world_file')
+                ])
+            ],
+            'debugger': LaunchConfiguration('debugger'),
+            'on_exit_shutdown': 'True'}.items(),
     )
 
     # Bridge to forward tf and joint states to ros2

buoy_gazebo/launch/mbari_wec_batch.launch.py

@@ -0,0 +1,51 @@
+# Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Launch Gazebo world with a buoy."""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, Shutdown
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    pkg_buoy_gazebo = get_package_share_directory('buoy_gazebo')
+    example_sim_params_yaml = os.path.join(pkg_buoy_gazebo, 'example_sim_params.yaml')
+
+    sim_params_yaml_launch_arg = DeclareLaunchArgument(
+        'sim_params_yaml', default_value=[example_sim_params_yaml],
+        description='Input batch sim params yaml'
+    )
+
+    batch_sim = Node(
+        package='buoy_gazebo',
+        executable='mbari_wec_batch',
+        arguments=[
+            LaunchConfiguration('sim_params_yaml'),
+        ],
+        output='screen',
+        on_exit=Shutdown()
+    )
+
+    return LaunchDescription([
+        sim_params_yaml_launch_arg,
+        batch_sim,
+    ])

buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py

@@ -66,7 +66,8 @@ def generate_launch_description():
             'worlds',
             LaunchConfiguration('world_file')
         ]),
-            'debugger': LaunchConfiguration('debugger')}.items(),
+            'debugger': LaunchConfiguration('debugger'),
+            'on_exit_shutdown': 'True'}.items(),
     )
 
     # Bridge to forward tf and joint states to ros2

buoy_gazebo/scripts/example_sim_params.yaml

@@ -0,0 +1,3 @@
+iterations: 5000
+door_state: ['closed', 'open']
+scale_factor: [0.5, 0.625, 0.75, 0.875, 1.0, 1.125, 1.25, 1.375, 1.4]

buoy_gazebo/scripts/mbari_wec_batch

@@ -0,0 +1,107 @@
+#!/usr/bin/python3
+# Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Launch batch of buoy simulations."""
+
+from em import invoke as empy
+import numpy as np
+import yaml
+
+import os
+import time
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_simulations(sim_params_yaml):
+    with open(sim_params_yaml, 'r') as fd:
+        sim_params = yaml.load(fd, Loader=yaml.CLoader)
+
+    iterations = sim_params['iterations']
+    door_state = sim_params['door_state']
+    scale_factor = sim_params['scale_factor']
+    batch_params = list(zip(*[param.ravel() for param in np.meshgrid(iterations,
+                                                                     door_state,
+                                                                     scale_factor)]))
+
+    print(f'Generated {len(batch_params)} simulation runs:')
+    print(f'Iterations, Door State, Scale Factor\n{np.array(batch_params)}')
+
+    timestr = time.strftime("%Y%m%d%H%M%S")
+    with open(f'batch_runs_{timestr}.log', 'w') as fd:
+        fd.write(f'# Generated {len(batch_params)} simulation runs\n')
+        fd.write(f'Run Index, Iterations, Door State, Scale Factor\n')
+
+    pkg_buoy_gazebo = get_package_share_directory('buoy_gazebo')
+    pkg_buoy_description = get_package_share_directory('buoy_description')
+
+    model_dir = 'mbari_wec_base'
+    empy_base_sdf_file = os.path.join(pkg_buoy_description, 'models', model_dir, 'model.sdf.em')
+    base_sdf_file = os.path.join(pkg_buoy_description, 'models', model_dir, 'model.sdf')
+
+    model_dir = 'mbari_wec'
+    empy_sdf_file = os.path.join(pkg_buoy_description, 'models', model_dir, 'model.sdf.em')
+    sdf_file = os.path.join(pkg_buoy_description, 'models', model_dir, 'model.sdf')
+
+    for idx, (itx, ds, sf) in enumerate(batch_params):
+        print(f'\n\nSim run [{idx}] for {itx} iterations: door state={ds}, scale factor={sf}\n\n')
+        empy(['-D', f"door_state = '{ds}'",
+              '-o', base_sdf_file,
+              empy_base_sdf_file])
+
+        empy(['-D', f"scale_factor = '{sf}'",
+              '-o', sdf_file,
+              empy_sdf_file])
+
+        def generate_launch_description():
+            mbari_wec = IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(pkg_buoy_gazebo, 'launch', 'mbari_wec.launch.py'),
+                ),
+                launch_arguments={'gz_args': f'-rs --iterations {itx}'}.items(),
+            )
+
+            rosbag2_name = f'rosbag2_batch_sim_{idx}'
+            rosbag2 = ExecuteProcess(
+                cmd=['ros2', 'bag', 'record', '-o', rosbag2_name, '-a'],
+                output='screen'
+            )
+
+            with open(f'batch_runs_{timestr}.log', 'a') as fd:
+                fd.write(f'{idx, itx, ds, sf}\n')
+
+            return LaunchDescription([
+                rosbag2,
+                mbari_wec
+            ])
+
+        ls = LaunchService()
+        ls.include_launch_description(generate_launch_description())
+        result = ls.run()
+        if result:
+            print(f'Simulation run [{idx}] was not successful: return code [{result}]')
+
+if __name__=='__main__':
+    import argparse
+    parser = argparse.ArgumentParser(description='Batch running for buoy_sim')
+    parser.add_argument('sim_params_yaml',
+                        help='yaml file with batch parameters')
+    args = parser.parse_args()
+    generate_simulations(args.sim_params_yaml)

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp

@@ -54,12 +54,15 @@ class ElectroHydraulicPTOPrivate
   /// \brief Piston joint entity
   gz::sim::Entity PrismaticJointEntity{gz::sim::kNullEntity};
 
-/// \brief Piston area
+  /// \brief Piston area
   double PistonArea{1.0};
 
   /// \brief Rotor Inertia
   double RotorInertia{1.0};
 
+  /// \brief Default Scale Factor
+  double DefaultScaleFactor{DEFAULT_SCALE_FACTOR};
+
   /// \brief Model interface
   gz::sim::Model model{gz::sim::kNullEntity};
 
@@ -72,6 +75,7 @@ class ElectroHydraulicPTOPrivate
   static constexpr double I_BattChargeMax{7.0};
   static constexpr double MaxTargetVoltage{325.0};
 
+
 // Dummy compensator pressure for ROS messages, not simulated
   static constexpr double CompensatorPressure{2.91};
 
@@ -133,6 +137,9 @@ void ElectroHydraulicPTO::Configure(
     this->dataPtr->PistonArea = SdfParamDouble(_sdf, "PistonArea", this->dataPtr->PistonArea);
   }
 
+  this->dataPtr->DefaultScaleFactor =
+    SdfParamDouble(_sdf, "ScaleFactor", this->dataPtr->DefaultScaleFactor);
+
   if (_sdf->HasElement("VelMode")) {
     this->dataPtr->VelMode = true;
   }
@@ -247,7 +254,7 @@ void ElectroHydraulicPTO::PreUpdate(
   if (pto_state.scale_command) {
     this->dataPtr->functor.I_Wind.ScaleFactor = pto_state.scale_command.value();
   } else {
-    this->dataPtr->functor.I_Wind.ScaleFactor = DEFAULT_SCALE_FACTOR;
+    this->dataPtr->functor.I_Wind.ScaleFactor = this->dataPtr->DefaultScaleFactor;
   }
 
   if (pto_state.retract_command) {