Set default namespace to model name if <namespace> is not specified (…

…#1319) * set default namespace to model name if not specified Signed-off-by: Ian Chen <[email protected]> * add legacy_namespace param to preserve existing behavior Signed-off-by: Ian Chen <[email protected]> * style fix Signed-off-by: Ian Chen <[email protected]> * another style fix Signed-off-by: Ian Chen <[email protected]> * add new Get overloaded function for visuals, update doc Signed-off-by: Ian Chen <[email protected]> Co-authored-by: Ian Chen <[email protected]>
tiiuae · Oct 28, 2021 · 7207bc1 · 7207bc1
1 parent 84a088b
commit 7207bc1
Show file tree

Hide file tree

Showing 24 changed files with 133 additions and 23 deletions.
diff --git a/gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp b/gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp
@@ -210,7 +210,7 @@ void GazeboRosAckermannDrive::Load(gazebo::physics::ModelPtr _model, sdf::Elemen
   physicsEngine->SetParam("friction_model", std::string("cone_model"));
 
   // Initialize ROS node
-  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf, _model);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_bumper.cpp b/gazebo_plugins/src/gazebo_ros_bumper.cpp
@@ -73,7 +73,7 @@ GazeboRosBumper::~GazeboRosBumper()
 void GazeboRosBumper::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
 {
   // Initialize ROS node
-  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf, _sensor);
 
   impl_->parent_sensor_ = std::dynamic_pointer_cast<gazebo::sensors::ContactSensor>(_sensor);
   if (!impl_->parent_sensor_) {

diff --git a/gazebo_plugins/src/gazebo_ros_camera.cpp b/gazebo_plugins/src/gazebo_ros_camera.cpp
@@ -152,7 +152,7 @@ GazeboRosCamera::~GazeboRosCamera()
 void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
 {
   // Initialize ROS node
-  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf, _sensor);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp
@@ -238,7 +238,7 @@ void GazeboRosDiffDrive::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr
   impl_->model_ = _model;
 
   // Initialize ROS node
-  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf, _model);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_elevator.cpp b/gazebo_plugins/src/gazebo_ros_elevator.cpp
@@ -51,7 +51,7 @@ void GazeboRosElevator::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _
   ElevatorPlugin::Load(_model, _sdf);
 
   // Initialize ROS node
-  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf, _model);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_force.cpp b/gazebo_plugins/src/gazebo_ros_force.cpp
@@ -97,7 +97,7 @@ void GazeboRosForce::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
   }
 
   // Subscribe to wrench messages
-  impl_->ros_node_ = gazebo_ros::Node::Get(sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(sdf, model);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp b/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp
@@ -92,7 +92,7 @@ GazeboRosFTSensor::~GazeboRosFTSensor()
 void GazeboRosFTSensor::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf)
 {
   // Initialize ROS node
-  impl_->rosnode_ = gazebo_ros::Node::Get(_sdf);
+  impl_->rosnode_ = gazebo_ros::Node::Get(_sdf, _model);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->rosnode_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_gps_sensor.cpp b/gazebo_plugins/src/gazebo_ros_gps_sensor.cpp
@@ -60,7 +60,7 @@ GazeboRosGpsSensor::~GazeboRosGpsSensor()
 
 void GazeboRosGpsSensor::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
 {
-  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf, _sensor);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp b/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp
@@ -103,7 +103,7 @@ GazeboRosHandOfGod::~GazeboRosHandOfGod()
 void GazeboRosHandOfGod::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf)
 {
   // Initialize ROS node
-  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf, _model);
 
   impl_->frame_ = _sdf->Get<std::string>("frame_id", "world").first;
 

diff --git a/gazebo_plugins/src/gazebo_ros_harness.cpp b/gazebo_plugins/src/gazebo_ros_harness.cpp
@@ -57,7 +57,7 @@ void GazeboRosHarness::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _s
   gazebo::HarnessPlugin::Load(_model, _sdf);
 
   // Initialize ROS node
