Create HW package, fixes

UbiquityRobotics · Aug 29, 2024 · 3f8ad47 · 3f8ad47
1 parent 574e9b6
commit 3f8ad47
Show file tree

Hide file tree

Showing 7 changed files with 338 additions and 209 deletions.
diff --git a/include/ubiquity_motor_ros2/motor_hardware.h b/include/ubiquity_motor_ros2/motor_hardware.h
@@ -142,7 +142,10 @@ struct MotorDiagnostics {
 
 class MotorHardware : public hardware_interface::ActuatorInterface {
 public:
-    MotorHardware(const std::shared_ptr<rclcpp::Node>& n, NodeParams& node_params, CommsParams& serial_params, FirmwareParams& firmware_params);
+    // MotorHardware(const std::shared_ptr<rclcpp::Node>& n, NodeParams& node_params, CommsParams& serial_params, FirmwareParams& firmware_params);
+    MotorHardware();
+    void init(const std::shared_ptr<rclcpp::Node>& n, const std::shared_ptr<NodeParams>& node_params, const std::shared_ptr<CommsParams>& serial_params, const std::shared_ptr<FirmwareParams>& firmware_params);
+
     virtual ~MotorHardware();
 
     hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;
@@ -249,7 +252,7 @@ class MotorHardware : public hardware_interface::ActuatorInterface {
     int drive_type;
     int wheel_slip_nulling;
 
-    diagnostic_updater::Updater diag_updater;
+    std::unique_ptr<diagnostic_updater::Updater> diag_updater;
     // std::shared_ptr<controller_manager::ControllerManager> controller_manager = nullptr;
 
 private:
@@ -268,10 +271,10 @@ class MotorHardware : public hardware_interface::ActuatorInterface {
 
     rclcpp::Node::SharedPtr node;
 
-    NodeParams& node_params;
-    CommsParams& serial_params;
+    std::shared_ptr<NodeParams> node_params;
+    std::shared_ptr<CommsParams> serial_params;
+    std::shared_ptr<FirmwareParams> fw_params;
 
-    FirmwareParams& fw_params;
     FirmwareParams prev_fw_params;
 
     rclcpp::Logger logger;
@@ -285,7 +288,7 @@ class MotorHardware : public hardware_interface::ActuatorInterface {
     rclcpp::Time last_sys_maint_time;
     rclcpp::Time last_joint_time;
     uint32_t loopIdx;
-    rclcpp::Rate ctrlLoopDelay;
+    // rclcpp::Rate ctrlLoopDelay;
     rclcpp::Duration zeroVelocityTime;
     std::chrono::nanoseconds mcbStatusSleepPeriodNs;
 

diff --git a/include/ubiquity_motor_ros2/motor_node.h b/include/ubiquity_motor_ros2/motor_node.h
@@ -28,9 +28,13 @@ class MotorNode : public rclcpp::Node{
 
     std::unique_ptr<MotorHardware> robot;
 
-    FirmwareParams g_firmware_params;
-    CommsParams    g_serial_params;
-    NodeParams     g_node_params;
+    // FirmwareParams g_firmware_params;
+    // CommsParams    g_serial_params;
+    // NodeParams     g_node_params;
+
+    std::shared_ptr<NodeParams> node_params;
+    std::shared_ptr<CommsParams> serial_params;
+    std::shared_ptr<FirmwareParams> firmware_params;
 };
 
 #endif

diff --git a/include/ubiquity_motor_ros2/motor_parameters.h b/include/ubiquity_motor_ros2/motor_parameters.h
@@ -194,7 +194,7 @@ struct CommsParams {
 };
 
 struct NodeParams {
-    double controller_loop_rate;
+    int controller_loop_rate;
     std::string wheel_type;
     std::string wheel_direction;
     double wheel_gear_ratio;
@@ -203,15 +203,15 @@ struct NodeParams {
     int mcbControlEnabled;    // State to allow suspension of MCB serial control for diagnostic purposes
     int mcbSpeedEnabled;      // State to allow zero speed override for safety reasons
 
-    NodeParams() : controller_loop_rate(10.0),
+    NodeParams() : controller_loop_rate(10),
         wheel_type("firmware_default"), 
         wheel_direction("firmware_default"),
         wheel_gear_ratio(WHEEL_GEAR_RATIO_1),
         drive_type("firmware_default"),
         mcbControlEnabled(1),
         mcbSpeedEnabled(1){};
 
-    NodeParams(const std::shared_ptr<rclcpp::Node>& n) : controller_loop_rate(10.0),
+    NodeParams(const std::shared_ptr<rclcpp::Node>& n) : controller_loop_rate(10),
         wheel_type("firmware_default"),
         wheel_direction("firmware_default"),
 	wheel_gear_ratio(WHEEL_GEAR_RATIO_1),

diff --git a/package.xml b/package.xml
@@ -28,7 +28,12 @@
 
   <depend>ubiquity_motor_ros2_msgs</depend>
 
+  <build_depend>pluginlib</build_depend>
+  <exec_depend>pluginlib</exec_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
+    <pluginlib plugin="${prefix}/plugin_description.xml"/>
   </export>
+
 </package>
diff --git a/plugin_description.xml b/plugin_description.xml
@@ -0,0 +1,7 @@
+<library path="libubiquity_motor_ros2">
+  <class name="ubiquity_motor_ros2/MotorHardware" type="ubiquity_motor_ros2::MotorHardware">
+    <description>
+      This is the motor hardware plugin for the ubiquity_motor_ros2 package.
+    </description>
+  </class>
+</library>