Refactor motor node methods

UbiquityRobotics · Aug 27, 2024 · c64baa2 · c64baa2
1 parent bf7abdf
commit c64baa2
Show file tree

Hide file tree

Showing 7 changed files with 241 additions and 322 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -18,7 +18,11 @@ find_package(diagnostic_msgs REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(ros2_control REQUIRED)
 find_package(diagnostic_updater REQUIRED)
+find_package(controller_manager REQUIRED)
 find_package(ubiquity_motor_ros2_msgs REQUIRED)
+# find_package(Boost REQUIRED)
+find_package(Boost REQUIRED COMPONENTS thread)
+
 # find_package(zenohc REQUIRED)
 
 # add_compile_definitions(ZENOHCXX_ZENOHC) # Tells zenoh-cpp to use zenoh-c
@@ -132,28 +136,38 @@ ament_target_dependencies(${PROJECT_NAME}_lib
   # "${PROJECT_NAME}"  # This adds a dependency on the generated messages in the same package
 )
 
-# # Add executable
-# add_executable(motor_node src/motor_node.cc)
+# Add executable
+add_executable(motor_node 
+  src/motor_message.cc 
+  src/motor_serial.cc 
+  src/motor_hardware.cc
+  src/motor_node.cc)
 
-# # Include directories for executable
-# target_include_directories(motor_node
-#   PUBLIC
-#     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-#     $<INSTALL_INTERFACE:include>
-# )
+# Include directories for executable
+target_include_directories(motor_node
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+    ${CMAKE_CURRENT_SOURCE_DIR}/serial/include
+    ${controller_manager_INCLUDE_DIRS}
+)
 
-# ament_target_dependencies(motor_node
-#   "rclcpp"
-#   "std_msgs"
-#   "sensor_msgs"
-#   "hardware_interface"
-#   "ros2_control"
-#   "diagnostic_updater"
-# )
+ament_target_dependencies(motor_node
+  "rclcpp"
+  "std_msgs"
+  "sensor_msgs"
+  "hardware_interface"
+  "ros2_control"
+  "diagnostic_updater"
+  "controller_manager"
+  "ubiquity_motor_ros2_msgs"
+)
+
+target_link_libraries(motor_node Boost::thread ur_serial)
 
 # Install targets
 install(TARGETS
-  # motor_node
+  motor_node
   ${PROJECT_NAME}_lib
   DESTINATION lib/${PROJECT_NAME}
 )

diff --git a/CMakeLists_message.txt b/CMakeLists_message.txt
diff --git a/CMakeLists_old.txt b/CMakeLists_old.txt
diff --git a/include/ubiquity_motor_ros2/motor_node.h b/include/ubiquity_motor_ros2/motor_node.h
@@ -3,6 +3,13 @@
 #define MOTORNODE_H
 
 #include <rclcpp/rclcpp.hpp>
+// #include <ubiquity_motor/PIDConfig.h>
+#include <std_msgs/msg/string.hpp>
+#include <ubiquity_motor_ros2/motor_parameters.h>
+#include <ubiquity_motor_ros2/motor_hardware.h>
+#include <ubiquity_motor_ros2/motor_message.h>
+
+
 
 // TODO: Enhancement - Make WHEEL_SLIP_THRESHOLD be a ROS param
 #define WHEEL_SLIP_THRESHOLD  (0.08)   // Rotation below which we null excess wheel torque in 4wd drive_type
@@ -14,9 +21,10 @@ class MotorNode : public rclcpp::Node{
 public:
     MotorNode();
 
-    void PID_update_callback(const ubiquity_motor::PIDConfig& config, uint32_t level);
-    void SystemControlCallback(const std_msgs::String::ConstPtr& msg);
+    // void PID_update_callback(const ubiquity_motor::PIDConfig& config, uint32_t level);
+    void SystemControlCallback(const std_msgs::msg::String::SharedPtr msg);
     void initMcbParameters();
+    hardware_interface::HardwareInfo getHwInfo();
     void run();
 
     int g_wheel_slip_nulling;

diff --git a/package.xml b/package.xml
@@ -17,6 +17,7 @@
   <member_of_group>rosidl_interface_packages</member_of_group>
 
   <depend>hardware_interface</depend>
+  <depend>controller_manager</depend>
 
   <build_depend>std_msgs</build_depend>
   <exec_depend>std_msgs</exec_depend>

diff --git a/src/motor_hardware.cc b/src/motor_hardware.cc
@@ -164,8 +164,6 @@ MotorHardware::~MotorHardware() {
     closePort();
 }
 
-// Implement the necessary methods...
-
 hardware_interface::CallbackReturn MotorHardware::on_init(const hardware_interface::HardwareInfo &info) {
     // Initialize joint names and other variables
     if (info.joints.size() > sizeof(joints_) / sizeof(joints_[0])) {