diff --git a/README.md b/README.md index 8346b53b..1445dacf 100644 --- a/README.md +++ b/README.md @@ -7,17 +7,19 @@ Foxy | Edifice | [foxy](https://github.com/gazebosim/ros_gz/tree/foxy) | only fr Galactic | Edifice | [galactic](https://github.com/gazebosim/ros_gz/tree/galactic) | https://packages.ros.org Galactic | Fortress | [galactic](https://github.com/gazebosim/ros_gz/tree/galactic) | only from source Humble | Fortress | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | https://packages.ros.org -Humble | Garden | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | only from source -Iron | Fortress | [iron](https://github.com/gazebosim/ros_gz/tree/iron) | https://packages.ros.org -Iron | Garden | [iron](https://github.com/gazebosim/ros_gz/tree/iron) | only from source -Iron | Harmonic | [iron](https://github.com/gazebosim/ros_gz/tree/iron) | only from source -Jazzy* | Fortress | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | only from source +Humble | Garden | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | [gazebo packages](https://gazebosim.org/docs/latest/ros_installation#gazebo-garden-with-ros-2-humble-iron-or-rolling-use-with-caution-)[^1] +Humble | Harmonic | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | [gazebo packages](https://gazebosim.org/docs/harmonic/ros_installation#-gazebo-harmonic-with-ros-2-humble-iron-or-rolling-use-with-caution-)[^1] +Iron | Fortress | [humble](https://github.com/gazebosim/ros_gz/tree/iron) | https://packages.ros.org +Iron | Garden | [humble](https://github.com/gazebosim/ros_gz/tree/iron) | only from source +Iron | Harmonic | [humble](https://github.com/gazebosim/ros_gz/tree/iron) | only from source Jazzy* | Garden | [ros2](https://github.com/gazebosim/ros_gz/tree/ros2) | only from source Jazzy* | Harmonic | [ros2](https://github.com/gazebosim/ros_gz/tree/ros2) | https://packages.ros.org Rolling | Fortress | [humble](https://github.com/gazebosim/ros_gz/tree/humble) | https://packages.ros.org Rolling | Garden | [ros2](https://github.com/gazebosim/ros_gz/tree/ros2) | only from source Rolling | Harmonic | [ros2](https://github.com/gazebosim/ros_gz/tree/ros2) | only from source +[^1]: Binaries for these pairings are provided from a the packages.osrfoundation.org repository. Refer to https://gazebosim.org/docs/latest/ros_installation for installation instructions. + * ROS 2 Jazzy Jalisco is slated for release on May 23rd, 2024. [Full ROS 2 release information is available in REP-2000.] For information on ROS(1) and Gazebo compatibility, refer to the [noetic branch README](https://github.com/gazebosim/ros_gz/tree/noetic) @@ -91,7 +93,7 @@ Install either [Edifice, Fortress, or Garden](https://gazebosim.org/docs). Set the `GZ_VERSION` environment variable to the Gazebo version you'd like to compile against. For example: - export GZ_VERSION=edifice + export GZ_VERSION=edifice # IMPORTANT: Replace with correct version > You only need to set this variable when compiling, not when running. diff --git a/ros_gz_bridge/ros_gz_bridge/__init__.py b/ros_gz_bridge/ros_gz_bridge/__init__.py index d1cf4289..b7263f22 100644 --- a/ros_gz_bridge/ros_gz_bridge/__init__.py +++ b/ros_gz_bridge/ros_gz_bridge/__init__.py @@ -41,15 +41,15 @@ def ign_string(self): return f'ignition.msgs.{self.gz_message_name}' def ign_type(self): - # Return GZ type of a message (eg ignition::msgs::Bool) - return f'ignition::msgs::{self.gz_message_name}' + # Return GZ type of a message (eg gz::msgs::Bool) + return f'gz::msgs::{self.gz_message_name}' def gz_string(self): # Return GZ string version of a message (eg ignition.msgs.Bool) return f'gz.msgs.{self.gz_message_name}' def gz_type(self): - # Return GZ type of a message (eg ignition::msgs::Bool) + # Return GZ type of a message (eg gz::msgs::Bool) return f'gz::msgs::{self.gz_message_name}' def unique(self): diff --git a/ros_gz_bridge/src/convert/gps_msgs.cpp b/ros_gz_bridge/src/convert/gps_msgs.cpp index de806239..cf2774d6 100644 --- a/ros_gz_bridge/src/convert/gps_msgs.cpp +++ b/ros_gz_bridge/src/convert/gps_msgs.cpp @@ -55,7 +55,7 @@ convert_gz_to_ros( ros_msg.track = atan2(gz_msg.velocity_north(), gz_msg.velocity_east()); ros_msg.climb = gz_msg.velocity_up(); - // position_covariance is not supported in Ignition::Msgs::NavSat. + // position_covariance is not supported in gz::msgs::NavSat. ros_msg.position_covariance_type = gps_msgs::msg::GPSFix::COVARIANCE_TYPE_UNKNOWN; ros_msg.status.status = gps_msgs::msg::GPSStatus::STATUS_GBAS_FIX; } diff --git a/ros_gz_bridge/src/convert/sensor_msgs.cpp b/ros_gz_bridge/src/convert/sensor_msgs.cpp index 887f16d0..691b823c 100644 --- a/ros_gz_bridge/src/convert/sensor_msgs.cpp +++ b/ros_gz_bridge/src/convert/sensor_msgs.cpp @@ -520,7 +520,7 @@ convert_gz_to_ros( ros_msg.longitude = gz_msg.longitude_deg(); ros_msg.altitude = gz_msg.altitude(); - // position_covariance is not supported in Ignition::Msgs::NavSat. + // position_covariance is not supported in gz::msgs::NavSat. ros_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; ros_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; } diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index 6308a103..cbaff583 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -288,7 +288,9 @@ void compareTestMsg(const std::shared_ptr & _msg) gz::msgs::SensorNoise expected_msg; createTestMsg(expected_msg); - EXPECT_EQ(expected_msg.type(), gz::msgs::SensorNoise_Type::SensorNoise_Type_GAUSSIAN_QUANTIZED); + EXPECT_EQ( + expected_msg.type(), + gz::msgs::SensorNoise_Type::SensorNoise_Type_GAUSSIAN_QUANTIZED); EXPECT_EQ(expected_msg.mean(), _msg->mean()); EXPECT_EQ(expected_msg.stddev(), _msg->stddev()); EXPECT_EQ(expected_msg.bias_mean(), _msg->bias_mean()); diff --git a/ros_gz_image/test/publishers/gz_publisher.cpp b/ros_gz_image/test/publishers/gz_publisher.cpp index 24c745b8..6bf9aa7f 100644 --- a/ros_gz_image/test/publishers/gz_publisher.cpp +++ b/ros_gz_image/test/publishers/gz_publisher.cpp @@ -45,7 +45,7 @@ int main(int /*argc*/, char **/*argv*/) gz::transport::Node node; // gz::msgs::Image. - auto image_pub = node.Advertise("image"); + auto image_pub = node.Advertise("image"); gz::msgs::Image image_msg; ros_gz_image::testing::createTestMsg(image_msg); diff --git a/ros_gz_image/test/test_utils.h b/ros_gz_image/test/test_utils.h index 02996f3e..f6509eec 100644 --- a/ros_gz_image/test/test_utils.h +++ b/ros_gz_image/test/test_utils.h @@ -25,7 +25,7 @@ #include #include #include -#include +#include namespace ros_gz_image { @@ -142,7 +142,7 @@ namespace testing /// \brief Create a message used for testing. /// \param[out] _msg The message populated. - void createTestMsg(ignition::msgs::Header &_msg) + void createTestMsg(gz::msgs::Header &_msg) { auto seq_entry = _msg.add_data(); seq_entry->set_key("seq"); @@ -156,9 +156,9 @@ namespace testing /// \brief Compare a message with the populated for testing. /// \param[in] _msg The message to compare. - void compareTestMsg(const ignition::msgs::Header &_msg) + void compareTestMsg(const gz::msgs::Header &_msg) { - ignition::msgs::Header expected_msg; + gz::msgs::Header expected_msg; createTestMsg(expected_msg); EXPECT_EQ(expected_msg.stamp().sec(), _msg.stamp().sec()); @@ -183,24 +183,24 @@ namespace testing /// \brief Create a message used for testing. /// \param[out] _msg The message populated. - void createTestMsg(ignition::msgs::Image &_msg) + void createTestMsg(gz::msgs::Image &_msg) { - ignition::msgs::Header header_msg; + gz::msgs::Header header_msg; createTestMsg(header_msg); _msg.mutable_header()->CopyFrom(header_msg); _msg.set_width(320); _msg.set_height(240); - _msg.set_pixel_format_type(ignition::msgs::PixelFormatType::RGB_INT8); + _msg.set_pixel_format_type(gz::msgs::PixelFormatType::RGB_INT8); _msg.set_step(_msg.width() * 3); _msg.set_data(std::string(_msg.height() * _msg.step(), '1')); } /// \brief Compare a message with the populated for testing. /// \param[in] _msg The message to compare. - void compareTestMsg(const ignition::msgs::Image &_msg) + void compareTestMsg(const gz::msgs::Image &_msg) { - ignition::msgs::Image expected_msg; + gz::msgs::Image expected_msg; createTestMsg(expected_msg); compareTestMsg(_msg.header()); diff --git a/ros_gz_point_cloud/src/point_cloud.cc b/ros_gz_point_cloud/src/point_cloud.cc index 66a0e597..7b0820aa 100644 --- a/ros_gz_point_cloud/src/point_cloud.cc +++ b/ros_gz_point_cloud/src/point_cloud.cc @@ -13,19 +13,19 @@ // limitations under the License. #include "point_cloud.hh" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -35,7 +35,7 @@ IGNITION_ADD_PLUGIN( ros_gz_point_cloud::PointCloud, - ignition::gazebo::System, + gz::sim::System, ros_gz_point_cloud::PointCloud::ISystemConfigure, ros_gz_point_cloud::PointCloud::ISystemPostUpdate) @@ -76,49 +76,49 @@ class ros_gz_point_cloud::PointCloudPrivate /// \param[in] _ecm Immutable reference to ECM. public: - void LoadDepthCamera(const ignition::gazebo::EntityComponentManager & _ecm); + void LoadDepthCamera(const gz::sim::EntityComponentManager & _ecm); /// \brief Get RGB camera from rendering. /// \param[in] _ecm Immutable reference to ECM. public: - void LoadRgbCamera(const ignition::gazebo::EntityComponentManager & _ecm); + void LoadRgbCamera(const gz::sim::EntityComponentManager & _ecm); /// \brief Get GPU rays from rendering. /// \param[in] _ecm Immutable reference to ECM. public: - void LoadGpuRays(const ignition::gazebo::EntityComponentManager & _ecm); + void LoadGpuRays(const gz::sim::EntityComponentManager & _ecm); /// \brief Rendering scene which manages the cameras. public: - ignition::rendering::ScenePtr scene_; + gz::rendering::ScenePtr scene_; /// \brief Entity ID for sensor within Gazebo. public: - ignition::gazebo::Entity entity_; + gz::sim::Entity entity_; /// \brief Rendering depth camera public: - std::shared_ptr < ignition::rendering::DepthCamera > depth_camera_; + std::shared_ptr < gz::rendering::DepthCamera > depth_camera_; /// \brief Rendering RGB camera public: - std::shared_ptr < ignition::rendering::Camera > rgb_camera_; + std::shared_ptr < gz::rendering::Camera > rgb_camera_; /// \brief Rendering GPU lidar public: - std::shared_ptr < ignition::rendering::GpuRays > gpu_rays_; + std::shared_ptr < gz::rendering::GpuRays > gpu_rays_; /// \brief Keep latest image from RGB camera. public: - ignition::rendering::Image rgb_image_; + gz::rendering::Image rgb_image_; /// \brief Message populated with latest image from RGB camera. @@ -128,12 +128,12 @@ class ros_gz_point_cloud::PointCloudPrivate /// \brief Connection to depth frame event. public: - ignition::common::ConnectionPtr depth_connection_; + gz::common::ConnectionPtr depth_connection_; /// \brief Connection to GPU rays frame event. public: - ignition::common::ConnectionPtr gpu_rays_connection_; + gz::common::ConnectionPtr gpu_rays_connection_; /// \brief Node to publish ROS messages. @@ -179,18 +179,18 @@ PointCloud::PointCloud() ////////////////////////////////////////////////// void PointCloud::Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr < const sdf::Element > & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager &) + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager &) { this->dataPtr->entity_ = _entity; - if (_ecm.Component < ignition::gazebo::components::RgbdCamera > (_entity) != nullptr) { + if (_ecm.Component < gz::sim::components::RgbdCamera > (_entity) != nullptr) { this->dataPtr->type_ = SensorType::RGBD_CAMERA; - } else if (_ecm.Component < ignition::gazebo::components::DepthCamera > (_entity) != nullptr) { + } else if (_ecm.Component < gz::sim::components::DepthCamera > (_entity) != nullptr) { this->dataPtr->type_ = SensorType::DEPTH_CAMERA; - } else if (_ecm.Component < ignition::gazebo::components::GpuLidar > (_entity) != nullptr) { + } else if (_ecm.Component < gz::sim::components::GpuLidar > (_entity) != nullptr) { this->dataPtr->type_ = SensorType::GPU_LIDAR; } else { ROS_ERROR_NAMED( @@ -208,7 +208,7 @@ void PointCloud::Configure( } // Sensor scoped name - auto scoped_name = ignition::gazebo::scopedName(this->dataPtr->entity_, _ecm, "/", false); + auto scoped_name = gz::sim::scopedName(this->dataPtr->entity_, _ecm, "/", false); // ROS node auto ns = _sdf->Get < std::string > ("namespace", scoped_name).first; @@ -229,14 +229,14 @@ void PointCloud::Configure( ////////////////////////////////////////////////// void PointCloud::PostUpdate( - const ignition::gazebo::UpdateInfo & _info, - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo & _info, + const gz::sim::EntityComponentManager & _ecm) { this->dataPtr->current_time_ = _info.simTime; // Find engine / scene if (!this->dataPtr->scene_) { - auto engine = ignition::rendering::engine(this->dataPtr->engine_name_); + auto engine = gz::rendering::engine(this->dataPtr->engine_name_); if (!engine) { return; } @@ -268,11 +268,11 @@ void PointCloud::PostUpdate( ////////////////////////////////////////////////// void PointCloudPrivate::LoadDepthCamera( - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::EntityComponentManager & _ecm) { // Sensor name scoped from the model auto sensor_name = - ignition::gazebo::scopedName(this->entity_, _ecm, "::", false); + gz::sim::scopedName(this->entity_, _ecm, "::", false); sensor_name = sensor_name.substr(sensor_name.find("::") + 2); // Get sensor @@ -285,7 +285,7 @@ void PointCloudPrivate::LoadDepthCamera( } this->depth_camera_ = - std::dynamic_pointer_cast < ignition::rendering::DepthCamera > (sensor); + std::dynamic_pointer_cast < gz::rendering::DepthCamera > (sensor); if (!this->depth_camera_) { ROS_ERROR_NAMED( "ros_gz_point_cloud", @@ -302,11 +302,11 @@ void PointCloudPrivate::LoadDepthCamera( ////////////////////////////////////////////////// void PointCloudPrivate::LoadRgbCamera( - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::EntityComponentManager & _ecm) { // Sensor name scoped from the model auto sensor_name = - ignition::gazebo::scopedName(this->entity_, _ecm, "::", false); + gz::sim::scopedName(this->entity_, _ecm, "::", false); sensor_name = sensor_name.substr(sensor_name.find("::") + 2); // Get sensor @@ -315,7 +315,7 @@ void PointCloudPrivate::LoadRgbCamera( return; } - this->rgb_camera_ = std::dynamic_pointer_cast < ignition::rendering::Camera > (sensor); + this->rgb_camera_ = std::dynamic_pointer_cast < gz::rendering::Camera > (sensor); if (!this->rgb_camera_) { ROS_ERROR_NAMED( "ros_gz_point_cloud", @@ -328,11 +328,11 @@ void PointCloudPrivate::LoadRgbCamera( ////////////////////////////////////////////////// void PointCloudPrivate::LoadGpuRays( - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::EntityComponentManager & _ecm) { // Sensor name scoped from the model auto sensor_name = - ignition::gazebo::scopedName(this->entity_, _ecm, "::", false); + gz::sim::scopedName(this->entity_, _ecm, "::", false); sensor_name = sensor_name.substr(sensor_name.find("::") + 2); // Get sensor @@ -342,7 +342,7 @@ void PointCloudPrivate::LoadGpuRays( } this->gpu_rays_ = - std::dynamic_pointer_cast < ignition::rendering::GpuRays > (sensor); + std::dynamic_pointer_cast < gz::rendering::GpuRays > (sensor); if (!this->gpu_rays_) { ROS_ERROR_NAMED( "ros_gz_point_cloud", @@ -395,7 +395,7 @@ void PointCloudPrivate::OnNewDepthFrame( // Fill message // Logic borrowed from // https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_depth_camera.cpp - auto sec_nsec = ignition::math::durationToSecNsec(this->current_time_); + auto sec_nsec = gz::math::durationToSecNsec(this->current_time_); sensor_msgs::PointCloud2 msg; msg.header.frame_id = this->frame_id_; @@ -486,11 +486,11 @@ void PointCloudPrivate::OnNewDepthFrame( // Clamp according to REP 117 if (depth > this->depth_camera_->FarClipPlane()) { - *iter_z = ignition::math::INF_D; + *iter_z = gz::math::INF_D; msg.is_dense = false; } if (depth < this->depth_camera_->NearClipPlane()) { - *iter_z = -ignition::math::INF_D; + *iter_z = -gz::math::INF_D; msg.is_dense = false; } } else if (nullptr != this->gpu_rays_) { diff --git a/ros_gz_point_cloud/src/point_cloud.hh b/ros_gz_point_cloud/src/point_cloud.hh index 437efac9..d8a46d46 100644 --- a/ros_gz_point_cloud/src/point_cloud.hh +++ b/ros_gz_point_cloud/src/point_cloud.hh @@ -16,7 +16,7 @@ #define ROS_GZ_POINTCLOUD__POINTCLOUD_HPP_ #include -#include +#include namespace ros_gz_point_cloud { @@ -36,9 +36,9 @@ namespace ros_gz_point_cloud /// * ``: Render engine name, defaults to 'ogre2' /// * ``: Scene name, defaults to 'scene' class PointCloud: - public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPostUpdate + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPostUpdate { /// \brief Constructor @@ -54,17 +54,17 @@ public: public: void Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr < const sdf::Element > & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager & _eventMgr) override; + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & _eventMgr) override; // Documentation inherited public: void PostUpdate( - const ignition::gazebo::UpdateInfo & _info, - const ignition::gazebo::EntityComponentManager & _ecm) override; + const gz::sim::UpdateInfo & _info, + const gz::sim::EntityComponentManager & _ecm) override; /// \brief Private data pointer. diff --git a/ros_gz_sim/README.md b/ros_gz_sim/README.md index 227d53a4..cf4e8ca9 100644 --- a/ros_gz_sim/README.md +++ b/ros_gz_sim/README.md @@ -37,3 +37,77 @@ See more options with: ``` ros2 run ros_gz_sim create --helpshort ``` + +### Using `` to export model paths in `package.xml` + +The `` tag inside the `` tag of a `package.xml` file can be +used to add paths to `GZ_SIM_RESOURCE_PATH` and `GZ_SIM_SYSTEM_PLUGIN_PATH`, +which are environment variables used to configure Gazebo search paths for +resources (e.g. SDFormat files, meshes, etc) and plugins respectively. + +The values in the attributes `gazebo_model_path` and `gazebo_media_path` are +appended to `GZ_SIM_RESOURCE_PATH`. The value of `plugin_path` is appended to +`GZ_SIM_SYSTEM_PLUGIN_PATH`. See the +[Finding resources](https://gazebosim.org/api/sim/8/resources.html) tutorial to +learn more about these environment variables. + +The keyword `${prefix}` can be used when setting these values and it will be +expanded to the package's share path (i.e., the value of +`ros2 pkg prefix --share `) + +```xml + + + + + + +``` + +Thus the required directory needs to be installed from `CMakeLists.txt` + +```cmake +install(DIRECTORY models + DESTINATION share/${PROJECT_NAME}) +``` + +In order to reference the models in a ROS package unambiguously, it is +recommended to set the value of `gazebo_model_path` to be the parent +of the `prefix`. + +```xml + + + + +``` + +Consider an example where we have a ROS package called `my_awesome_pkg` +and it contains an SDFormat model cool `cool_robot`: + +```bash +my_awesome_pkg +├── models +│   └── cool_robot +│   ├── model.config +│   └── model.sdf +└── package.xml +``` + +With `gazebo_model_path="${prefix}/../` set up, we can +reference the `cool_robot` model in a world file using the package name +in the `uri`: + +```xml + + + + package://my_awesome_pkg/models/cool_robot + + + +``` + +However, if we set `gazebo_model_path=${prefix}/models`, we would +need to reference `cool_robot` as `package://cool_robot`, which +might have a name conflict with other models in the system. diff --git a/ros_gz_sim/launch/gz_sim.launch.py.in b/ros_gz_sim/launch/gz_sim.launch.py.in index 7bed5243..d41bede6 100644 --- a/ros_gz_sim/launch/gz_sim.launch.py.in +++ b/ros_gz_sim/launch/gz_sim.launch.py.in @@ -14,20 +14,86 @@ """Launch Gazebo Sim with command line arguments.""" +import os from os import environ +from ament_index_python.packages import get_package_share_directory +from catkin_pkg.package import InvalidPackage, PACKAGE_MANIFEST_FILENAME, parse_package +from ros2pkg.api import get_package_names from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.actions import ExecuteProcess, Shutdown from launch.substitutions import LaunchConfiguration +# Copied from https://github.com/ros-simulation/gazebo_ros_pkgs/blob/79fd94c6da76781a91499bc0f54b70560b90a9d2/gazebo_ros/scripts/gazebo_ros_paths.py +""" +Search for model, plugin and media paths exported by packages. + +e.g. + + + +${prefix} is replaced by package's share directory in install. + +Thus the required directory needs to be installed from CMakeLists.txt +e.g. install(DIRECTORY models + DESTINATION share/${PROJECT_NAME}) +""" + +class GazeboRosPaths: + + @staticmethod + def get_paths(): + gazebo_model_path = [] + gazebo_plugin_path = [] + gazebo_media_path = [] + + for package_name in get_package_names(): + package_share_path = get_package_share_directory(package_name) + package_file_path = os.path.join(package_share_path, PACKAGE_MANIFEST_FILENAME) + if os.path.isfile(package_file_path): + try: + package = parse_package(package_file_path) + except InvalidPackage: + continue + for export in package.exports: + if export.tagname == 'gazebo_ros': + if 'gazebo_model_path' in export.attributes: + xml_path = export.attributes['gazebo_model_path'] + xml_path = xml_path.replace('${prefix}', package_share_path) + gazebo_model_path.append(xml_path) + if 'plugin_path' in export.attributes: + xml_path = export.attributes['plugin_path'] + xml_path = xml_path.replace('${prefix}', package_share_path) + gazebo_plugin_path.append(xml_path) + if 'gazebo_media_path' in export.attributes: + xml_path = export.attributes['gazebo_media_path'] + xml_path = xml_path.replace('${prefix}', package_share_path) + gazebo_media_path.append(xml_path) + + gazebo_model_path = os.pathsep.join(gazebo_model_path + gazebo_media_path) + gazebo_plugin_path = os.pathsep.join(gazebo_plugin_path) + + return gazebo_model_path, gazebo_plugin_path + def launch_gz(context, *args, **kwargs): - env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': - ':'.join([environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', default=''), - environ.get('LD_LIBRARY_PATH', default='')]), - 'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': # TODO(CH3): To support pre-garden. Deprecated. - ':'.join([environ.get('IGN_GAZEBO_SYSTEM_PLUGIN_PATH', default=''), - environ.get('LD_LIBRARY_PATH', default='')])} + model_paths, plugin_paths = GazeboRosPaths.get_paths() + + env = { + "GZ_SIM_SYSTEM_PLUGIN_PATH": os.pathsep.join( + [ + environ.get("GZ_SIM_SYSTEM_PLUGIN_PATH", default=""), + environ.get("LD_LIBRARY_PATH", default=""), + plugin_paths, + ] + ), + "GZ_SIM_RESOURCE_PATH": os.pathsep.join( + [ + environ.get("GZ_SIM_RESOURCE_PATH", default=""), + model_paths, + ] + ), + } gz_args = LaunchConfiguration('gz_args').perform(context) gz_version = LaunchConfiguration('gz_version').perform(context)