-  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf, _model);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp b/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp
@@ -61,7 +61,7 @@ GazeboRosImuSensor::~GazeboRosImuSensor()
 
 void GazeboRosImuSensor::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
 {
-  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf, _sensor);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp b/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp
@@ -110,7 +110,7 @@ void GazeboRosJointPoseTrajectory::Load(gazebo::physics::ModelPtr model, sdf::El
   impl_->world_ = model->GetWorld();
 
   // Initialize ROS node
-  impl_->ros_node_ = gazebo_ros::Node::Get(sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(sdf, model);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp
@@ -88,7 +88,7 @@ GazeboRosJointStatePublisher::~GazeboRosJointStatePublisher()
 void GazeboRosJointStatePublisher::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
 {
   // ROS node
-  impl_->ros_node_ = gazebo_ros::Node::Get(sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(sdf, model);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_p3d.cpp b/gazebo_plugins/src/gazebo_ros_p3d.cpp
@@ -94,7 +94,7 @@ GazeboRosP3D::~GazeboRosP3D()
 void GazeboRosP3D::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
 {
   // Configure the plugin from the SDF file
-  impl_->ros_node_ = gazebo_ros::Node::Get(sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(sdf, model);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_planar_move.cpp b/gazebo_plugins/src/gazebo_ros_planar_move.cpp
@@ -132,7 +132,7 @@ void GazeboRosPlanarMove::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr
   impl_->world_ = _model->GetWorld();
 
   // Initialize ROS node
-  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf, _model);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_projector.cpp b/gazebo_plugins/src/gazebo_ros_projector.cpp
@@ -68,7 +68,7 @@ GazeboRosProjector::~GazeboRosProjector()
 void GazeboRosProjector::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf)
 {
   // Initialize ROS node
-  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf, _model);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_ray_sensor.cpp b/gazebo_plugins/src/gazebo_ros_ray_sensor.cpp
@@ -101,7 +101,7 @@ GazeboRosRaySensor::~GazeboRosRaySensor()
 void GazeboRosRaySensor::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
 {
   // Create ros_node configured from sdf
-  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf, _sensor);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_template.cpp b/gazebo_plugins/src/gazebo_ros_template.cpp
@@ -46,7 +46,7 @@ void GazeboRosTemplate::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sd
   // Create a GazeboRos node instead of a common ROS node.
   // Pass it SDF parameters so common options like namespace and remapping
   // can be handled.
-  impl_->ros_node_ = gazebo_ros::Node::Get(sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(sdf, model);
 
   // The model pointer gives you direct access to the physics object,
   // for example:

diff --git a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp
@@ -239,7 +239,7 @@ void GazeboRosTricycleDrive::Load(gazebo::physics::ModelPtr _model, sdf::Element
   impl_->model_ = _model;
 
   // Initialize ROS node
-  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf, _model);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp b/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp
@@ -97,7 +97,7 @@ void GazeboRosVacuumGripper::Load(gazebo::physics::ModelPtr _model, sdf::Element
   impl_->world_ = _model->GetWorld();
 
   // Initialize ROS node
-  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf, _model);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->ros_node_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_video.cpp b/gazebo_plugins/src/gazebo_ros_video.cpp
@@ -197,7 +197,7 @@ void GazeboRosVideo::Load(
   gazebo::rendering::VisualPtr _parent,
   sdf::ElementPtr _sdf)
 {
-  impl_->rosnode_ = gazebo_ros::Node::Get(_sdf);
+  impl_->rosnode_ = gazebo_ros::Node::Get(_sdf, _parent);
 
   // Get QoS profiles
   const gazebo_ros::QoS & qos = impl_->rosnode_->get_qos();

diff --git a/gazebo_plugins/src/gazebo_ros_wheel_slip.cpp b/gazebo_plugins/src/gazebo_ros_wheel_slip.cpp
@@ -52,7 +52,7 @@ void GazeboRosWheelSlip::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr
   WheelSlipPlugin::Load(_model, _sdf);
 
   // Initialize ROS node
-  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf);
+  impl_->ros_node_ = gazebo_ros::Node::Get(_sdf, _model);
 
   auto param_change_callback =
     [this](std::vector<rclcpp::Parameter> parameters) {

diff --git a/gazebo_ros/include/gazebo_ros/node.hpp b/gazebo_ros/include/gazebo_ros/node.hpp
@@ -17,6 +17,10 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <gazebo/physics/Model.hh>
+#include <gazebo/rendering/Visual.hh>
+#include <gazebo/sensors/Sensor.hh>
+
 #include <gazebo_ros/executor.hpp>
 #include <gazebo_ros/node_visibility_control.h>
 #include <gazebo_ros/qos.hpp>
@@ -66,6 +70,16 @@ class GAZEBO_ROS_NODE_PUBLIC Node : public rclcpp::Node
    *   <ros>
    *    <!-- Namespace of the node -->
    *    <namespace>/my_ns</namespace>
+   *    <!-- Legacy namespace behavior. True to default to the root namespace
+   *         if <namespace> is not specified, otherwise false to default
+   *         namespace to the the model name -->
+   *    <!-- Legacy behavior for setting namespace when <namespace> is not
+   *         specified.
+   *         When <legacy_namespace> is unspecified or set to true, the root
+   *         namespace `/` is the default.
+   *         When <legacy_namespace> is set to false, the default namespace is
+   *         the model name.
+   *    <legacy_namespace>true</legacy_namespace>
    *    <!-- Command line arguments sent to Node's constructor for remappings -->
    *    <argument>__name:=super_cool_node</argument>
    *    <argument>__log_level:=debug</argument>
@@ -86,6 +100,39 @@ class GAZEBO_ROS_NODE_PUBLIC Node : public rclcpp::Node
    */
   static SharedPtr Get(sdf::ElementPtr _sdf);
 
+  /// Get reference to a #gazebo_ros::Node and add it to the global
+  /// #gazebo_ros::Executor.
+  /// This overloaded function allows users to specify a default namespace if
+  /// <namespace> is not present
+  static SharedPtr Get(
+    sdf::ElementPtr _sdf,
+    const std::string & _defaultNamespace);
+
+  /// Get reference to a #gazebo_ros::Node and add it to the global
+  /// #gazebo_ros::Executor.
+  /// This overloaded function sets the node namespace to the parent model name
+  /// if <namespace> is not present
+  static SharedPtr Get(
+    sdf::ElementPtr _sdf,
+    const gazebo::physics::ModelPtr & parent);
+
+  /// Get reference to a #gazebo_ros::Node and add it to the global
+  /// #gazebo_ros::Executor.
+  /// This overloaded function sets the node namespace to the name of the
+  /// parent model containing this sensor if <namespace> is not present
+  static SharedPtr Get(
+    sdf::ElementPtr _sdf,
+    const gazebo::sensors::SensorPtr & parent);
+
+  /// Get reference to a #gazebo_ros::Node and add it to the global
+  /// #gazebo_ros::Executor.
+  /// This overloaded function sets the node namespace to the name of the
+  /// parent model containing this visual if <namespace> is not present
+  /// if <namespace> is not present
+  static SharedPtr Get(
+    sdf::ElementPtr _sdf,
+    const gazebo::rendering::VisualPtr & parent);
+
   /// Create a #gazebo_ros::Node and add it to the global #gazebo_ros::Executor.
   /**
    * \details This will call rclcpp::init if it hasn't been called yet.

diff --git a/gazebo_ros/src/node.cpp b/gazebo_ros/src/node.cpp
@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <gazebo/common/CommonIface.hh>
+
 #include <gazebo_ros/node.hpp>
 
 #include <rcl/arguments.h>
@@ -35,10 +37,58 @@ Node::~Node()
 }
 
 Node::SharedPtr Node::Get(sdf::ElementPtr sdf)
+{
+  return Get(sdf, "/");
+}
+
+Node::SharedPtr Node::Get(
+  sdf::ElementPtr sdf,
+  const gazebo::physics::ModelPtr & parent)
+{
+  std::string modelName;
+  if (parent) {
+    modelName = parent->GetName();
+  }
+
+  return Get(sdf, "/" + modelName);
+}
+
+Node::SharedPtr Node::Get(
+  sdf::ElementPtr sdf,
+  const gazebo::sensors::SensorPtr & parent)
+{
+  std::string modelName;
+  std::vector<std::string> values;
+  std::string scopedName = parent->ScopedName();
+  values = gazebo::common::split(scopedName, "::");
+  if (values.size() < 2) {
+    modelName = "";
+  } else {
+    // the second element is the model name; the first one is the world name
+    modelName = values[1];
+  }
+
+  return Get(sdf, "/" + modelName);
+}
+
+Node::SharedPtr Node::Get(
+  sdf::ElementPtr sdf,
+  const gazebo::rendering::VisualPtr & parent)
+{
+  std::string modelName;
+  if (parent) {
+    modelName = parent->GetRootVisual()->Name();
+  }
+
+  return Get(sdf, "/" + modelName);
+}
+
+Node::SharedPtr Node::Get(
+  sdf::ElementPtr sdf,
+  const std::string & defaultNamespace)
 {
   // Initialize arguments
   std::string name = "";
-  std::string ns = "/";
   std::vector<std::string> arguments;
   std::vector<rclcpp::Parameter> parameter_overrides;
 
@@ -53,6 +103,19 @@ Node::SharedPtr Node::Get(sdf::ElementPtr sdf)
     sdf = sdf->GetElement("ros");
   }
 
+  // Legacy namespace
+  // True to default to the root (/) namespace if <namespace> is not specified
+  // False to use the model name as the namespace if <namespace> is not
+  // specified.
+  // todo(anyone) change this to false in humble
+  bool legacyNamespace = true;
+  if (sdf->HasElement("legacy_namespace")) {
+    legacyNamespace = sdf->Get<bool>("legacy_namespace");
+  }
+  std::string ns =
+    (legacyNamespace || defaultNamespace.empty() ||
+    defaultNamespace[0] != '/') ? "/" : defaultNamespace;
+
   // Set namespace if tag is present
   if (sdf->HasElement("namespace")) {
     ns = sdf->GetElement("namespace")->Get<std::string>();