Possible to read robot_description from topic

mujoco_ros2_control/include/mujoco_ros2_control/mujoco_rendering.hpp

@@ -34,7 +34,7 @@ class MujocoRendering
   void operator=(const MujocoRendering &) = delete;
 
   static MujocoRendering *get_instance();
-  void init(rclcpp::Node::SharedPtr &node, mjModel *mujoco_model, mjData *mujoco_data);
+  void init(mjModel *mujoco_model, mjData *mujoco_data);
   bool is_close_flag_raised();
   void update();
   void close();
@@ -51,7 +51,6 @@ class MujocoRendering
   void scroll_callback_impl(GLFWwindow *window, double xoffset, double yoffset);
 
   static MujocoRendering *instance_;
-  rclcpp::Node::SharedPtr node_;  // TODO(sangtaek): delete node and add logger
   mjModel *mj_model_;
   mjData *mj_data_;
   mjvCamera mjv_cam_;

mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp

@@ -22,6 +22,7 @@
 #define MUJOCO_ROS2_CONTROL__MUJOCO_ROS2_CONTROL_HPP_
 
 #include <memory>
+#include <string>
 
 #include "controller_manager/controller_manager.hpp"
 #include "pluginlib/class_loader.hpp"
@@ -37,23 +38,22 @@ namespace mujoco_ros2_control
 class MujocoRos2Control
 {
 public:
-  MujocoRos2Control(rclcpp::Node::SharedPtr &node, mjModel *mujoco_model, mjData *mujoco_data);
+  MujocoRos2Control(mjModel *mujoco_model, mjData *mujoco_data);
   ~MujocoRos2Control();
-  void init();
+  void init(std::shared_ptr<rclcpp::Executor> executor);
   void update();
 
 private:
   void publish_sim_time(rclcpp::Time sim_time);
-  rclcpp::Node::SharedPtr node_;  // TODO(sangteak601): delete node
+  std::string get_robot_description();
   mjModel *mj_model_;
   mjData *mj_data_;
 
   rclcpp::Logger logger_;
   std::shared_ptr<pluginlib::ClassLoader<MujocoSystemInterface>> robot_hw_sim_loader_;
 
   std::shared_ptr<controller_manager::ControllerManager> controller_manager_;
-  rclcpp::executors::MultiThreadedExecutor::SharedPtr cm_executor_;
-  std::thread cm_thread_;
+  rclcpp::Executor::SharedPtr cm_executor_;
   bool stop_cm_thread_;
   rclcpp::Duration control_period_;
 

mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp

@@ -51,8 +51,8 @@ class MujocoSystem : public MujocoSystemInterface
     const rclcpp::Time &time, const rclcpp::Duration &period) override;
 
   bool init_sim(
-    rclcpp::Node::SharedPtr &node, mjModel *mujoco_model, mjData *mujoco_data,
-    const urdf::Model &urdf_model, const hardware_interface::HardwareInfo &hardware_info) override;
+    mjModel *mujoco_model, mjData *mujoco_data, const urdf::Model &urdf_model,
+    const hardware_interface::HardwareInfo &hardware_info) override;
 
   struct JointState
   {

mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp

@@ -34,11 +34,10 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface
 {
 public:
   virtual bool init_sim(
-    rclcpp::Node::SharedPtr &node, mjModel *mujoco_model, mjData *mujoco_data,
-    const urdf::Model &urdf_model, const hardware_interface::HardwareInfo &hardware_info) = 0;
+    mjModel *mujoco_model, mjData *mujoco_data, const urdf::Model &urdf_model,
+    const hardware_interface::HardwareInfo &hardware_info) = 0;
 
 protected:
-  rclcpp::Node::SharedPtr node_;  // TODO(sangteak601): need node?
 };
 }  // namespace mujoco_ros2_control
 

mujoco_ros2_control/src/mujoco_rendering.cpp

@@ -45,10 +45,8 @@ MujocoRendering::MujocoRendering()
 {
 }
 
