Fix model namespacing

flatland_plugins/src/bumper.cpp

@@ -95,7 +95,8 @@ void Bumper::OnInitialize(const YAML::Node & config)
   }
 
   update_timer_.SetRate(update_rate_);
-  collisions_publisher_ = node_->create_publisher<flatland_msgs::msg::Collisions>(topic_name_, 1);
+  collisions_publisher_ = node_->create_publisher<flatland_msgs::msg::Collisions>(
+    GetModel()->NameSpaceTopic(topic_name_), 1);
 
   RCLCPP_DEBUG(
     rclcpp::get_logger("Bumper"),

flatland_plugins/src/diff_drive.cpp

@@ -122,13 +122,10 @@ void DiffDrive::OnInitialize(const YAML::Node & config)
     throw YAMLException("Body with name " + Q(body_name) + " does not exist");
   }
 
-  //append namespace to topics
-  std::string ns = this->GetModel()->GetNameSpace();
-  twist_topic = ns + "/" + twist_topic;
-  odom_topic = ns + "/" + odom_topic;
-  ground_truth_topic = ns + "/" + ground_truth_topic;
-  twist_pub_topic = ns + "/" + twist_pub_topic;
-
+  twist_topic = GetModel()->NameSpaceTopic(twist_topic);
+  odom_topic = GetModel()->NameSpaceTopic(odom_topic);
+  ground_truth_topic = GetModel()->NameSpaceTopic(ground_truth_topic);
+  twist_pub_topic = GetModel()->NameSpaceTopic(twist_pub_topic);
 
   // publish and subscribe to topics
   using std::placeholders::_1;

flatland_plugins/src/gps.cpp

@@ -15,6 +15,7 @@ void Gps::OnInitialize(const YAML::Node & config)
   tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node_);
   ParseParameters(config);
   update_timer_.SetRate(update_rate_);
+  topic_ = GetModel()->NameSpaceTopic(topic_);
   fix_publisher_ = node_->create_publisher<sensor_msgs::msg::NavSatFix>(topic_, 1);
 
   double c = cos(origin_.theta);

flatland_plugins/src/imu.cpp

@@ -112,6 +112,9 @@ void Imu::OnInitialize(const YAML::Node& config) {
 
   reader.EnsureAccessedAllKeys();
 
+  imu_topic = GetModel()->NameSpaceTopic(imu_topic);
+  ground_truth_topic = GetModel()->NameSpaceTopic(ground_truth_topic);
+
   body_ = GetModel()->GetBody(body_name);
   if (body_ == nullptr) {
     throw YAMLException("Body with name " + Q(body_name) + " does not exist");

flatland_plugins/src/tricycle_drive.cpp

@@ -141,6 +141,10 @@ void TricycleDrive::OnInitialize(const YAML::Node & config)
     throw YAMLException("Joint with name " + Q(rear_right_wj_name) + " does not exist");
   }
 
+  twist_topic = GetModel()->NameSpaceTopic(twist_topic);
+  odom_topic = GetModel()->NameSpaceTopic(odom_topic);
+  ground_truth_topic = GetModel()->NameSpaceTopic(ground_truth_topic);
+
   // validate the that joints fits the assumption of the robot model and
   // calculate rear wheel separation and wheel base
   ComputeJoints();

flatland_plugins/src/tween.cpp

@@ -107,6 +107,7 @@ void Tween::OnInitialize(const YAML::Node & config)
   // Boolean play pause topic
   std::string trigger_topic = reader.Get<std::string>("trigger_topic", "");
   if (trigger_topic != "") {
+    trigger_topic = GetModel()->NameSpaceTopic(trigger_topic);
     trigger_sub_ = node_->create_subscription<std_msgs::msg::Bool>(
       trigger_topic, 1, std::bind(&Tween::TriggerCallback, this, std::placeholders::_1));
   }