feat: add ros2 topic name parameters for multiple node execution

mjlee111 · Nov 21, 2024 · 28fbfac · 28fbfac
1 parent f237793
commit 28fbfac
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 5 deletions.
diff --git a/dynamixel_rdk_ros/config/dynamixel.yaml b/dynamixel_rdk_ros/config/dynamixel.yaml
@@ -3,10 +3,10 @@
     ros__parameters:
         device_port: "/dev/ttyUSB0"
         baud_rate: 1000000
+        control_topic: "/dynamixel_control"
+        status_topic: "/dynamixel_status"
         dynamixels:
             ids: [1, 2, 3]
             types: ["MX", "MX", "MX"]
             max_position_limits: [3.14159, 3.14159, 3.14159]
-            min_position_limits: [-3.14159, -3.14159, -3.14159]
-
-
+            min_position_limits: [-3.14159, -3.14159, -3.14159]
diff --git a/dynamixel_rdk_ros/include/dynamixel_rdk_ros/dynamixel_rdk_node.hpp b/dynamixel_rdk_ros/include/dynamixel_rdk_ros/dynamixel_rdk_node.hpp
@@ -71,6 +71,9 @@ class DynamixelRDKNode : public rclcpp_lifecycle::LifecycleNode
 
   std::string device_port_;
   int baud_rate_;
+  std::string control_topic_;
+  std::string status_topic_;
+
   std::shared_ptr<DynamixelCtrl> dynamixel_ctrl_;
 
   std::vector<uint8_t> dynamixel_ids_;

diff --git a/dynamixel_rdk_ros/src/dynamixel_rdk_node.cpp b/dynamixel_rdk_ros/src/dynamixel_rdk_node.cpp
@@ -40,10 +40,10 @@ DynamixelRDKNode::on_configure(const rclcpp_lifecycle::State &)
   get_parameters();
 
   dynamixel_status_pub_ =
-    create_publisher<dynamixel_rdk_msgs::msg::DynamixelBulkReadMsgs>("dynamixel_status", 1);
+    create_publisher<dynamixel_rdk_msgs::msg::DynamixelBulkReadMsgs>(status_topic_.c_str(), 1);
 
   dynamixel_control_sub_ = create_subscription<dynamixel_rdk_msgs::msg::DynamixelControlMsgs>(
-    "dynamixel_control", 10,
+    control_topic_.c_str(), 10,
     std::bind(&DynamixelRDKNode::dynamixel_control_callback, this, std::placeholders::_1));
 
   try {
@@ -132,6 +132,8 @@ void DynamixelRDKNode::init_parameters()
   // ROS2 Parameters
   declare_parameter("device_port", rclcpp::ParameterValue("/dev/ttyUSB0"));
   declare_parameter("baud_rate", rclcpp::ParameterValue(1000000));
+  declare_parameter("control_topic", rclcpp::ParameterValue("/dynamixel_control"));
+  declare_parameter("status_topic", rclcpp::ParameterValue("/dynamixel_status"));
 
   declare_parameter("dynamixels.ids", std::vector<int64_t>{1});
   declare_parameter("dynamixels.types", std::vector<std::string>{"MX"});
@@ -143,6 +145,11 @@ void DynamixelRDKNode::get_parameters()
 {
   device_port_ = get_parameter("device_port").as_string();
   baud_rate_ = get_parameter("baud_rate").as_int();
+  control_topic_ = get_parameter("control_topic").as_string();
+  status_topic_ = get_parameter("status_topic").as_string();
+
+  RCLCPP_INFO(get_logger(), "Control topic: %s", control_topic_.c_str());
+  RCLCPP_INFO(get_logger(), "Status topic: %s", status_topic_.c_str());
 
   std::vector<int64_t> dynamixel_ids = get_parameter("dynamixels.ids").as_integer_array();
   for (auto & id : dynamixel_ids) {