Merge branch 'main' into port/7_to_main

Signed-off-by: Michael Carroll <[email protected]>

.github/ci/packages.apt

@@ -30,6 +30,8 @@ libxi-dev
 libxmu-dev
 python3-distutils
 python3-gz-math7
+python3-gz-msgs10
+python3-gz-transport13
 python3-pybind11
 python3-pytest
 python3-sdformat14

CMakeLists.txt

@@ -205,16 +205,15 @@ set(Protobuf_IMPORT_DIRS ${gz-msgs10_INCLUDE_DIRS})
 if (SKIP_PYBIND11)
   message(STATUS "SKIP_PYBIND11 set - disabling python bindings")
 else()
-  find_package(PythonLibs QUIET)
-  if (NOT PYTHONLIBS_FOUND)
+  find_package(Python3 QUIET COMPONENTS Interpreter Development)
+  if (NOT Python3_FOUND)
     GZ_BUILD_WARNING("Python is missing: Python interfaces are disabled.")
     message (STATUS "Searching for Python - not found.")
   else()
     message (STATUS "Searching for Python - found version ${PYTHONLIBS_VERSION_STRING}.")
 
     set(PYBIND11_PYTHON_VERSION 3)
-    find_package(Python3 QUIET COMPONENTS Interpreter Development)
-    find_package(pybind11 2.2 QUIET)
+    find_package(pybind11 2.9 CONFIG QUIET)
 
     if (pybind11_FOUND)
       message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.")

docker/build.bash