-void MujocoRendering::init(
-  rclcpp::Node::SharedPtr &node, mjModel *mujoco_model, mjData *mujoco_data)
+void MujocoRendering::init(mjModel *mujoco_model, mjData *mujoco_data)
 {
-  node_ = node;
   mj_model_ = mujoco_model;
   mj_data_ = mujoco_data;
 

mujoco_ros2_control/src/mujoco_ros2_control.cpp

@@ -26,12 +26,10 @@
 
 namespace mujoco_ros2_control
 {
-MujocoRos2Control::MujocoRos2Control(
-  rclcpp::Node::SharedPtr &node, mjModel *mujoco_model, mjData *mujoco_data)
-    : node_(node),
-      mj_model_(mujoco_model),
+MujocoRos2Control::MujocoRos2Control(mjModel *mujoco_model, mjData *mujoco_data)
+    : mj_model_(mujoco_model),
       mj_data_(mujoco_data),
-      logger_(rclcpp::get_logger(node_->get_name() + std::string(".mujoco_ros2_control"))),
+      logger_(rclcpp::get_logger("mujoco_ros2_control")),
       control_period_(rclcpp::Duration(1, 0)),
       last_update_sim_time_ros_(0, 0, RCL_ROS_TIME)
 {
@@ -42,19 +40,54 @@ MujocoRos2Control::~MujocoRos2Control()
   stop_cm_thread_ = true;
   cm_executor_->remove_node(controller_manager_);
   cm_executor_->cancel();
-  cm_thread_.join();
 }
 
-void MujocoRos2Control::init()
+std::string MujocoRos2Control::get_robot_description()
 {
-  clock_publisher_ = node_->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 10);
+  // Getting robot description from parameter first. If not set trying from topic
+  std::string robot_description;
+
+  auto node = std::make_shared<rclcpp::Node>(
+    "robot_description_node",
+    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
+  cm_executor_->add_node(node);
+
+  if (node->has_parameter("robot_description"))
+  {
+    robot_description = node->get_parameter("robot_description").as_string();
+    return robot_description;
+  }
+
+  RCLCPP_WARN(
+    logger_, "Impossible to get robot_description from parameter. Getting it from topic...");
+
+  auto robot_description_sub = node->create_subscription<std_msgs::msg::String>(
+    "robot_description", rclcpp::QoS(1).transient_local(),
+    [&](const std_msgs::msg::String::SharedPtr msg)
+    {
+      if (!msg->data.empty() && robot_description.empty()) robot_description = msg->data;
+    });
+
+  while (robot_description.empty() && rclcpp::ok())
+  {
+    cm_executor_->spin_once();
+    RCLCPP_INFO(node->get_logger(), "Waiting for robot description message");
+    rclcpp::sleep_for(std::chrono::milliseconds(500));
+  }
+
+  return robot_description;
+}
+
+void MujocoRos2Control::init(rclcpp::Executor::SharedPtr executor)
+{
+  cm_executor_ = executor;
+  std::string urdf_string = this->get_robot_description();
+
   // Read urdf from ros parameter server then
   // setup actuators and mechanism control node.
-  std::string urdf_string;
   std::vector<hardware_interface::HardwareInfo> control_hardware_info;
   try
   {
-    urdf_string = node_->get_parameter("robot_description").as_string();
     control_hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf_string);
   }
   catch (const std::runtime_error &ex)
@@ -103,7 +136,7 @@ void MujocoRos2Control::init()
 
     urdf::Model urdf_model;
     urdf_model.initString(urdf_string);
-    if (!mujoco_system->init_sim(node_, mj_model_, mj_data_, urdf_model, hardware))
+    if (!mujoco_system->init_sim(mj_model_, mj_data_, urdf_model, hardware))
     {
       RCLCPP_FATAL(logger_, "Could not initialize robot simulation interface");
       return;
@@ -119,12 +152,13 @@ void MujocoRos2Control::init()
 
   // Create the controller manager
   RCLCPP_INFO(logger_, "Loading controller_manager");
-  cm_executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
   controller_manager_ = std::make_shared<controller_manager::ControllerManager>(
-    std::move(resource_manager), cm_executor_, "controller_manager", node_->get_namespace());
+    std::move(resource_manager), cm_executor_, "controller_manager");
 
   cm_executor_->add_node(controller_manager_);
 
+  clock_publisher_ = controller_manager_->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 10);
+
   if (!controller_manager_->has_parameter("update_rate"))
   {
     RCLCPP_ERROR_STREAM(logger_, "controller manager doesn't have an update_rate parameter");
@@ -140,14 +174,6 @@ void MujocoRos2Control::init()
     rclcpp::Parameter("use_sim_time", rclcpp::ParameterValue(true)));
 
   stop_cm_thread_ = false;
-  auto spin = [this]()
-  {
-    while (rclcpp::ok() && !stop_cm_thread_)
-    {
-      cm_executor_->spin_once();
-    }
-  };
-  cm_thread_ = std::thread(spin);
 }
 
 void MujocoRos2Control::update()

