Merge pull request #34 from pucciland95/main

Possible to read robot_description from topic

Author: sangteak601

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"
@@ -44,15 +45,16 @@ class MujocoRos2Control
 
 private:
   void publish_sim_time(rclcpp::Time sim_time);
-  rclcpp::Node::SharedPtr node_;  // TODO(sangteak601): delete node
+  std::string get_robot_description();
+  rclcpp::Node::SharedPtr node_;
   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_;
+  rclcpp::Executor::SharedPtr cm_executor_;
   std::thread cm_thread_;
   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_ros2_control.cpp

@@ -42,19 +42,57 @@ MujocoRos2Control::~MujocoRos2Control()
   stop_cm_thread_ = true;
   cm_executor_->remove_node(controller_manager_);
   cm_executor_->cancel();
-  cm_thread_.join();
+
+  if (cm_thread_.joinable()) cm_thread_.join();
+}
+
+std::string MujocoRos2Control::get_robot_description()
+{
+  // 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));
+
+  if (node->has_parameter("robot_description"))
+  {
+    robot_description = node->get_parameter("robot_description").as_string();
+    return robot_description;
+  }
+
+  RCLCPP_WARN(
+    logger_,
+    "Failed to get robot_description from parameter. Will listen on the ~/robot_description "
+    "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())
+  {
+    rclcpp::spin_some(node);
+    RCLCPP_INFO(node->get_logger(), "Waiting for robot description message");
+    rclcpp::sleep_for(std::chrono::milliseconds(500));
+  }
+
+  return robot_description;
 }
 
 void MujocoRos2Control::init()
 {
   clock_publisher_ = node_->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 10);
-  // Read urdf from ros parameter server then
+
+  std::string urdf_string = this->get_robot_description();
+
   // 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 +141,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;
@@ -122,7 +160,6 @@ void MujocoRos2Control::init()
   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());
-
   cm_executor_->add_node(controller_manager_);
 
   if (!controller_manager_->has_parameter("update_rate"))

mujoco_ros2_control/src/mujoco_ros2_control_node.cpp

@@ -62,8 +62,9 @@ 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(node, mujoco_model, mujoco_data);
+
+  mujoco_control.init();
   RCLCPP_INFO_STREAM(
     node->get_logger(), "Mujoco ros2 controller has been successfully initialized !");
 
@@ -90,7 +91,7 @@ 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();
 

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);