From bfbfa4f17c7c8b2c249b6e9a2db083d5216bc2da Mon Sep 17 00:00:00 2001 From: wgtayar Date: Wed, 17 Sep 2025 17:00:29 +0300 Subject: [PATCH 1/3] Odrive CAN node and Status msg patch to include timestamps and node IDs. --- odrive_node/msg/ODriveStatus.msg | 2 ++ odrive_node/src/odrive_can_node.cpp | 3 +++ 2 files changed, 5 insertions(+) diff --git a/odrive_node/msg/ODriveStatus.msg b/odrive_node/msg/ODriveStatus.msg index 3800e59..e38ee4a 100644 --- a/odrive_node/msg/ODriveStatus.msg +++ b/odrive_node/msg/ODriveStatus.msg @@ -1,3 +1,5 @@ +std_msgs/Header header +uint32 node_id float32 bus_voltage float32 bus_current float32 fet_temperature diff --git a/odrive_node/src/odrive_can_node.cpp b/odrive_node/src/odrive_can_node.cpp index d53fbec..872f508 100644 --- a/odrive_node/src/odrive_can_node.cpp +++ b/odrive_node/src/odrive_can_node.cpp @@ -70,6 +70,8 @@ bool ODriveCanNode::init(EpollEventLoop* event_loop) { axis_idle_on_shutdown_ = rclcpp::Node::get_parameter("axis_idle_on_shutdown").as_bool(); std::string interface = rclcpp::Node::get_parameter("interface").as_string(); + odrv_stat_.node_id = node_id_; + if (!can_intf_.init(interface, event_loop, std::bind(&ODriveCanNode::recv_callback, this, _1))) { RCLCPP_ERROR(rclcpp::Node::get_logger(), "Failed to initialize socket can interface: %s", interface.c_str()); return false; @@ -142,6 +144,7 @@ void ODriveCanNode::recv_callback(const can_frame& frame) { case CmdId::kGetBusVoltageCurrent: { if (!verify_length("kGetBusVoltageCurrent", 8, frame.can_dlc)) break; std::lock_guard guard(odrv_stat_mutex_); + odrv_stat_.header.stamp = rclcpp::Clock().now(); odrv_stat_.bus_voltage = read_le(frame.data + 0); odrv_stat_.bus_current = read_le(frame.data + 4); odrv_pub_flag_ |= 0b100; From ec2a62d6ee8efe02d018ceccc977505980ac3317 Mon Sep 17 00:00:00 2001 From: wgtayar Date: Wed, 17 Sep 2025 17:01:46 +0300 Subject: [PATCH 2/3] CMakeLists and package.xml for correct inclusion of std_msgs --- odrive_node/CMakeLists.txt | 2 ++ odrive_node/package.xml | 1 + 2 files changed, 3 insertions(+) diff --git a/odrive_node/CMakeLists.txt b/odrive_node/CMakeLists.txt index 3bfda36..14cf775 100644 --- a/odrive_node/CMakeLists.txt +++ b/odrive_node/CMakeLists.txt @@ -10,12 +10,14 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(std_srvs REQUIRED) +find_package(std_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/ControlMessage.msg" "msg/ControllerStatus.msg" "msg/ODriveStatus.msg" "srv/AxisState.srv" + DEPENDENCIES std_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/odrive_node/package.xml b/odrive_node/package.xml index 0db7e05..223d5b5 100644 --- a/odrive_node/package.xml +++ b/odrive_node/package.xml @@ -14,6 +14,7 @@ rclcpp std_srvs + std_msgs ament_lint_auto ament_lint_common From 60576da5fd9193a7b55456680c48b30e9f16c518 Mon Sep 17 00:00:00 2001 From: wgtayar Date: Wed, 17 Sep 2025 17:16:09 +0300 Subject: [PATCH 3/3] Launch file for multiple Odrive nodes. --- .../multiple_odrives_example_launch.yaml | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100644 odrive_node/launch/multiple_odrives_example_launch.yaml diff --git a/odrive_node/launch/multiple_odrives_example_launch.yaml b/odrive_node/launch/multiple_odrives_example_launch.yaml new file mode 100644 index 0000000..2ae9766 --- /dev/null +++ b/odrive_node/launch/multiple_odrives_example_launch.yaml @@ -0,0 +1,38 @@ +launch: + +- node: + pkg: "odrive_can" + exec: "odrive_can_node" + name: "can_node6" + namespace: "odrive_axis0" + param: + - + name: "node_id" + value: 6 + - + name: "interface" + value: "can0" +- node: + pkg: "odrive_can" + exec: "odrive_can_node" + name: "can_node7" + namespace: "odrive_axis0" + param: + - + name: "node_id" + value: 7 + - + name: "interface" + value: "can0" +- node: + pkg: "odrive_can" + exec: "odrive_can_node" + name: "can_node8" + namespace: "odrive_axis0" + param: + - + name: "node_id" + value: 8 + - + name: "interface" + value: "can0" \ No newline at end of file