mujoco_ros2_control/src/mujoco_ros2_control_node.cpp

@@ -61,16 +61,23 @@ int main(int argc, const char **argv)
   mujoco_data = mj_makeData(mujoco_model);
 
   // initialize mujoco control
-  auto control = mujoco_ros2_control::MujocoRos2Control(node, mujoco_model, mujoco_data);
-  control.init();
+  auto mujoco_control = mujoco_ros2_control::MujocoRos2Control(mujoco_model, mujoco_data);
+
+  auto executor = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
+
+  mujoco_control.init(executor);
   RCLCPP_INFO_STREAM(
     node->get_logger(), "Mujoco ros2 controller has been successfully initialized !");
 
   // initialize mujoco redering
   auto rendering = mujoco_ros2_control::MujocoRendering::get_instance();
-  rendering->init(node, mujoco_model, mujoco_data);
+  rendering->init(mujoco_model, mujoco_data);
   RCLCPP_INFO_STREAM(node->get_logger(), "Mujoco rendering has been successfully initialized !");
 
+  // Thread to allow node's spinning
+  auto spin = [executor]() { executor->spin(); };
+  std::thread spin_thread = std::thread(spin);
+
   // run main loop, target real-time simulation and 60 fps rendering
   while (rclcpp::ok() && !rendering->is_close_flag_raised())
   {
@@ -81,13 +88,15 @@ int main(int argc, const char **argv)
     mjtNum simstart = mujoco_data->time;
     while (mujoco_data->time - simstart < 1.0 / 60.0)
     {
-      control.update();
+      mujoco_control.update();
     }
     rendering->update();
   }
 
   rendering->close();
 
+  if (spin_thread.joinable()) spin_thread.join();
+
   // free MuJoCo model and data
   mj_deleteData(mujoco_data);
   mj_deleteModel(mujoco_model);

mujoco_ros2_control/src/mujoco_system.cpp

@@ -138,14 +138,13 @@ hardware_interface::return_type MujocoSystem::write(
 }
 
 bool MujocoSystem::init_sim(
-  rclcpp::Node::SharedPtr &node, mjModel *mujoco_model, mjData *mujoco_data,
-  const urdf::Model &urdf_model, const hardware_interface::HardwareInfo &hardware_info)
+  mjModel *mujoco_model, mjData *mujoco_data, const urdf::Model &urdf_model,
+  const hardware_interface::HardwareInfo &hardware_info)
 {
-  node_ = node;
   mj_model_ = mujoco_model;
   mj_data_ = mujoco_data;
 
-  logger_ = rclcpp::get_logger(node_->get_name() + std::string("mujoco_system"));
+  logger_ = rclcpp::get_logger("mujoco_system");
 
   register_joints(urdf_model, hardware_info);
   register_sensors(urdf_model, hardware_info);