diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index d3e916b0e2..96a302fe1f 100644 --- a/.github/ci/packages.apt +++ b/.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 diff --git a/CMakeLists.txt b/CMakeLists.txt index ff98ca2607..ec13f8da66 100644 --- a/CMakeLists.txt +++ b/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}.") diff --git a/docker/build.bash b/docker/build.bash index 0f7db5cc07..b3ee429514 100755 --- a/docker/build.bash +++ b/docker/build.bash @@ -7,7 +7,7 @@ if [ $# -eq 0 ] then echo "Usage: $0 " - echo "Example: $0 ignition-blueprint ./Dockerfile.gz" + echo "Example: $0 gz-garden ./Dockerfile.gz" exit 1 fi diff --git a/examples/scripts/python_api/systems/test_system.py b/examples/scripts/python_api/systems/test_system.py new file mode 100644 index 0000000000..fe4e807919 --- /dev/null +++ b/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() diff --git a/examples/standalone/lrauv_control/lrauv_control.py b/examples/standalone/lrauv_control/lrauv_control.py new file mode 100644 index 0000000000..8f7ad2cf21 --- /dev/null +++ b/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:/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() diff --git a/examples/standalone/multi_lrauv_race/multi_lrauv_race.py b/examples/standalone/multi_lrauv_race/multi_lrauv_race.py new file mode 100644 index 0000000000..fa565feed1 --- /dev/null +++ b/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:/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() diff --git a/examples/worlds/deprecated_ignition.sdf b/examples/worlds/deprecated_ignition.sdf deleted file mode 100644 index 50dfe997a1..0000000000 --- a/examples/worlds/deprecated_ignition.sdf +++ /dev/null @@ -1,82 +0,0 @@ - - - - - - - - - - - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - true - 0 0 10 0 0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -0.5 0.1 -0.9 - - - - true - - - - - 0 0 1 - 100 100 - - - - - - - 0 0 1 - 100 100 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - - - - diff --git a/examples/worlds/mimic_fast_slow_pendulums_world.sdf b/examples/worlds/mimic_fast_slow_pendulums_world.sdf new file mode 100644 index 0000000000..b42c8bc73a --- /dev/null +++ b/examples/worlds/mimic_fast_slow_pendulums_world.sdf @@ -0,0 +1,661 @@ + + + + + + + 0.001 + 1 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 1.0 + 0 0 0.5 0 0 0 + + 0.087 + 0 + 0 + 0.087 + 0 + 0.006 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 1.0 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 1.0 + + + + + + + + -0.5 0 2.1 -1.5708 0 0 + 0 + + 1.0 + 0 0 0.25 0 0 0 + + 0.024 + 0 + 0 + 0.024 + 0 + 0.006 + + + + 0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.25 0 0 0 + + + 0.1 + 0.5 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 0.2 0 0 0 + + + 0.1 + 0.5 + + + + + + + base + slow_link + + -1.0 0 0 + + + + + base + fast_link + + -1.0 0 0 + + + + + + + 0 3 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 1.0 + 0 0 0.5 0 0 0 + + 0.087 + 0 + 0 + 0.087 + 0 + 0.006 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 1.0 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 1.0 + + + + + + + + -0.5 0 2.1 -1.5708 0 0 + 0 + + 1.0 + 0 0 0.25 0 0 0 + + 0.024 + 0 + 0 + 0.024 + 0 + 0.006 + + + + 0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.25 0 0 0 + + + 0.1 + 0.5 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 0.2 0 0 0 + + + 0.1 + 0.5 + + + + + + + base + slow_link + + -1.0 0 0 + + 1 + + + + + + base + fast_link + + -1.0 0 0 + + + + + + + 0 -3 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 1.0 + 0 0 0.5 0 0 0 + + 0.087 + 0 + 0 + 0.087 + 0 + 0.006 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 1.0 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 1.0 + + + + + + + + -0.5 0 2.1 -1.5708 0 0 + 0 + + 1.0 + 0 0 0.25 0 0 0 + + 0.024 + 0 + 0 + 0.024 + 0 + 0.006 + + + + 0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.25 0 0 0 + + + 0.1 + 0.5 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 0.2 0 0 0 + + + 0.1 + 0.5 + + + + + + + base + slow_link + + -1.0 0 0 + + + + + base + fast_link + + -1.0 0 0 + + 1 + + + + + + + + diff --git a/examples/worlds/python_system_loader.sdf b/examples/worlds/python_system_loader.sdf new file mode 100644 index 0000000000..a483b095e2 --- /dev/null +++ b/examples/worlds/python_system_loader.sdf @@ -0,0 +1,106 @@ + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + test_system + 3000 + + + + + diff --git a/include/gz/sim/EventManager.hh b/include/gz/sim/EventManager.hh index ace4848c04..1237b801da 100644 --- a/include/gz/sim/EventManager.hh +++ b/include/gz/sim/EventManager.hh @@ -56,6 +56,9 @@ namespace gz /// \brief Constructor public: EventManager() = default; + public: EventManager(const EventManager &) = delete; + public: EventManager &operator=(const EventManager &) = delete; + /// \brief Destructor public: ~EventManager() = default; diff --git a/include/gz/sim/System.hh b/include/gz/sim/System.hh index c5ef4ef965..cc0139161e 100644 --- a/include/gz/sim/System.hh +++ b/include/gz/sim/System.hh @@ -102,7 +102,7 @@ namespace gz EventManager &_eventMgr) = 0; }; - /// \class ISystemConfigureParameters ISystem.hh ignition/gazebo/System.hh + /// \class ISystemConfigureParameters ISystem.hh gz/sim/System.hh /// \brief Interface for a system that declares parameters. /// /// ISystemConfigureParameters::ConfigureParameters is called after diff --git a/include/gz/sim/Util.hh b/include/gz/sim/Util.hh index e918cb7fb8..e8e4bed0e0 100644 --- a/include/gz/sim/Util.hh +++ b/include/gz/sim/Util.hh @@ -234,7 +234,7 @@ namespace gz /// 2. "../shapes.sdf" - This is referencing a relative world file. /// 3. "/home/user/shapes.sdf" - This is reference an absolute world /// file. - /// 4. "https://fuel.ignitionrobotics.org/1.0/openrobotics/worlds/shapes.sdf" + /// 4. "https://fuel.gazebosim.org/1.0/openrobotics/worlds/shapes.sdf" /// This is referencing a Fuel URI. This will download the world file. /// \param[in] _fuelResourceCache Path to a Fuel resource cache, if /// known. diff --git a/include/gz/sim/components/Model.hh b/include/gz/sim/components/Model.hh index 8b68028786..decd6776ed 100644 --- a/include/gz/sim/components/Model.hh +++ b/include/gz/sim/components/Model.hh @@ -58,8 +58,8 @@ namespace serializers { // Skip serializing models with //pose/@relative_to attribute // since deserialization will fail. This could be a nested model. - // see https://github.com/ignitionrobotics/ign-gazebo/issues/1071 - // Once https://github.com/ignitionrobotics/sdformat/issues/820 is + // see https://github.com/gazebosim/gz-sim/issues/1071 + // Once https://github.com/gazebosim/sdformat/issues/820 is // resolved, there should be an API that returns sdf::Errors objects // instead of printing console msgs so it would be easier to ignore // specific errors in Deserialize. diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 25c976c439..6caede67ec 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -63,6 +63,8 @@ target_link_libraries(${BINDINGS_MODULE_NAME} PRIVATE gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} ) +set_target_properties(${BINDINGS_MODULE_NAME} PROPERTIES CXX_VISIBILITY_PRESET "hidden") + target_compile_definitions(${BINDINGS_MODULE_NAME} PRIVATE BINDINGS_MODULE_NAME=${BINDINGS_MODULE_NAME}) @@ -84,17 +86,6 @@ target_link_libraries(${GZ_COMMON_BINDINGS_MODULE_NAME} PRIVATE configure_build_install_location(${BINDINGS_MODULE_NAME}) configure_build_install_location(${GZ_COMMON_BINDINGS_MODULE_NAME}) -if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") - # Workaround for Clang and pybind11 on Focal - # https://github.com/pybind/pybind11/issues/1604 - # Resolved by newer versions of pybind11 - if(${pybind11_VERSION} VERSION_LESS "2.4.4") - target_compile_options(common PRIVATE -fsized-deallocation) - target_compile_options(sim PRIVATE -fsized-deallocation) - endif() -endif() - - if (BUILD_TESTING) set(python_tests actor_TEST diff --git a/python/test/gz_test_deps/msgs.py b/python/test/gz_test_deps/msgs.py new file mode 100644 index 0000000000..e91bef7942 --- /dev/null +++ b/python/test/gz_test_deps/msgs.py @@ -0,0 +1,3 @@ +import sys +import gz.msgs10 +sys.modules["gz_test_deps.msgs"] = gz.msgs10 diff --git a/python/test/gz_test_deps/transport.py b/python/test/gz_test_deps/transport.py new file mode 100644 index 0000000000..242eeb064a --- /dev/null +++ b/python/test/gz_test_deps/transport.py @@ -0,0 +1 @@ +from gz.transport13 import * diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py index b4e91602f2..35071869e1 100755 --- a/python/test/link_TEST.py +++ b/python/test/link_TEST.py @@ -18,7 +18,7 @@ from gz_test_deps.common import set_verbosity from gz_test_deps.sim import K_NULL_ENTITY, TestFixture, Link, Model, World, world_entity -from gz_test_deps.math import Matrix3d, Vector3d +from gz_test_deps.math import Matrix3d, Vector3d, Pose3d class TestModel(unittest.TestCase): post_iterations = 0 @@ -59,9 +59,8 @@ def on_pre_udpate_cb(_info, _ecm): # Visuals Test self.assertNotEqual(K_NULL_ENTITY, link.visual_by_name(_ecm, 'visual_test')) self.assertEqual(1, link.visual_count(_ecm)) - # World Pose Test - self.assertEqual(None, link.world_pose(_ecm)) - self.assertEqual(None, link.world_inertial_pose(_ecm)) + # World Inertial Pose Test. + self.assertEqual(Pose3d(), link.world_inertial_pose(_ecm)) # World Velocity Test self.assertEqual(None, link.world_linear_velocity(_ecm)) self.assertEqual(None, link.world_angular_velocity(_ecm)) diff --git a/python/test/plugins/test_model_system.py b/python/test/plugins/test_model_system.py new file mode 100644 index 0000000000..d390022537 --- /dev/null +++ b/python/test/plugins/test_model_system.py @@ -0,0 +1,74 @@ +# 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. + +import sys +from os.path import dirname, realpath +import threading + +# Add "../__file__" to sys.path to get gz_test_deps +sys.path.append(dirname(dirname(realpath(__file__)))) + +from gz_test_deps.sim import Model +from gz_test_deps.transport import Node +from gz_test_deps.msgs.pose_pb2 import Pose +from gz_test_deps.msgs.clock_pb2 import Clock + + +# Test system to be used with test/integration/python_system_loader.cc +class TestModelSystem(object): + def __init__(self): + self.node = Node() + self.has_been_reset = False + self.lock = threading.Lock() + self.sim_time_from_clock = None + + def configure(self, entity, sdf, ecm, event_mgr): + self.model = Model(entity) + if not self.model.valid(ecm): + raise RuntimeError(f"Model {entity} is invalid") + + self.target_pose = sdf.get_pose("target_pose") + self.reset_pose = sdf.get_pose("reset_pose") + self.pub = self.node.advertise(f"{self.model.name(ecm)}/pose", Pose) + self.sub = self.node.subscribe(Clock, "/clock", self.clock_cb) + + def pre_update(self, info, ecm): + if info.paused or self.has_been_reset: + return + + self.model.set_world_pose_cmd(ecm, self.target_pose) + + def post_update(self, info, ecm): + msg = Pose() + msg.position.x = self.target_pose.x() + msg.position.y = self.target_pose.y() + msg.position.z = self.target_pose.z() + with self.lock: + if self.sim_time_from_clock is not None: + stamp = msg.header.stamp + stamp.sec = self.sim_time_from_clock.sec + stamp.nsec = self.sim_time_from_clock.nsec + self.pub.publish(msg) + + def reset(self, info, ecm): + self.model.set_world_pose_cmd(ecm, self.reset_pose) + self.has_been_reset = True + + def clock_cb(self, msg): + with self.lock: + self.sim_time_from_clock = msg.sim + + +def get_system(): + return TestModelSystem() diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 49a9b81580..83adc9dc0d 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -218,6 +218,12 @@ target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE gz-plugin${GZ_PLUGIN_VER}::loader ) + +if (pybind11_FOUND) + target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE pybind11::embed) + target_compile_definitions(${PROJECT_LIBRARY_TARGET_NAME} PUBLIC -DHAVE_PYBIND11) +endif() + if (UNIX AND NOT APPLE) target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE stdc++fs) diff --git a/src/Link.cc b/src/Link.cc index 8378661dbe..426c57e780 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -186,7 +186,8 @@ bool Link::WindMode(const EntityComponentManager &_ecm) const std::optional Link::WorldPose( const EntityComponentManager &_ecm) const { - return _ecm.ComponentData(this->dataPtr->id); + return _ecm.ComponentData(this->dataPtr->id) + .value_or(sim::worldPose(this->dataPtr->id, _ecm)); } ////////////////////////////////////////////////// @@ -194,12 +195,13 @@ std::optional Link::WorldInertialPose( const EntityComponentManager &_ecm) const { auto inertial = _ecm.Component(this->dataPtr->id); - auto worldPose = _ecm.Component(this->dataPtr->id); + auto worldPose = _ecm.ComponentData(this->dataPtr->id) + .value_or(sim::worldPose(this->dataPtr->id, _ecm)); - if (!worldPose || !inertial) + if (!inertial) return std::nullopt; - return std::make_optional(worldPose->Data() * inertial->Data().Pose()); + return std::make_optional(worldPose * inertial->Data().Pose()); } ////////////////////////////////////////////////// @@ -216,17 +218,17 @@ std::optional Link::WorldLinearVelocity( { auto worldLinVel = _ecm.Component(this->dataPtr->id); - auto worldPose = - _ecm.Component(this->dataPtr->id); + auto worldPose = _ecm.ComponentData(this->dataPtr->id) + .value_or(sim::worldPose(this->dataPtr->id, _ecm)); auto worldAngVel = _ecm.Component(this->dataPtr->id); - if (!worldLinVel || !worldPose || !worldAngVel) + if (!worldLinVel || !worldAngVel) return std::nullopt; return std::make_optional( worldLinVel->Data() + - worldAngVel->Data().Cross(worldPose->Data().Rot().RotateVector(_offset))); + worldAngVel->Data().Cross(worldPose.Rot().RotateVector(_offset))); } ////////////////////////////////////////////////// @@ -326,13 +328,14 @@ std::optional Link::WorldInertiaMatrix( const EntityComponentManager &_ecm) const { auto inertial = _ecm.Component(this->dataPtr->id); - auto worldPose = _ecm.Component(this->dataPtr->id); + auto worldPose = _ecm.ComponentData(this->dataPtr->id) + .value_or(sim::worldPose(this->dataPtr->id, _ecm)); - if (!worldPose || !inertial) + if (!inertial) return std::nullopt; const math::Pose3d &comWorldPose = - worldPose->Data() * inertial->Data().Pose(); + worldPose * inertial->Data().Pose(); return std::make_optional( math::Inertiald(inertial->Data().MassMatrix(), comWorldPose).Moi()); } @@ -367,17 +370,18 @@ void Link::AddWorldForce(EntityComponentManager &_ecm, const math::Vector3d &_force) const { auto inertial = _ecm.Component(this->dataPtr->id); - auto worldPose = _ecm.Component(this->dataPtr->id); + auto worldPose = _ecm.ComponentData(this->dataPtr->id) + .value_or(sim::worldPose(this->dataPtr->id, _ecm)); // Can't apply force if the inertial's pose is not found - if (!inertial || !worldPose) + if (!inertial) return; // We want the force to be applied at the center of mass, but // ExternalWorldWrenchCmd applies the force at the link origin so we need to // compute the resulting force and torque on the link origin. auto posComWorldCoord = - worldPose->Data().Rot().RotateVector(inertial->Data().Pose().Pos()); + worldPose.Rot().RotateVector(inertial->Data().Pose().Pos()); math::Vector3d torque = posComWorldCoord.Cross(_force); @@ -390,16 +394,17 @@ void Link::AddWorldForce(EntityComponentManager &_ecm, const math::Vector3d &_position) const { auto inertial = _ecm.Component(this->dataPtr->id); - auto worldPose = _ecm.Component(this->dataPtr->id); + auto worldPose = _ecm.ComponentData(this->dataPtr->id) + .value_or(sim::worldPose(this->dataPtr->id, _ecm)); // Can't apply force if the inertial's pose is not found - if (!inertial || !worldPose) + if (!inertial) return; // We want the force to be applied at an offset from the center of mass, but // ExternalWorldWrenchCmd applies the force at the link origin so we need to // compute the resulting force and torque on the link origin. - auto posComWorldCoord = worldPose->Data().Rot().RotateVector( + auto posComWorldCoord = worldPose.Rot().RotateVector( _position + inertial->Data().Pose().Pos()); math::Vector3d torque = posComWorldCoord.Cross(_force); diff --git a/src/Server.cc b/src/Server.cc index 28a3d01725..70d676916c 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -17,6 +17,10 @@ #include +#ifdef HAVE_PYBIND11 +#include +#endif + #include #include #include @@ -55,6 +59,24 @@ struct DefaultWorld Server::Server(const ServerConfig &_config) : dataPtr(new ServerPrivate) { +#ifdef HAVE_PYBIND11 + if (Py_IsInitialized() == 0) + { + // We can't used pybind11::scoped_interpreter because: + // 1. It gets destructed before plugins are unloaded, which can cause + // segfaults if the plugin tries to run python code, e.g. a message + // that arrives during destruction. + // 2. It will prevent instantiation of other Servers. Running python + // systems will not be supported with multiple servers in the same + // process, but we shouldn't break existing behior for non-python use + // cases. + // This means, we will not be calling pybind11::finalize_interpreter to + // clean up the interpreter. This could cause issues with tests suites that + // have multiple tests that load python systems. + pybind11::initialize_interpreter(); + } +#endif + this->dataPtr->config = _config; // Configure the fuel client diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 97a99ae791..b8a33fd965 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -546,17 +546,7 @@ const std::string &ServerConfig::RenderEngineServer() const ///////////////////////////////////////////////// void ServerConfig::SetRenderEngineServer(const std::string &_engine) { - // Deprecated: accept ignition-prefixed engines - std::string deprecatedPrefix{"ignition"}; - auto engine = _engine; - auto pos = engine.find(deprecatedPrefix); - if (pos != std::string::npos) - { - engine.replace(pos, deprecatedPrefix.size(), "gz"); - gzwarn << "Trying to load deprecated engine [" << _engine - << "] for the server. Use [" << engine << "] instead." << std::endl; - } - this->dataPtr->renderEngineServer = engine; + this->dataPtr->renderEngineServer = _engine; } ///////////////////////////////////////////////// @@ -593,17 +583,7 @@ const std::string &ServerConfig::RenderEngineGui() const ///////////////////////////////////////////////// void ServerConfig::SetRenderEngineGui(const std::string &_engine) { - // Deprecated: accept ignition-prefixed engines - std::string deprecatedPrefix{"ignition"}; - auto engine = _engine; - auto pos = engine.find(deprecatedPrefix); - if (pos != std::string::npos) - { - engine.replace(pos, deprecatedPrefix.size(), "gz"); - gzwarn << "Trying to load deprecated engine [" << _engine - << "] for the GUI. Use [" << engine << "] instead." << std::endl; - } - this->dataPtr->renderEngineGui = engine; + this->dataPtr->renderEngineGui = _engine; } ///////////////////////////////////////////////// diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 97b3930c55..80df07819e 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -45,7 +45,7 @@ struct LoggingPlugin public: static std::string &LoggingPluginFileName() { static std::string recordPluginFileName = - std::string("ignition-gazebo") + + std::string("gz-sim") + GZ_SIM_MAJOR_VERSION_STR + "-log-system"; return recordPluginFileName; } diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 02d417e754..a52fcb030a 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -18,6 +18,9 @@ #include "SimulationRunner.hh" #include +#ifdef HAVE_PYBIND11 +#include +#endif #include #include @@ -53,6 +56,34 @@ using namespace sim; using StringSet = std::unordered_set; +namespace { +#ifdef HAVE_PYBIND11 +// Helper struct to maybe do a scoped release of the Python GIL. There are a +// number of ways where releasing and acquiring the GIL is not necessary: +// 1. Gazebo is built without Pybind11 +// 2. The python interpreter is not initialized. This could happen in tests that +// create a SimulationRunner without sim::Server where the interpreter is +// intialized. +// 3. sim::Server was instantiated by a Python module. In this case, there's a +// chance that this would be called with the GIL already released. +struct MaybeGilScopedRelease +{ + MaybeGilScopedRelease() + { + if (Py_IsInitialized() != 0 && PyGILState_Check() == 1) + { + this->release.emplace(); + } + } + std::optional release; +}; +#else + struct MaybeGilScopedRelease + { + }; +#endif +} + ////////////////////////////////////////////////// SimulationRunner::SimulationRunner(const sdf::World *_world, @@ -570,6 +601,10 @@ void SimulationRunner::UpdateSystems() // the barriers will be uninitialized, so guard against that condition. if (this->postUpdateStartBarrier && this->postUpdateStopBarrier) { + // Release the GIL from the main thread to run PostUpdate threads which + // might be calling into python. The system that does call into python + // needs to lock the GIL from its thread. + MaybeGilScopedRelease release; this->postUpdateStartBarrier->Wait(); this->postUpdateStopBarrier->Wait(); } diff --git a/src/SystemLoader.cc b/src/SystemLoader.cc index 1807819674..582ebbbabf 100644 --- a/src/SystemLoader.cc +++ b/src/SystemLoader.cc @@ -64,7 +64,8 @@ class gz::sim::SystemLoaderPrivate public: bool InstantiateSystemPlugin(const sdf::Plugin &_sdfPlugin, gz::plugin::PluginPtr &_gzPlugin) { - // Deprecated: accept ignition-gazebo-prefixed systems + // Deprecated: accept ignition-gazebo-prefixed systems. Remove this on + // gz-sim9 std::string deprecatedPrefix{"ignition-gazebo"}; auto filename = _sdfPlugin.Filename(); auto pos = filename.find(deprecatedPrefix); @@ -124,6 +125,7 @@ class gz::sim::SystemLoaderPrivate std::string pluginToInstantiate = _sdfPlugin.Name().empty() ? pluginName : _sdfPlugin.Name(); + // Deprecated: accept ignition plugins. Remove this on gz-sim9 std::string deprecatedPluginNamePrefix{"ignition::gazebo"}; pos = pluginToInstantiate.find(deprecatedPluginNamePrefix); if (pos != std::string::npos) diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 889234a9d0..6be8d27903 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -974,11 +974,11 @@ TEST_F(UtilTest, ResolveSdfWorldFile) // URI to a Fuel world. std::string fuelUri = - "https://fuel.ignitionrobotics.org/1.0/openrobotics/worlds/test world"; + "https://fuel.gazebosim.org/1.0/openrobotics/worlds/test world"; // The expect path for the local Fuel world. std::string expectedPath = common::joinPaths( - config.CacheLocation(), "fuel.ignitionrobotics.org", + config.CacheLocation(), "fuel.gazebosim.org", "openrobotics", "worlds", "test world"); // Get the Fuel world. diff --git a/src/gui/plugins/banana_for_scale/BananaForScale.cc b/src/gui/plugins/banana_for_scale/BananaForScale.cc index 6bfe427b8d..54b12621b1 100644 --- a/src/gui/plugins/banana_for_scale/BananaForScale.cc +++ b/src/gui/plugins/banana_for_scale/BananaForScale.cc @@ -44,10 +44,10 @@ namespace gz::sim } const char kBanana[] = - "https://fuel.ignitionrobotics.org/1.0/mjcarroll/models/banana for scale"; + "https://fuel.gazebosim.org/1.0/mjcarroll/models/banana for scale"; const char kBigBanana[] = - "https://fuel.ignitionrobotics.org/1.0/mjcarroll/models/big banana for scale"; + "https://fuel.gazebosim.org/1.0/mjcarroll/models/big banana for scale"; using namespace gz; using namespace sim; diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 5c2a39f71e..5b5cd794e7 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -1257,8 +1257,8 @@ void ComponentInspector::QuerySystems() // Remove common prefixes and suffixes auto humanReadable = plugin.filename(); - removePrefix("ignition-gazebo-", humanReadable); - removePrefix("ignition-gazebo" + + removePrefix("gz-sim-", humanReadable); + removePrefix("gz-sim" + std::string(GZ_SIM_MAJOR_VERSION_STR) + "-", humanReadable); removeSuffix("-system", humanReadable); removeSuffix("system", humanReadable); diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 475e099903..0c833a60ca 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -2797,17 +2797,7 @@ void RenderUtil::ShowGrid() ///////////////////////////////////////////////// void RenderUtil::SetEngineName(const std::string &_name) { - // Deprecated: accept ignition-prefixed engines - std::string deprecatedPrefix{"ignition"}; - auto name = _name; - auto pos = name.find(deprecatedPrefix); - if (pos != std::string::npos) - { - name.replace(pos, deprecatedPrefix.size(), "gz"); - gzwarn << "Trying to load deprecated engine [" << _name - << "] for the server. Use [" << name << "] instead." << std::endl; - } - this->dataPtr->engineName = name; + this->dataPtr->engineName = _name; } ///////////////////////////////////////////////// diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index c9c8f441cd..7eb7786908 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -147,6 +147,7 @@ add_subdirectory(performer_detector) add_subdirectory(perfect_comms) add_subdirectory(physics) add_subdirectory(pose_publisher) +add_subdirectory(python_system_loader) add_subdirectory(rf_comms) add_subdirectory(scene_broadcaster) add_subdirectory(sensors) diff --git a/src/systems/hydrodynamics/Hydrodynamics.cc b/src/systems/hydrodynamics/Hydrodynamics.cc index 8e53087ab7..171ca8f021 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.cc +++ b/src/systems/hydrodynamics/Hydrodynamics.cc @@ -293,16 +293,7 @@ void Hydrodynamics::Configure( gz::sim::EventManager &/*_eventMgr*/ ) { - if (_sdf->HasElement("waterDensity")) - { - gzwarn << - " parameter is deprecated and will be removed Ignition G.\n" - << "\tPlease update your SDF to use instead."; - } - - this->dataPtr->waterDensity = SdfParamDouble(_sdf, "waterDensity", - SdfParamDouble(_sdf, "water_density", 998) - ); + this->dataPtr->waterDensity = SdfParamDouble(_sdf, "water_density", 998); // Load stability derivatives // Use SNAME 1950 convention to load the coeffecients. const auto snameConventionVel = "UVWPQR"; diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index b8813f6bf8..df759ae79f 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -510,8 +510,7 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { auto msgType = iter->Type(); - // Support ignition.msgs for backwards compatibility, don't remove on tock - // so users can use logs across versions + // Support ignition.msgs for backwards compatibility. Remove on gz-sim9 std::string deprecatedPrefix{"ignition.msgs"}; auto pos = msgType.find(deprecatedPrefix); if (pos != std::string::npos) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 59cd31bc33..047e209db9 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -478,6 +478,10 @@ class gz::sim::systems::PhysicsPrivate JointFeatureList, gz::physics::sdf::ConstructSdfJoint>{}; + /// \brief Feature list for mimic constraints + public: struct MimicConstraintJointFeatureList : gz::physics::FeatureList< + physics::SetMimicConstraintFeature>{}; + ////////////////////////////////////////////////// // Detachable joints @@ -681,6 +685,7 @@ class gz::sim::systems::PhysicsPrivate physics::Joint, JointFeatureList, DetachableJointFeatureList, + MimicConstraintJointFeatureList, JointVelocityCommandFeatureList, JointGetTransmittedWrenchFeatureList, JointPositionLimitsCommandFeatureList, @@ -776,16 +781,6 @@ void Physics::Configure(const Entity &_entity, pluginLib = "gz-physics-dartsim-plugin"; } - // Deprecated: accept ignition-prefixed engines - std::string deprecatedPrefix{"ignition"}; - auto pos = pluginLib.find(deprecatedPrefix); - if (pos != std::string::npos) - { - auto msg = "Trying to load deprecated plugin [" + pluginLib + "]. Use ["; - pluginLib.replace(pos, deprecatedPrefix.size(), "gz"); - gzwarn << msg << pluginLib << "] instead." << std::endl; - } - // Update component if (!engineComp) { @@ -1683,6 +1678,62 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm, this->entityJointMap.AddEntity(_entity, existingJoint); this->topLevelModelMap.insert( std::make_pair(_entity, topLevelModel(_entity, _ecm))); + + // Check if mimic constraint should be applied to this joint's axes. + using AxisIndex = std::size_t; + std::map jointAxisByIndex; + auto jointAxis = _ecm.Component(_entity); + auto jointAxis2 = _ecm.Component(_entity); + + if (jointAxis) + { + jointAxisByIndex[0] = jointAxis->Data(); + } + + if (jointAxis2) + { + jointAxisByIndex[1] = jointAxis2->Data(); + } + + for (const auto &[axisIndex, axis] : jointAxisByIndex) + { + if (auto mimic = axis.Mimic()) + { + auto jointPtrMimic = this->entityJointMap + .EntityCast(existingJoint); + if (jointPtrMimic) + { + const auto leaderJoint = + basicModelPtrPhys->GetJoint(mimic->Joint()); + std::size_t leaderAxis = 0; + if (mimic->Axis() == "axis2") + { + leaderAxis = 1; + } + jointPtrMimic->SetMimicConstraint(axisIndex, + leaderJoint, + leaderAxis, + mimic->Multiplier(), + mimic->Offset(), + mimic->Reference()); + } + else + { + static bool informed{false}; + if (!informed) + { + gzerr << "Attempting to create a mimic constraint for joint [" + << _name->Data() + << "] but the chosen physics engine does not support " + << "mimic constraints, so no constraint will be " + << "created." + << std::endl; + informed = true; + } + } + } + } + return true; } diff --git a/src/systems/python_system_loader/CMakeLists.txt b/src/systems/python_system_loader/CMakeLists.txt new file mode 100644 index 0000000000..212dbc080b --- /dev/null +++ b/src/systems/python_system_loader/CMakeLists.txt @@ -0,0 +1,8 @@ +if (pybind11_FOUND) + gz_add_system(python-system-loader + SOURCES + PythonSystemLoader.cc + PRIVATE_LINK_LIBS + pybind11::embed + ) +endif() diff --git a/src/systems/python_system_loader/PythonSystemLoader.cc b/src/systems/python_system_loader/PythonSystemLoader.cc new file mode 100644 index 0000000000..d9dcdb0547 --- /dev/null +++ b/src/systems/python_system_loader/PythonSystemLoader.cc @@ -0,0 +1,223 @@ +/* + * 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. + * + */ + +#include "PythonSystemLoader.hh" + +#include + +#include +#include +#include +#include +#include +#include + +#include "gz/sim/Entity.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/EventManager.hh" +#include "gz/sim/System.hh" +#include "gz/sim/SystemLoader.hh" + +namespace py = pybind11; + + +using namespace gz; +using namespace sim; +using namespace systems; + +PythonSystemLoader::~PythonSystemLoader() +{ + if (this->pythonSystem) + { + if (py::hasattr(this->pythonSystem, "shutdown")) + { + this->pythonSystem.attr("shutdown")(); + } + } +} + +void PythonSystemLoader::Configure( + const Entity &_entity, const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, EventManager &_eventMgr) +{ + auto [moduleName, hasModule] = _sdf->Get("module_name", ""); + if (!hasModule) + { + gzerr << "PythonSystemLoader missing required argument . " + << "Failed to initialize." << std::endl; + return; + } + + // Load the `gz.sim` and sdformat modules to register all pybind bindings + // necessary for System interface functions + const auto gzSimModule = + std::string("gz.sim") + std::to_string(GZ_SIM_MAJOR_VERSION); + const auto sdformatModule = + std::string("sdformat") + std::to_string(SDF_MAJOR_VERSION); + py::module_ sysModule; + try + { + py::module::import(gzSimModule.c_str()); + py::module::import(sdformatModule.c_str()); + sysModule = py::module_::import("sys"); + } + catch (const pybind11::error_already_set &_err) + { + gzerr << "Error while loading required modules:\n" + << _err.what() << std::endl; + return; + } + + // Add GZ_SIM_SYSTEM_PLUGIN_PATH and other default plugin + // locations to PYTHONPATH + try + { + SystemLoader systemLoader; + for (const auto &pluginPath : systemLoader.PluginPaths()) + { + sysModule.attr("path").attr("append")(pluginPath); + } + } + catch (const std::runtime_error &_err) + { + gzerr << _err.what() << std::endl; + return; + } + + try + { + this->pythonModule = py::module_::import(moduleName.c_str()); + } + catch (const pybind11::error_already_set &_err) + { + gzerr << "Error while loading module " << moduleName << "\n" + << _err.what() << "\n"; + + gzerr << "Searched in:\n"; + for (const auto &path : sysModule.attr("path")) + { + gzerr << " \"" << path << "\"\n"; + } + return; + } + + try + { + py::object getSystem = this->pythonModule.attr("get_system"); + if (!getSystem) + { + gzerr << "Could not call 'get_system' on the module " << moduleName + << "\n"; + return; + } + this->pythonSystem = getSystem(); + } + catch (const pybind11::error_already_set &_err) + { + gzerr << _err.what() << std::endl; + return; + } + if (py::hasattr(this->pythonSystem, "configure")) + { + try + { + // _ecm and _eventMgr are passed in as pointers so pybind11 would use the + // write `return_value_policy`. Otherwise, it would use + // `return_value_policy = copy`. + this->pythonSystem.attr("configure")(_entity, _sdf, &_ecm, &_eventMgr); + } + catch (const pybind11::error_already_set &_err) + { + gzerr << _err.what() << std::endl; + return; + } + } + // TODO(azeey) Add support for ConfigureParameters + if (py::hasattr(this->pythonSystem, "pre_update")) + { + this->preUpdateMethod = this->pythonSystem.attr("pre_update"); + } + if (py::hasattr(this->pythonSystem, "update")) + { + this->updateMethod = this->pythonSystem.attr("update"); + } + if (py::hasattr(this->pythonSystem, "post_update")) + { + this->postUpdateMethod = this->pythonSystem.attr("post_update"); + } + if (py::hasattr(this->pythonSystem, "reset")) + { + this->resetMethod = this->pythonSystem.attr("reset"); + } + this->validConfig = true; +} + +////////////////////////////////////////////////// +template +void PythonSystemLoader::CallPythonMethod(py::object _method, Args &&..._args) +{ + if (!this->validConfig) + { + return; + } + + if (_method) + { + try + { + _method(std::forward(_args)...); + } + catch (const pybind11::error_already_set &_err) + { + gzerr << _err.what() << std::endl; + this->validConfig = false; + return; + } + } +} + +////////////////////////////////////////////////// +void PythonSystemLoader::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + CallPythonMethod(this->preUpdateMethod, _info, &_ecm); +} + +////////////////////////////////////////////////// +void PythonSystemLoader::Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + CallPythonMethod(this->updateMethod, _info, &_ecm); +} + +////////////////////////////////////////////////// +void PythonSystemLoader::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + py::gil_scoped_acquire gil; + CallPythonMethod(this->postUpdateMethod, _info, &_ecm); +} +////////////////////////////////////////////////// +void PythonSystemLoader::Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + CallPythonMethod(this->resetMethod, _info, &_ecm); +} + +GZ_ADD_PLUGIN(PythonSystemLoader, System, ISystemConfigure, ISystemPreUpdate, + ISystemUpdate, ISystemPostUpdate, ISystemReset) +GZ_ADD_PLUGIN_ALIAS(PythonSystemLoader, "gz::sim::systems::PythonSystemLoader") diff --git a/src/systems/python_system_loader/PythonSystemLoader.hh b/src/systems/python_system_loader/PythonSystemLoader.hh new file mode 100644 index 0000000000..9ba0673fd1 --- /dev/null +++ b/src/systems/python_system_loader/PythonSystemLoader.hh @@ -0,0 +1,127 @@ +/* + * 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. + * + */ + +#ifndef GZ_SIM_SYSTEMS_PYTHONSYSTEMLOADER_HH_ +#define GZ_SIM_SYSTEMS_PYTHONSYSTEMLOADER_HH_ + +#include + +#include +#include + +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/EventManager.hh" +#include "gz/sim/System.hh" +#include "gz/sim/config.hh" +#include "gz/sim/python-system-loader-system/Export.hh" + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems +{ +/// \brief Allows systems to be written in Python. +/// +/// The convention for a system written in Python supported by the +/// `PythonSystemLoader` is that it's a Python module providing a `get_system` +/// function which itself returns an instance of a class that implements the +/// various interfaces in gz::sim::System. The spelling of the interfaces +/// conforms to Python code style guides, so the following name mapping applies +/// +/// * `configure` : Corresponds to System::ISystemConfigure::Configure +/// * `pre_update` : Corresponds to System::ISystemPreUpdate::PreUpdate +/// * `update` : Corresponds to System::ISystemUpdate::Update +/// * `post_update` : Corresponds to System::ISystemPostUpdate::PostUpdate +/// * `reset` : Corresponds to System::ISystemReset::Reset +/// +/// It is not necessary to implement all the interfaces. PythonSystemLoader will +/// check if the corresponding method is implemented in the Python system and +/// skip it if it's not found. +/// +/// See examples/scripts/python_api/systems/test_system.py for an example +/// +/// ## Parameters +/// * : Name of python module to be loaded. The search path +/// includes `GZ_SIM_SYSTEM_PLUGIN_PATH` as well as +/// `PYTHONPATH`. +/// +/// The contents of the tag will be available in the configure method +/// of the Python system +// TODO(azeey) Add ParameterConfigure +class GZ_SIM_PYTHON_SYSTEM_LOADER_SYSTEM_HIDDEN PythonSystemLoader final + : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemUpdate, + public ISystemPostUpdate, + public ISystemReset +{ + public: ~PythonSystemLoader() final; + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) final; + + // Documentation inherited + public: void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + // Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) final; + + // Documentation inherited + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + /// \brief Function that calls each of the python equivalents of Configure, + /// PreUpdate, etc. + private: template + void CallPythonMethod(pybind11::object _method, Args&&...); + + /// \brief Whether we have a valid configuration after Configure has run. This + /// includes checking if the Python module is found and that the system is + /// instantiated. This is set to false in the *Update/Reset calls if an + /// exception is thrown by the Python module. + private: bool validConfig{false}; + /// \brief Represents the imported Python module + private: pybind11::module_ pythonModule; + /// \brief Represents the instantiated Python System + private: pybind11::object pythonSystem; + /// \brief Pointer to the PreUpdate method inside the Python System + private: pybind11::object preUpdateMethod; + /// \brief Pointer to the Update method inside the Python System + private: pybind11::object updateMethod; + /// \brief Pointer to the PostUpdate method inside the Python System + private: pybind11::object postUpdateMethod; + /// \brief Pointer to the Reset method inside the Python System + private: pybind11::object resetMethod; +}; +} // namespace systems +} // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace sim +} // namespace gz + +#endif /* end of include guard: GZ_SIM_SYSTEMS_PYTHONSYSTEMLOADER_HH_ */ diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index f503c48b11..776b86c76f 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -86,8 +86,8 @@ namespace systems /// * Attributes: /// * `name`: Service name (eg. `/world/triggered_publisher/set_pose`) /// * `timeout`: Service timeout - /// * `reqType`: Service request message type (eg. ignition.msgs.Pose) - /// * `repType`: Service response message type (eg. ignition.msgs.Empty) + /// * `reqType`: Service request message type (eg. gz.msgs.Pose) + /// * `repType`: Service response message type (eg. gz.msgs.Empty) /// * `reqMsg`: String used to construct the service protobuf message. /// /// Examples: diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index ee8290a5c4..ef2166b55f 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -138,6 +138,11 @@ else() endforeach(test) endif() +# Add tests that need pybind11 +if (${pybind11_FOUND}) + list(APPEND tests python_system_loader.cc) +endif() + if (MSVC) # Warning #4251 is the "dll-interface" warning that tells you when types used # by a class are not being exported. These generated source files have private @@ -234,3 +239,8 @@ if(VALID_DISPLAY AND VALID_DRI_DISPLAY AND TARGET INTEGRATION_sensors_system) gz-rendering${GZ_RENDERING_VER}::gz-rendering${GZ_RENDERING_VER} ) endif() + +if (TARGET INTEGRATION_python_system_loader) + set_tests_properties(INTEGRATION_python_system_loader PROPERTIES + ENVIRONMENT "PYTHONPATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}/python/") +endif() diff --git a/test/integration/battery_plugin.cc b/test/integration/battery_plugin.cc index 6873341208..7f31376af4 100644 --- a/test/integration/battery_plugin.cc +++ b/test/integration/battery_plugin.cc @@ -292,7 +292,7 @@ TEST_F(BatteryPluginTest, // the LinearBatteryPlugin is not zero when created. If // components::BatterySoC is zero on start, then the Physics plugin // can disable a joint. This in turn can prevent the joint from - // rotating. See https://github.com/ignitionrobotics/ign-gazebo/issues/55 + // rotating. See https://github.com/gazebosim/gz-sim/issues/55 EXPECT_GT(batComp->Data(), 0); EXPECT_GT(batComp2->Data(), 0); }; @@ -333,7 +333,7 @@ TEST_F(BatteryPluginTest, ///////////////////////////////////////////////// // Battery with power draining topics -// See https://github.com/ignitionrobotics/ign-gazebo/issues/1175 +// See https://github.com/gazebosim/gz-sim/issues/1175 TEST_F(BatteryPluginTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PowerDrainTopic)) { const auto sdfPath = common::joinPaths(std::string(PROJECT_SOURCE_PATH), diff --git a/test/integration/link.cc b/test/integration/link.cc index 10063b247a..6603633ae0 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -229,7 +229,6 @@ TEST_F(LinkIntegrationTest, LinkPoses) // Before we add components, pose functions should return nullopt - EXPECT_EQ(std::nullopt, link.WorldPose(ecm)); EXPECT_EQ(std::nullopt, link.WorldInertialPose(ecm)); math::Pose3d linkWorldPose; diff --git a/test/integration/python_system_loader.cc b/test/integration/python_system_loader.cc new file mode 100644 index 0000000000..baeed3ff33 --- /dev/null +++ b/test/integration/python_system_loader.cc @@ -0,0 +1,118 @@ +/* + * 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. + * + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "gz/sim/Server.hh" +#include "gz/sim/ServerConfig.hh" +#include "gz/sim/Util.hh" +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/Name.hh" +#include "helpers/EnvTestFixture.hh" +#include "plugins/MockSystem.hh" + +using namespace gz; +namespace components = gz::sim::components; + +class PythonSystemLoaderTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +void worldReset() +{ + gz::msgs::WorldControl req; + gz::msgs::Boolean rep; + req.mutable_reset()->set_all(true); + transport::Node node; + + unsigned int timeout = 1000; + bool result; + bool executed = + node.Request("/world/default/control", req, timeout, rep, result); + + ASSERT_TRUE(executed); + ASSERT_TRUE(result); + ASSERT_TRUE(rep.data()); +} + +///////////////////////////////////////////////// +TEST_F(PythonSystemLoaderTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(LoadMultipleSystems)) +{ + common::setenv("GZ_SIM_SYSTEM_PLUGIN_PATH", + common::joinPaths(std::string(PROJECT_SOURCE_PATH), "python", + "test", "plugins")); + + sim::ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "python_system_loader.sdf")); + + sim::Server server(serverConfig); + using namespace std::chrono_literals; + server.SetUpdatePeriod(1ns); + + std::optional postUpdateModelPose; + // Create a system that adds a recreate component to models + auto testSystem = std::make_shared(); + testSystem->postUpdateCallback = + [&](const sim::UpdateInfo &, const sim::EntityComponentManager &_ecm) + { + auto testModel = _ecm.EntityByComponents(components::Model(), + components::Name("box")); + if (testModel != sim::kNullEntity) + { + postUpdateModelPose = sim::worldPose(testModel, _ecm); + } + return false; + }; + server.AddSystem(testSystem); + server.Run(true, 1, false); + ASSERT_TRUE(postUpdateModelPose.has_value()); + EXPECT_EQ(math::Pose3d(0, 0, 10, 0, 0, 0), *postUpdateModelPose); + + server.RunOnce(true); + std::optional afterResetModelPose; + // Since the order of reset callbacks may not be reliable, we'll use the + // following PreUpdate call to record the model pose after reset. The test + // system does not set the world pose after reset. + testSystem->preUpdateCallback = ( + [&](const sim::UpdateInfo &, const sim::EntityComponentManager &_ecm) + { + auto testModel = _ecm.EntityByComponents(components::Model(), + components::Name("box")); + if (testModel != sim::kNullEntity) + { + afterResetModelPose = sim::worldPose(testModel, _ecm); + } + return false; + }); + + worldReset(); + server.Run(true, 400, false); + ASSERT_TRUE(afterResetModelPose.has_value()); + EXPECT_NEAR(10.0, afterResetModelPose->X(), 1e-3); +} diff --git a/test/worlds/battery_thruster_consumer.sdf b/test/worlds/battery_thruster_consumer.sdf index 58afe3720c..05329ab3e6 100644 --- a/test/worlds/battery_thruster_consumer.sdf +++ b/test/worlds/battery_thruster_consumer.sdf @@ -11,12 +11,12 @@ 0 0 0 + filename="gz-sim-physics-system" + name="gz::sim::systems::Physics"> + filename="gz-sim-scene-broadcaster-system" + name="gz::sim::systems::SceneBroadcaster"> @@ -95,8 +95,8 @@ + filename="gz-sim-thruster-system" + name="gz::sim::systems::Thruster"> lowbattery propeller_joint 0.005 @@ -108,8 +108,8 @@ thrust - + linear_battery 12.592 12.694 @@ -186,8 +186,8 @@ + filename="gz-sim-thruster-system" + name="gz::sim::systems::Thruster"> lowbattery propeller_joint 0.005 @@ -201,8 +201,8 @@ linear_battery2 - + linear_battery2 12.592 12.694 diff --git a/test/worlds/hydrodynamics.sdf.in b/test/worlds/hydrodynamics.sdf.in index 7e86144be7..627d6589b1 100644 --- a/test/worlds/hydrodynamics.sdf.in +++ b/test/worlds/hydrodynamics.sdf.in @@ -27,7 +27,7 @@ diff --git a/test/worlds/python_system_loader.sdf b/test/worlds/python_system_loader.sdf new file mode 100644 index 0000000000..d7c2ba1e6f --- /dev/null +++ b/test/worlds/python_system_loader.sdf @@ -0,0 +1,89 @@ + + + + + + 0 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + test_model_system + 0 0 10 0 0 0 + 10 0 0 0 0 0 + + + + + 0 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + test_model_system + 0 0 10 0 0 0 + 10 0 0 0 0 0 + + + + diff --git a/tutorials/erb_template.md b/tutorials/erb_template.md index e38f5cd1d4..d6dbbe350e 100644 --- a/tutorials/erb_template.md +++ b/tutorials/erb_template.md @@ -116,11 +116,11 @@ Instead of simple shapes, you can also use a nested loop to generate 100 actors - https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae + https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae 1.0 - https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae + https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae 0.055 true diff --git a/tutorials/migration_actor_api.md b/tutorials/migration_actor_api.md index d1aacda04f..065a0b19d8 100644 --- a/tutorials/migration_actor_api.md +++ b/tutorials/migration_actor_api.md @@ -34,10 +34,10 @@ can be divided in these categories: You'll find the Gazebo APIs below on the following headers: -* [ignition/gazebo/Actor.hh](https://gazebosim.org/api/gazebo/7/Actor_8hh.html) -* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/7/Util_8hh.html) -* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/7/SdfEntityCreator_8hh.html) -* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) +* [gz/sim/Actor.hh](https://gazebosim.org/api/gazebo/7/Actor_8hh.html) +* [gz/sim/Util.hh](https://gazebosim.org/api/gazebo/7/Util_8hh.html) +* [gz/sim/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/7/SdfEntityCreator_8hh.html) +* [gz/sim/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) It's worth remembering that most of this functionality can be performed using the @@ -75,15 +75,15 @@ CustomTrajectory | see `SetCustomTrajectory` DirtyPose | Not supported FillMsg | TODO GetAutoDisable | TODO -GetId | `ignition::gazebo::Model::Entity` -GetName | `ignition::gazebo::Actor::Name` +GetId | `gz::sim::Model::Entity` +GetName | `gz::sim::Actor::Name` GetPluginCount | TODO GetSaveable | Not supported -GetScopedName | `ignition::gazebo::scopedName` +GetScopedName | `gz::sim::scopedName` GetSDF | TODO GetSDFDom | TODO GetSelfCollide | TODO -GetType | `ignition::gazebo::entityType` +GetType | `gz::sim::entityType` GetWorldEnergy | TODO GetWorldEnergyKinetic | TODO GetWorldEnergyPotential | TODO @@ -102,17 +102,17 @@ RelativeAngularAccel | TODO RelativeAngularVel | TODO RelativeLinearAccel | TODO RelativeLinearVel | TODO -RelativePose | `ignition::gazebo::Actor::Pose` +RelativePose | `gz::sim::Actor::Pose` Scale | TODO -ScriptTime | `ignition::gazebo::Actor::AnimationTime` -SDFPoseRelativeToParent | `ignition::gazebo::Actor::Pose` -SDFSemanticPose | `ignition::gazebo::Actor::Pose` +ScriptTime | `gz::sim::Actor::AnimationTime` +SDFPoseRelativeToParent | `gz::sim::Actor::Pose` +SDFSemanticPose | `gz::sim::Actor::Pose` SensorScopedName | TODO SetAngularVel | TODO -SetAnimation | use `ignition::gazebo::Actor::SetTrajectoryPose` +SetAnimation | use `gz::sim::Actor::SetTrajectoryPose` SetAutoDisable | TODO SetCollideMode | TODO -SetCustomTrajectory | use `ignition::gazebo::Actor::SetTrajectoryPose`, `SetAnimationTime`, and `SetAnimationName` to achieve similar result. +SetCustomTrajectory | use `gz::sim::Actor::SetTrajectoryPose`, `SetAnimationTime`, and `SetAnimationName` to achieve similar result. SetEnabled | TODO SetGravityMode | TODO SetInitialRelativePose | TODO @@ -126,7 +126,7 @@ SetName | TODO SetRelativePose | TODO SetSaveable | Not supported SetScale | TODO -SetScriptTime | `ignition::gazebo::Actor::SetAnimationTime` +SetScriptTime | `gz::sim::Actor::SetAnimationTime` SetSelected | Selection is client-specific, not porting SetSelfCollide | TODO SetState | TODO @@ -136,8 +136,8 @@ SetWorldPose | TODO SetWorldTwist | TODO SkeletonAnimations | TODO Stop | TODO -StopAnimation | use `ignition::gazebo::Actor::SetTrajectoryPose` -TypeStr | `ignition::gazebo::entityTypeStr` +StopAnimation | use `gz::sim::Actor::SetTrajectoryPose` +TypeStr | `gz::sim::entityTypeStr` UnscaledSDF | TODO UpdateParamenters | TODO URI | TODO @@ -146,7 +146,7 @@ WorldAngularAccel | TODO WorldAngularVel | TODO WorldLinearAccel | TODO WorldLinearVel | TODO -WorldPose | `ignition::gazebo::Actor::WorldPose` +WorldPose | `gz::sim::Actor::WorldPose` --- @@ -175,11 +175,11 @@ GetJointCount | Not supported GetJoints | Not supported GetLink | TODO GetLinks | TODO -GetParent | `ignition::gazebo::EntiyComponentManager::ParentEntity` -GetParentId | `ignition::gazebo::EntiyComponentManager::ParentEntity` -GetParentModel | `ignition::gazebo::EntiyComponentManager::ParentEntity` +GetParent | `gz::sim::EntiyComponentManager::ParentEntity` +GetParentId | `gz::sim::EntiyComponentManager::ParentEntity` +GetParentModel | `gz::sim::EntiyComponentManager::ParentEntity` GetSensorCount | TODO -GetWorld | const `ignition::gazebo::worldEntity` +GetWorld | const `gz::sim::worldEntity` NestedModel | TODO NestedModels | TODO @@ -219,8 +219,8 @@ Classic | Gazebo -- | -- Fini | N/A Init | N/A -Load | `ignition::gazebo::SdfEntityCreator::CreateEntities` -LoadJoints | `ignition::gazebo::SdfEntityCreator::CreateEntities` +Load | `gz::sim::SdfEntityCreator::CreateEntities` +LoadJoints | `gz::sim::SdfEntityCreator::CreateEntities` LoadPlugins | TODO Reset | TODO ResetCustomTrajectory | TODO @@ -239,7 +239,7 @@ logic that should be performed from within a system. Classic | Gazebo -- | -- -GetJointController | Use this system: `ignition::gazebo::systems::JointController` +GetJointController | Use this system: `gz::sim::systems::JointController` GetNearestEntityBelow | Requires a system PlaceOnEntity | Requires a system PlaceOnNearestEntityBelow | Requires a system diff --git a/tutorials/migration_joint_api.md b/tutorials/migration_joint_api.md index 54179c31c3..8e33acd5e7 100644 --- a/tutorials/migration_joint_api.md +++ b/tutorials/migration_joint_api.md @@ -32,10 +32,10 @@ can be divided in these categories: You'll find the Gazebo APIs below on the following headers: -* [ignition/gazebo/Joint.hh](https://gazebosim.org/api/gazebo/7/Joint_8hh.html) -* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/7/Util_8hh.html) -* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/7/SdfEntityCreator_8hh.html) -* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) +* [gz/sim/Joint.hh](https://gazebosim.org/api/gazebo/7/Joint_8hh.html) +* [gz/sim/Util.hh](https://gazebosim.org/api/gazebo/7/Util_8hh.html) +* [gz/sim/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/7/SdfEntityCreator_8hh.html) +* [gz/sim/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) It's worth remembering that most of this functionality can be performed using the @@ -76,24 +76,24 @@ DOF | TODO FillMsg | TODO GetDamping | TODO GetEffortLimit | TODO -GetForce | `ignition::gazebo::Joint::TransmittedWrench` -GetForceTorque | `ignition::gazebo::Joint::TransmittedWrench` -GetId | `ignition::gazebo::Joint::Entity` +GetForce | `gz::sim::Joint::TransmittedWrench` +GetForceTorque | `gz::sim::Joint::TransmittedWrench` +GetId | `gz::sim::Joint::Entity` GetInertiaRatio | TODO -GetJointLink | use `ignition::gazebo::Joint::*LinkName` +GetJointLink | use `gz::sim::Joint::*LinkName` GetMsgType | TODO -GetName | `ignition::gazebo::Joint::Name` +GetName | `gz::sim::Joint::Name` GetParam | TODO GetSaveable | Not supported -GetScopedName | `ignition::gazebo::scopedName` +GetScopedName | `gz::sim::scopedName` GetSDF | TODO GetSDFDom | TODO GetSpringReferencePosition | TODO GetStiffness | TODO GetStopDissipation | TODO GetStopStiffness | TODO -GetType | `ignition::gazebo::Joint::Type` -GetVelocity | `ignition::gazebo::Joint::Velocity` +GetType | `gz::sim::Joint::Type` +GetVelocity | `gz::sim::Joint::Velocity` GetVelocityLimit | TODO GetWorldEnergyPotentialSpring | TODO GlobalAxis | TODO @@ -103,24 +103,24 @@ InitialAnchorPose | TODO IsSelected | Selection is client-specific, not porting LinkForce | TODO LinkTorque | TODO -LocalAxis | `ignition::gazebo::Joint::Axis` +LocalAxis | `gz::sim::Joint::Axis` LowerLimit | TODO ParentWorldPose | TODO -Position | `ignition::gazebo::Joint::Position` +Position | `gz::sim::Joint::Position` Print | TODO ResolveAxisXyz | TODO -SDFPoseRelativeToParent | `ignition::gazebo::Joint::Pose` -SDFSemanticPose | `ignition::gazebo::Joint::Pose` +SDFPoseRelativeToParent | `gz::sim::Joint::Pose` +SDFSemanticPose | `gz::sim::Joint::Pose` SetAnchor | TODO SetAxis | TODO SetDamping | TODO -SetEffortLimit | `ignition::gazebo::Joint::SetEffortLimits` -SetForce | `ignition::gazebo::Joint::SetForce` -SetLowerLimit | `ignition::gazebo::Joint::SetPositionLimits` +SetEffortLimit | `gz::sim::Joint::SetEffortLimits` +SetForce | `gz::sim::Joint::SetForce` +SetLowerLimit | `gz::sim::Joint::SetPositionLimits` SetName | TODO SeParam | TODO -SetPosition | `ignition::gazebo::Joint::ResetPosition` -SetProvideFeedback | `ignition::gazebo::Joint::EnableTransmittedWrenchCheck` +SetPosition | `gz::sim::Joint::ResetPosition` +SetProvideFeedback | `gz::sim::Joint::EnableTransmittedWrenchCheck` SetSaveable | Not supported SetSelected | Selection is client-specific, not porting SetState | TODO @@ -128,10 +128,10 @@ SetStiffness | TODO SetStiffnessDamping | TODO SetStopDissipation | TODO SetStopStiffness | TODO -SetUpperLimit | `ignition::gazebo::Joint::SetPositionLimits` -SetVelocity | `ignition::gazebo::Joint::SetVelocity` -SetVelocityLimit | `ignition::gazebo::Joint::SetVelocityLimits` -TypeStr | `ignition::gazebo::Joint::Type` +SetUpperLimit | `gz::sim::Joint::SetPositionLimits` +SetVelocity | `gz::sim::Joint::SetVelocity` +SetVelocityLimit | `gz::sim::Joint::SetVelocityLimits` +TypeStr | `gz::sim::Joint::Type` UpdateParameters | TODO UpperLimit | TODO URI | TODO @@ -152,14 +152,14 @@ they deal with entity IDs. Classic | Gazebo -- | -- -GetByName | Use type-specific `ignition::gazebo::Joint::*ByName` -GetChild | Use type-specific `ignition::gazebo::Joint::*ByName` -GetChild (Child link) | `ignition::gazebo::Joint::ChildLinkName` -GetChildCount | Use type-specific `ignition::gazebo::Joint::*Count` -GetParent | `ignition::gazebo::Joint::ParentModel` -GetParent (Parent link) | `ignition::gazebo::Joint::ParentLinkName` -GetParentId | `ignition::gazebo::EntiyComponentManager::ParentEntity` -GetWorld | `ignition::gazebo::worldEntity` +GetByName | Use type-specific `gz::sim::Joint::*ByName` +GetChild | Use type-specific `gz::sim::Joint::*ByName` +GetChild (Child link) | `gz::sim::Joint::ChildLinkName` +GetChildCount | Use type-specific `gz::sim::Joint::*Count` +GetParent | `gz::sim::Joint::ParentModel` +GetParent (Parent link) | `gz::sim::Joint::ParentLinkName` +GetParentId | `gz::sim::EntiyComponentManager::ParentEntity` +GetWorld | `gz::sim::worldEntity` --- @@ -196,8 +196,8 @@ Classic | Gazebo ConnectJointUpdate | TODO Fini | N/A Init | N/A -Load | `ignition::gazebo::SdfEntityCreator::CreateEntities` -Reset | `ignition::gazebo::Joint::ResetPosition` / `ignition::gazebo::Joint::ResetVelocity` +Load | `gz::sim::SdfEntityCreator::CreateEntities` +Reset | `gz::sim::Joint::ResetPosition` / `gz::sim::Joint::ResetVelocity` Update | Entities are updated by systems --- diff --git a/tutorials/migration_light_api.md b/tutorials/migration_light_api.md index 79ea920ddd..86f1f508c6 100644 --- a/tutorials/migration_light_api.md +++ b/tutorials/migration_light_api.md @@ -29,10 +29,10 @@ In Gazebo, the light APIs has been consolidated into a single Light class with some of generic functions available through other utility / core classes. You'll find the APIs below on the following headers: -* [ignition/gazebo/Light.hh](https://gazebosim.org/api/gazebo/7/Light_8hh.html) -* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/7/Util_8hh.html) -* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/7/SdfEntityCreator_8hh.html) -* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) +* [gz/sim/Light.hh](https://gazebosim.org/api/gazebo/7/Light_8hh.html) +* [gz/sim/Util.hh](https://gazebosim.org/api/gazebo/7/Util_8hh.html) +* [gz/sim/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/7/SdfEntityCreator_8hh.html) +* [gz/sim/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) It's worth remembering that most of this functionality can be performed using the @@ -62,35 +62,35 @@ This section focuses on migrating from APIs provided through the Classic | Gazebo -- | -- -CastShadows | `ignition::gazebo::Light::CastShadows` +CastShadows | `gz::sim::Light::CastShadows` Clone | TODO -DiffuseColor | `ignition::gazebo::Light::DiffuseColor` -Direction | `ignition::gazebo::Light::Direction` +DiffuseColor | `gz::sim::Light::DiffuseColor` +Direction | `gz::sim::Light::Direction` FillMsg | TODO -Id | `ignition::gazebo::Light::Entity` -LightType | `ignition::gazebo::Light::Type` -Name | `ignition::gazebo::Light::Name` -Position | `ignition::gazebo::Light::Pose` -Rotation | `ignition::gazebo::Light::Pose` -SetAttenuation | use `ignition::gazebo::Light::SetAttenuation*` -SetCastShadows | `ignition::gazebo::Light::SetCastShadows` -SetDiffuseColor | `ignition::gazebo::Light::SetDiffuseColor` -SetDirection | `ignition::gazebo::Light::SetDirection` +Id | `gz::sim::Light::Entity` +LightType | `gz::sim::Light::Type` +Name | `gz::sim::Light::Name` +Position | `gz::sim::Light::Pose` +Rotation | `gz::sim::Light::Pose` +SetAttenuation | use `gz::sim::Light::SetAttenuation*` +SetCastShadows | `gz::sim::Light::SetCastShadows` +SetDiffuseColor | `gz::sim::Light::SetDiffuseColor` +SetDirection | `gz::sim::Light::SetDirection` SetLightType | TODO SetName | TODO SetPosition | TODO -SetRange | `ignition::gazebo::Light::SetAttenuationRange` +SetRange | `gz::sim::Light::SetAttenuationRange` SetRotation | TODO SetSelected | Selection is client-specific, not porting -SetSpecularColor | `ignition::gazebo::Light::SetSpecularColor` -SetSpotFalloff | `ignition::gazebo::Light::SetSpotFalloff` -SetSpotInnerAngle | `ignition::gazebo::Light::SetSpotInnerAngle` -SetSpotOuterAngle | `ignition::gazebo::Light::SetSpotOuterAngle` +SetSpecularColor | `gz::sim::Light::SetSpecularColor` +SetSpotFalloff | `gz::sim::Light::SetSpotFalloff` +SetSpotInnerAngle | `gz::sim::Light::SetSpotInnerAngle` +SetSpotOuterAngle | `gz::sim::Light::SetSpotOuterAngle` SetVisible | TODO ShowVisual | TODO -SpecularColor | `ignition::gazebo::Light::SetSpecularColor` +SpecularColor | `gz::sim::Light::SetSpecularColor` ToggleShowVisual | TODO -Type | `ignition::gazebo::Light::Type` +Type | `gz::sim::Light::Type` Visible | TODO WorldPose | TODO --- @@ -112,9 +112,9 @@ they deal with entity IDs. Classic | Gazebo -- | -- -GetParent | `ignition::gazebo::Light::Parent` -GetParentId | `ignition::gazebo::Light::Parent` -GetWorld | `ignition::gazebo::worldEntity` +GetParent | `gz::sim::Light::Parent` +GetParentId | `gz::sim::Light::Parent` +GetWorld | `gz::sim::worldEntity` --- @@ -148,8 +148,8 @@ its properties. Classic | Gazebo -- | -- -Load | `ignition::gazebo::SdfEntityCreator::CreateEntities` -LoadFromMsg | `ignition::gazebo::SdfEntityCreator::CreateEntities` +Load | `gz::sim::SdfEntityCreator::CreateEntities` +LoadFromMsg | `gz::sim::SdfEntityCreator::CreateEntities` UpdateFromMsg | TODO --- diff --git a/tutorials/model_photo_shoot.md b/tutorials/model_photo_shoot.md index 0c90ef1ec6..7d7209d0f8 100644 --- a/tutorials/model_photo_shoot.md +++ b/tutorials/model_photo_shoot.md @@ -53,7 +53,7 @@ plugin will also affect the final resulting background color on the images. ``` - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Robonaut + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Robonaut diff --git a/tutorials/move_camera_to_model.md b/tutorials/move_camera_to_model.md index fb008afb3b..25f014c404 100644 --- a/tutorials/move_camera_to_model.md +++ b/tutorials/move_camera_to_model.md @@ -5,12 +5,12 @@ This tutorial gives an introduction to Gazebo's service `/gui/move_to/model`. Th ## How to move the camera to a model 1. Load the **View Angle** plugin. This service is only available when the **View Angle** plugin is loaded. -2. Call the service using the request message type `ignition.msgs.GUICamera` and the response message type `ignition.msgs.Boolean`. The distance to the object is defined as the z coordinate, and the direction of the camera with a quaternion. It's possible to select the projection type. +2. Call the service using the request message type `gz.msgs.GUICamera` and the response message type `gz.msgs.Boolean`. The distance to the object is defined as the z coordinate, and the direction of the camera with a quaternion. It's possible to select the projection type. For example, Let's move the camera to the `box` model looking down from 5 meters away. ```bash -ign service -s /gui/move_to/model --reqtype ignition.msgs.GUICamera --reptype ignition.msgs.Boolean -r 'name: "box", pose: {position: {z:5}, orientation: {x:0, y:0, z: -1, w:0}}, projection_type: "orbit"' --timeout 5000 +ign service -s /gui/move_to/model --reqtype gz.msgs.GUICamera --reptype gz.msgs.Boolean -r 'name: "box", pose: {position: {z:5}, orientation: {x:0, y:0, z: -1, w:0}}, projection_type: "orbit"' --timeout 5000 ``` @image html files/move_camera_to_model/box_5.gif @@ -18,7 +18,7 @@ ign service -s /gui/move_to/model --reqtype ignition.msgs.GUICamera --reptype The camera can also be placed far away, for example 20 meters: ```bash -ign service -s /gui/move_to/model --reqtype ignition.msgs.GUICamera --reptype ignition.msgs.Boolean -r 'name: "box", pose: {position: {z:20}, orientation: {x:0, y:0, z: -1, w:0}}, projection_type: "orbit"' --timeout 5000 +ign service -s /gui/move_to/model --reqtype gz.msgs.GUICamera --reptype gz.msgs.Boolean -r 'name: "box", pose: {position: {z:20}, orientation: {x:0, y:0, z: -1, w:0}}, projection_type: "orbit"' --timeout 5000 ``` @image html files/move_camera_to_model/box_20.gif diff --git a/tutorials/particle_tutorial.md b/tutorials/particle_tutorial.md index e310e41c42..b676d204de 100644 --- a/tutorials/particle_tutorial.md +++ b/tutorials/particle_tutorial.md @@ -20,11 +20,11 @@ Next, we can start adding particle emitter models into the world. In our example ```xml - https://fuel.ignitionrobotics.org/1.0/openrobotics/models/fog generator + https://fuel.gazebosim.org/1.0/openrobotics/models/fog generator ``` -Here is the content of the Fog Generator [model.sdf](https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fog%20Generator/1/files/model.sdf) file. +Here is the content of the Fog Generator [model.sdf](https://fuel.gazebosim.org/1.0/OpenRobotics/models/Fog%20Generator/1/files/model.sdf) file. ```xml diff --git a/tutorials/python_interfaces.md b/tutorials/python_interfaces.md index fbae70e2e2..ad0e0d8ddc 100644 --- a/tutorials/python_interfaces.md +++ b/tutorials/python_interfaces.md @@ -81,3 +81,64 @@ iterations 1000 post_iterations 1000 pre_iterations 1000 ``` + +# Gazebo Systems written in Python + +Gazebo also provides a way to write systems in Python. This is done using the +`gz::sim::systems::PythonSystemLoader` system which loads a given python module +specified by its `` parameter. The search path for the module +includes `GZ_SIM_SYSTEM_PLUGIN_PATH` as well as `PYTHONPATH`. The module is +expected to provide a function called `get_system` that returns an instance of +a class that implements the various interfaces in `gz::sim::System`. + +Example python system: + + + +```python +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() +``` + +The system can be added to SDFormat model or world with: + +```xml + + test_system + 100 + + +``` + +asuming the name of the module is `test_system` and the directory containing +the module has been added to `GZ_SIM_SYSTEM_PLUGIN_PATH`, diff --git a/tutorials/resources.md b/tutorials/resources.md index 9ae4b7d07d..d1af43a991 100644 --- a/tutorials/resources.md +++ b/tutorials/resources.md @@ -120,7 +120,7 @@ Gazebo will look for URIs (path / URL) in the following, in order: path is URI, scheme is stripped) 2. Current running path / absolute path 3. [Gazebo Fuel](https://app.gazebosim.org/fuel/models) - 1. Cache (i.e. `$HOME/.ignition/fuel`) + 1. Cache (i.e. `$HOME/.gz/fuel`) 2. Web server \* The `SDF_PATH` environment variable also works in some scenarios, but diff --git a/tutorials/triggered_publisher.md b/tutorials/triggered_publisher.md index 8670ab7798..f4b1678b4b 100644 --- a/tutorials/triggered_publisher.md +++ b/tutorials/triggered_publisher.md @@ -275,16 +275,16 @@ metadata to invoke a service call to `/world/triggered_publisher/set_pose`. The Multiple `` tags can be used as well as with the `` tag. ```xml - - - + + + linear: {x: 0} @@ -299,16 +299,16 @@ metadata to invoke a service call to `/world/triggered_publisher/set_pose`. The Multiple `` tags can be used as well as with the `` tag. ```xml - - - + + + linear: {x: 0}