diff --git a/README.md b/README.md index 8dba75c2..e730f5e4 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,7 @@ Galactic | Edifice | [galactic](https://github.com/gazebosim/ros_gz/tree/galacti 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) | [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) | only from source +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 diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md index 0715ad8b..c4e95ec7 100644 --- a/ros_gz_bridge/README.md +++ b/ros_gz_bridge/README.md @@ -5,68 +5,67 @@ between ROS and Gazebo Transport. The following message types can be bridged for topics: -| ROS type | Gazebo type | -|--------------------------------------|:-------------------------------------------:| -| builtin_interfaces/msg/Time | ignition::msgs::Time | -| std_msgs/msg/Bool | ignition::msgs::Boolean | -| std_msgs/msg/ColorRGBA | ignition::msgs::Color | -| std_msgs/msg/Empty | ignition::msgs::Empty | -| std_msgs/msg/Float32 | ignition::msgs::Float | -| std_msgs/msg/Float64 | ignition::msgs::Double | -| std_msgs/msg/Header | ignition::msgs::Header | -| std_msgs/msg/Int32 | ignition::msgs::Int32 | -| std_msgs/msg/UInt32 | ignition::msgs::UInt32 | -| std_msgs/msg/String | ignition::msgs::StringMsg | -| geometry_msgs/msg/Wrench | ignition::msgs::Wrench | -| geometry_msgs/msg/WrenchStamped | ignition::msgs::Wrench | -| geometry_msgs/msg/Quaternion | ignition::msgs::Quaternion | -| geometry_msgs/msg/Vector3 | ignition::msgs::Vector3d | -| geometry_msgs/msg/Point | ignition::msgs::Vector3d | -| geometry_msgs/msg/Pose | ignition::msgs::Pose | -| geometry_msgs/msg/PoseArray | ignition::msgs::Pose_V | -| geometry_msgs/msg/PoseWithCovariance | ignition::msgs::PoseWithCovariance | -| geometry_msgs/msg/PoseStamped | ignition::msgs::Pose | -| geometry_msgs/msg/Transform | ignition::msgs::Pose | -| geometry_msgs/msg/TransformStamped | ignition::msgs::Pose | -| geometry_msgs/msg/Twist | ignition::msgs::Twist | -| geometry_msgs/msg/TwistStamped | ignition::msgs::Twist | -| geometry_msgs/msg/TwistWithCovariance| ignition::msgs::TwistWithCovariance | -| gps_msgs/GPSFix | ignition::msgs::NavSat | -| nav_msgs/msg/Odometry | ignition::msgs::Odometry | -| nav_msgs/msg/Odometry | ignition::msgs::OdometryWithCovariance | -| rcl_interfaces/msg/ParameterValue | ignition::msgs::Any | -| ros_gz_interfaces/msg/Altimeter | ignition::msgs::Altimeter | -| ros_gz_interfaces/msg/Contact | ignition::msgs::Contact | -| ros_gz_interfaces/msg/Contacts | ignition::msgs::Contacts | -| ros_gz_interfaces/msg/Dataframe | ignition::msgs::Dataframe | -| ros_gz_interfaces/msg/Entity | ignition::msgs::Entity | -| ros_gz_interfaces/msg/Float32Array | ignition::msgs::Float_V | -| ros_gz_interfaces/msg/GuiCamera | ignition::msgs::GUICamera | -| ros_gz_interfaces/msg/JointWrench | ignition::msgs::JointWrench | -| ros_gz_interfaces/msg/Light | ignition::msgs::Light | -| ros_gz_interfaces/msg/SensorNoise | ignition::msgs::SensorNoise | -| ros_gz_interfaces/msg/StringVec | ignition::msgs::StringMsg_V | -| ros_gz_interfaces/msg/TrackVisual | ignition::msgs::TrackVisual | -| ros_gz_interfaces/msg/VideoRecord | ignition::msgs::VideoRecord | -| ros_gz_interfaces/msg/WorldControl | ignition::msgs::WorldControl | -| rosgraph_msgs/msg/Clock | ignition::msgs::Clock | -| sensor_msgs/msg/BatteryState | ignition::msgs::BatteryState | -| sensor_msgs/msg/CameraInfo | ignition::msgs::CameraInfo | -| sensor_msgs/msg/FluidPressure | ignition::msgs::FluidPressure | -| sensor_msgs/msg/Imu | ignition::msgs::IMU | -| sensor_msgs/msg/Image | ignition::msgs::Image | -| sensor_msgs/msg/JointState | ignition::msgs::Model | -| sensor_msgs/msg/Joy | ignition::msgs::Joy | -| sensor_msgs/msg/LaserScan | ignition::msgs::LaserScan | -| sensor_msgs/msg/MagneticField | ignition::msgs::Magnetometer | -| sensor_msgs/msg/NavSatFix | ignition::msgs::NavSat | -| sensor_msgs/msg/PointCloud2 | ignition::msgs::PointCloudPacked | -| tf2_msgs/msg/TFMessage | ignition::msgs::Pose_V | -| trajectory_msgs/msg/JointTrajectory | ignition::msgs::JointTrajectory | -| vision_msgs/msg/Detection2D | ignition::msgs::AnnotatedAxisAligned2DBox | -| vision_msgs/msg/Detection2DArray | ignition::msgs::AnnotatedAxisAligned2DBox_V | -| vision_msgs/msg/Detection3D | ignition::msgs::AnnotatedOriented3DBox | -| vision_msgs/msg/Detection3DArray | ignition::msgs::AnnotatedOriented3DBox_V | +| ROS type | Gazebo type | +|---------------------------------------------|:-------------------------------------------:| +| builtin_interfaces/msg/Time | ignition::msgs::Time | +| std_msgs/msg/Bool | ignition::msgs::Boolean | +| std_msgs/msg/ColorRGBA | ignition::msgs::Color | +| std_msgs/msg/Empty | ignition::msgs::Empty | +| std_msgs/msg/Float32 | ignition::msgs::Float | +| std_msgs/msg/Float64 | ignition::msgs::Double | +| std_msgs/msg/Header | ignition::msgs::Header | +| std_msgs/msg/Int32 | ignition::msgs::Int32 | +| std_msgs/msg/UInt32 | ignition::msgs::UInt32 | +| std_msgs/msg/String | ignition::msgs::StringMsg | +| geometry_msgs/msg/Wrench | ignition::msgs::Wrench | +| geometry_msgs/msg/WrenchStamped | ignition::msgs::Wrench | +| geometry_msgs/msg/Quaternion | ignition::msgs::Quaternion | +| geometry_msgs/msg/Vector3 | ignition::msgs::Vector3d | +| geometry_msgs/msg/Point | ignition::msgs::Vector3d | +| geometry_msgs/msg/Pose | ignition::msgs::Pose | +| geometry_msgs/msg/PoseArray | ignition::msgs::Pose_V | +| geometry_msgs/msg/PoseWithCovariance | ignition::msgs::PoseWithCovariance | +| geometry_msgs/msg/PoseStamped | ignition::msgs::Pose | +| geometry_msgs/msg/Transform | ignition::msgs::Pose | +| geometry_msgs/msg/TransformStamped | ignition::msgs::Pose | +| geometry_msgs/msg/Twist | ignition::msgs::Twist | +| geometry_msgs/msg/TwistStamped | ignition::msgs::Twist | +| geometry_msgs/msg/TwistWithCovariance | ignition::msgs::TwistWithCovariance | +| geometry_msgs/msg/TwistWithCovarianceStamped| ignition::msgs::TwistWithCovariance | +| gps_msgs/GPSFix | ignition::msgs::NavSat | +| nav_msgs/msg/Odometry | ignition::msgs::Odometry | +| nav_msgs/msg/Odometry | ignition::msgs::OdometryWithCovariance | +| rcl_interfaces/msg/ParameterValue | ignition::msgs::Any | +| ros_gz_interfaces/msg/Altimeter | ignition::msgs::Altimeter | +| ros_gz_interfaces/msg/Contact | ignition::msgs::Contact | +| ros_gz_interfaces/msg/Contacts | ignition::msgs::Contacts | +| ros_gz_interfaces/msg/Dataframe | ignition::msgs::Dataframe | +| ros_gz_interfaces/msg/Entity | ignition::msgs::Entity | +| ros_gz_interfaces/msg/Float32Array | ignition::msgs::Float_V | +| ros_gz_interfaces/msg/GuiCamera | ignition::msgs::GUICamera | +| ros_gz_interfaces/msg/JointWrench | ignition::msgs::JointWrench | +| ros_gz_interfaces/msg/Light | ignition::msgs::Light | +| ros_gz_interfaces/msg/SensorNoise | ignition::msgs::SensorNoise | +| ros_gz_interfaces/msg/StringVec | ignition::msgs::StringMsg_V | +| ros_gz_interfaces/msg/TrackVisual | ignition::msgs::TrackVisual | +| ros_gz_interfaces/msg/VideoRecord | ignition::msgs::VideoRecord | +| ros_gz_interfaces/msg/WorldControl | ignition::msgs::WorldControl | +| rosgraph_msgs/msg/Clock | ignition::msgs::Clock | +| sensor_msgs/msg/BatteryState | ignition::msgs::BatteryState | +| sensor_msgs/msg/CameraInfo | ignition::msgs::CameraInfo | +| sensor_msgs/msg/FluidPressure | ignition::msgs::FluidPressure | +| sensor_msgs/msg/Imu | ignition::msgs::IMU | +| sensor_msgs/msg/Image | ignition::msgs::Image | +| sensor_msgs/msg/JointState | ignition::msgs::Model | +| sensor_msgs/msg/Joy | ignition::msgs::Joy | +| sensor_msgs/msg/LaserScan | ignition::msgs::LaserScan | +| sensor_msgs/msg/MagneticField | ignition::msgs::Magnetometer | +| sensor_msgs/msg/NavSatFix | ignition::msgs::NavSat | +| sensor_msgs/msg/PointCloud2 | ignition::msgs::PointCloudPacked | +| tf2_msgs/msg/TFMessage | ignition::msgs::Pose_V | +| trajectory_msgs/msg/JointTrajectory | ignition::msgs::JointTrajectory | +| vision_msgs/msg/Detection3D | ignition::msgs::AnnotatedOriented3DBox | +| vision_msgs/msg/Detection3DArray | ignition::msgs::AnnotatedOriented3DBox_V | And the following for services: diff --git a/ros_gz_bridge/include/ros_gz_bridge/convert/geometry_msgs.hpp b/ros_gz_bridge/include/ros_gz_bridge/convert/geometry_msgs.hpp index 43860e69..70537def 100644 --- a/ros_gz_bridge/include/ros_gz_bridge/convert/geometry_msgs.hpp +++ b/ros_gz_bridge/include/ros_gz_bridge/convert/geometry_msgs.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -189,6 +190,18 @@ convert_gz_to_ros( const gz::msgs::TwistWithCovariance & gz_msg, geometry_msgs::msg::TwistWithCovariance & ros_msg); +template<> +void +convert_ros_to_gz( + const geometry_msgs::msg::TwistWithCovarianceStamped & ros_msg, + gz::msgs::TwistWithCovariance & gz_msg); + +template<> +void +convert_gz_to_ros( + const gz::msgs::TwistWithCovariance & gz_msg, + geometry_msgs::msg::TwistWithCovarianceStamped & ros_msg); + template<> void convert_ros_to_gz( diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py index 321cf502..d17fb56d 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -42,6 +42,7 @@ Mapping('Twist', 'Twist'), Mapping('TwistStamped', 'Twist'), Mapping('TwistWithCovariance', 'TwistWithCovariance'), + Mapping('TwistWithCovarianceStamped', 'TwistWithCovariance'), Mapping('Wrench', 'Wrench'), Mapping('WrenchStamped', 'Wrench'), Mapping('Vector3', 'Vector3d'), diff --git a/ros_gz_bridge/src/convert/geometry_msgs.cpp b/ros_gz_bridge/src/convert/geometry_msgs.cpp index 60ae1413..ad83ba6d 100644 --- a/ros_gz_bridge/src/convert/geometry_msgs.cpp +++ b/ros_gz_bridge/src/convert/geometry_msgs.cpp @@ -306,6 +306,26 @@ convert_gz_to_ros( } } +template<> +void +convert_ros_to_gz( + const geometry_msgs::msg::TwistWithCovarianceStamped & ros_msg, + gz::msgs::TwistWithCovariance & gz_msg) +{ + convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_twist()->mutable_header())); + convert_ros_to_gz(ros_msg.twist, gz_msg); +} + +template<> +void +convert_gz_to_ros( + const gz::msgs::TwistWithCovariance & gz_msg, + geometry_msgs::msg::TwistWithCovarianceStamped & ros_msg) +{ + convert_gz_to_ros(gz_msg.twist().header(), ros_msg.header); + convert_gz_to_ros(gz_msg, ros_msg.twist); +} + template<> void convert_ros_to_gz( diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index 7142d7c4..71a651b3 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -433,12 +433,15 @@ void createTestMsg(gz::msgs::TwistWithCovariance & _msg) { gz::msgs::Vector3d linear_msg; gz::msgs::Vector3d angular_msg; + gz::msgs::Header header_msg; createTestMsg(linear_msg); createTestMsg(angular_msg); + createTestMsg(header_msg); _msg.mutable_twist()->mutable_linear()->CopyFrom(linear_msg); _msg.mutable_twist()->mutable_angular()->CopyFrom(angular_msg); + _msg.mutable_twist()->mutable_header()->CopyFrom(header_msg); for (int i = 0; i < 36; i++) { _msg.mutable_covariance()->add_data(i); } diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index 9d9e3c25..76e80d29 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -484,6 +484,18 @@ void compareTestMsg(const std::shared_ptr & _msg) +{ + compareTestMsg(std::make_shared(_msg->twist)); + compareTestMsg(std::make_shared(_msg->header)); +} + void createTestMsg(geometry_msgs::msg::Wrench & _msg) { createTestMsg(_msg.force); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index e97fa82b..6bc395cb 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -319,6 +320,14 @@ void createTestMsg(geometry_msgs::msg::TwistWithCovariance & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(geometry_msgs::msg::TwistWithCovarianceStamped & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare +void compareTestMsg(const std::shared_ptr & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(geometry_msgs::msg::Wrench & _msg); diff --git a/ros_gz_sim/README.md b/ros_gz_sim/README.md index c4561a5d..e0b05f62 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..8f87fb7d 100644 --- a/ros_gz_sim/launch/gz_sim.launch.py.in +++ b/ros_gz_sim/launch/gz_sim.launch.py.in @@ -14,20 +14,99 @@ """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, + ] + ), + "IGN_GAZEBO_SYSTEM_PLUGIN_PATH": os.pathsep.join( # TODO(azeey): To support pre-garden. Deprecated. + [ + environ.get("IGN_GAZEBO_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, + ] + ), + "IGN_GAZEBO_RESOURCE_PATH": os.pathsep.join( # TODO(azeey): To support pre-garden. Deprecated. + [ + environ.get("IGN_GAZEBO_RESOURCE_PATH", default=""), + model_paths, + ] + ), + } gz_args = LaunchConfiguration('gz_args').perform(context) gz_version = LaunchConfiguration('gz_version').perform(context)