@@ -7,7 +7,7 @@
 if [ $# -eq 0 ]
 then
     echo "Usage: $0 <Gazebo meta-package name> <dockerfile>"
-    echo "Example: $0 ignition-blueprint ./Dockerfile.gz"
+    echo "Example: $0 gz-garden ./Dockerfile.gz"
     exit 1
 fi
 

examples/scripts/python_api/systems/test_system.py

@@ -0,0 +1,45 @@
+# Copyright (C) 2023 Open Source Robotics Foundation
+#
+# 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.
+
+from gz.math7 import Vector3d
+from gz.sim8 import Model, Link
+import random
+
+
+class TestSystem(object):
+    def __init__(self):
+        self.id = random.randint(1, 100)
+
+    def configure(self, entity, sdf, ecm, event_mgr):
+        self.model = Model(entity)
+        self.link = Link(self.model.canonical_link(ecm))
+        print("Configured on", entity)
+        print("sdf name:", sdf.get_name())
+        self.force = sdf.get_double("force")
+        print(f"Applying {self.force} N on link {self.link.name(ecm)}")
+
+    def pre_update(self, info, ecm):
+        if info.paused:
+            return
+
+        if info.iterations % 3000 == 0:
+            print(f"{self.id} {info.real_time} pre_update")
+
+            self.link.add_world_force(
+                ecm, Vector3d(0, 0, self.force),
+                Vector3d(random.random(), random.random(), 0))
+
+
+def get_system():
+    return TestSystem()

examples/standalone/lrauv_control/lrauv_control.py

@@ -0,0 +1,202 @@
+#
+# Copyright (C) 2023 Open Source Robotics Foundation
+#
+# 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.
+#
+#
+
+#
+# Check README for detailed instructions.
+# Note: You need to update the PYTHONPATH variable if it is not set
+# before for other python examples. You can use then following:
+# $ export PYTHONPATH=$PYTHONPATH:<path_to_gazebo_ws>/install/lib/python
+# Usage:
+#   $ gz sim -r worlds/lrauv_control_demo.sdf
+#   $ # python3 lrauv_control.py speed_m_s yaw_rad pitch_rad
+#   $ python3 lrauv_control.py 0.5 0.78 0.174
+#
+
+from gz.msgs10.double_pb2 import Double
+from gz.msgs10.odometry_pb2 import Odometry
+from gz.math7 import Quaterniond, Vector3d
+from gz.transport13 import Node
+
+from threading import Lock
+
+import math
+import sys
+import time
+
+
+# Helper class for the PID controller for
+# linear speed, pitch and yaw angles.
+class Controller:
+    def __init__(self):
+        # Mutex to synchronize internal variables.
+        self.controllerMutex = Lock()
+
+        # Desired state.
+        self.targetSpeed = 0
+        self.targetYawAngle = 0
+        self.targetPitchAngle = 0
+
+        # Errors
+        self.errorSpeed = 0
+        self.errorSpeedIntegral = 0
+        self.errorYawAngle = 0
+        self.errorPitchAngle = 0
+
+        # States to be tracked and controlled.
+        self.speed = 0
+        self.yawAngle = 0
+        self.pitchAngle = 0
+
+        # PID gains and error limits.
+        # PI for speed.
+        self.kSpeed = -30
+        self.kISpeed = -0.5
+        self.errorSpeedIntegralMax = 10
+
+        # P for yaw and pitch control.
+        self.kYawAngle = -0.5
+        self.kPitchAngle = 0.6
+
+    # Set the target states to be tracked,
+    # i.e. linear speed (m/s), pitch and yaw angles (rad).
+    def SetTargets(self, _speed, _yaw, _pitch):
+        with self.controllerMutex:
+            if _speed == 0 and (_yaw != 0 or _pitch != 0):
+                print("Speed needs to be non zero for non zero pitch and yaw angles")
+                return
+            self.targetSpeed = float(_speed)
+            self.targetYawAngle = float(_yaw)
+            self.targetPitchAngle = float(_pitch)
+
+    # Update the state of the vehicle.
+    def UpdateState(self, _speed, _yaw, _pitch):
+        with self.controllerMutex:
+            self.speed = _speed
+            self.yawAngle = _yaw
+            self.pitchAngle = _pitch
+
+            self.errorSpeed = self.targetSpeed - self.speed
+            self.errorSpeedIntegral = min(
+                self.errorSpeedIntegral + self.errorSpeed, self.errorSpeedIntegralMax
+            )
+            self.errorYawAngle = self.targetYawAngle - self.yawAngle
+            self.errorPitchAngle = self.targetPitchAngle - self.pitchAngle
+
+    # Generate control input to be applied to the thruster.
+    def SpeedControl(self):
+        return self.errorSpeed * self.kSpeed + self.errorSpeedIntegral * self.kISpeed
+
+    # Generate control input to be supplied to the yaw rudders.
+    def YawControl(self):
+        return self.errorYawAngle * self.kYawAngle
+
+    # Generate control input to be supplied to the pitch rudders.
+    def PitchControl(self):
+        return self.errorPitchAngle * self.kPitchAngle
+
+
+def main():
+    control = Controller()
+    argc = len(sys.argv)
+    if argc == 4:
+        targetSpeed = sys.argv[1]
+        targetYaw = sys.argv[2]
+        targetPitch = sys.argv[3]
+
+        # Target state : speed (m/s), yaw angle, pitch angle (rad).
+        control.SetTargets(targetSpeed, targetYaw, targetPitch)
+
+    node = Node()
+
+    # Propeller command publisher.
+    propellerTopicTethys = "/model/tethys/joint/propeller_joint/cmd_thrust"
+    propellerPubTethys = node.advertise(propellerTopicTethys, Double)
+
+    # Subscriber for vehicle pose.
+    def cbPos(odometry_msg: Odometry):
+        orientation = odometry_msg.pose.orientation
+
+        q = Quaterniond(orientation.w, orientation.x, orientation.y, orientation.z)
+
+        # Get the velocity of the vehicle.
+        velocity = Vector3d(
+            odometry_msg.twist.linear.x,
+            odometry_msg.twist.linear.y,
+            odometry_msg.twist.linear.z,
+        )
+
+        control.UpdateState(velocity.length(), q.yaw(), q.pitch())
+
+    node.subscribe(Odometry, "/model/tethys/odometry", cbPos)
+
+    # Rudder command publisher.
+    rudderTopicTethys = "/model/tethys/joint/vertical_fins_joint/0/cmd_pos"
+    rudderPubTethys = node.advertise(rudderTopicTethys, Double)
+
+    # Fin command publisher.
+    finTopicTethys = "/model/tethys/joint/horizontal_fins_joint/0/cmd_pos"
+    finPubTethys = node.advertise(finTopicTethys, Double)
+
+    while True:
+        with control.controllerMutex:
+            # Publish propeller command for speed.
+            propellerMsg = Double()
+            propellerMsg.data = control.SpeedControl()
+            propellerPubTethys.publish(propellerMsg)
+
+            # Publish rudder command for yaw.
+            rudderMsg = Double()
+            rudderMsg.data = control.YawControl()
+            rudderPubTethys.publish(rudderMsg)
+
+            # Publish fin command for pitch.
+            finMsg = Double()
+            finMsg.data = control.PitchControl()
+            finPubTethys.publish(finMsg)
+
+            # Print the states.
+            print("-----------------------")
+            print("States            ( target, current, error) : ")
+            print(
+                "Speed (m/s)       :  ",
+                str(round(control.targetSpeed, 2)),
+                "  ",
+                str(round(float(control.speed), 2)),
+                "  ",
+                str(round(float(control.errorSpeed), 2)),
+            )
+            print(
+                "Yaw angle (deg)   : ",
+                str(round(control.targetYawAngle * 180 / math.pi, 2)),
+                " ",
+                str(round(control.yawAngle * 180 / math.pi, 2)),
+                "  ",
+                str(round(control.errorYawAngle * 180 / math.pi, 2)),
+            )
+            print(
+                "Pitch angle (deg) : ",
+                str(round(control.targetPitchAngle * 180 / math.pi, 2)),
+                " ",
+                str(round(control.pitchAngle * 180 / math.pi, 2)),
+                "  ",
+                str(round(control.errorPitchAngle * 180 / math.pi, 2)),
+            )
+        time.sleep(0.2)
+
+
+if __name__ == "__main__":
+    main()

examples/standalone/multi_lrauv_race/multi_lrauv_race.py

@@ -0,0 +1,96 @@
+#
+# Copyright (C) 2023 Open Source Robotics Foundation
+#
+# 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.
+#
+#
+#
+# In each iteration, for each vehicle, generate a random fin angle and thrust
+# within reasonable limits, and send the command to the vehicle.
+#
+# Usage:
+#   $ gz sim -r worlds/multi_lrauv_race.sdf
+#   $ python3 multi_lrauv_race.py
+#
+# Note: You need to update the PYTHONPATH variable if it is not set
+# before for other python examples. You can use then following:
+# $ export PYTHONPATH=$PYTHONPATH:<path_to_gazebo_ws>/install/lib/python
+
+from gz.msgs10.double_pb2 import Double
+from gz.transport13 import Node
+
+import random
+import time
+
+
+# Find joint limits from tethys model.sdf
+def random_angle_within_limits(min=-0.261799, max=0.261799):
+    return random.uniform(min, max)
+
+
+# Nominal speed is thruster 300 rpm ~ 31.4 radians per second ~ 6.14 Newtons
+def random_thrust_within_limits(min=-6.14, max=6.14):
+    return random.uniform(min, max)
+
+
+def main():
+    # Set up node
+    node = Node()
+
+    # Set up publishers
+    ns = ["tethys", "triton", "daphne"]
+    rudder_pubs = [
+        node.advertise(
+            "/model/" + name + "/joint/vertical_fins_joint/0/cmd_pos", Double
+        )
+        for name in ns
+    ]
+    propeller_pubs = [
+        node.advertise("/model/" + name + "/joint/propeller_joint/cmd_thrust", Double)
+        for name in ns
+    ]
+
+    artificial_speedup = 1
+
+    # Set up messages
+    rudder_msg = Double()
+    propeller_msg = Double()
+
+    try:
+        while True:
+            for rudder_pub, propelled_pub, name in zip(rudder_pubs, propeller_pubs, ns):
+                rudder_msg.data = random_angle_within_limits(-0.01, 0.01)
+                rudder_pub.publish(rudder_msg)
+                propeller_msg.data = random_thrust_within_limits(
+                    -6.14 * artificial_speedup, 0
+                )
+                propelled_pub.publish(propeller_msg)
+                print(
+                    "Commanding: "
+                    + name
+                    + " ruddder angle "
+                    + str(round(rudder_msg.data, 4))
+                    + " rad, thrust "
+                    + str(round(propeller_msg.data, 2))
+                    + " Newtons."
+                )
+            print(
+                "----------------------------------------------------------------------"
+            )
+            time.sleep(0.5)
+    except KeyboardInterrupt:
+        print("\nProcess terminated")
+
+
+if __name__ == "__main__":
+    main()