From fe59c52565a18aca9bc7d24bf7af319bac327d6f Mon Sep 17 00:00:00 2001
From: mbostic <bosticmatjaz@gmail.com>
Date: Fri, 6 Sep 2024 14:06:10 +0200
Subject: [PATCH] fixes

---
 CMakeLists.txt                                |  4 ++
 cfg/test.yaml                                 | 48 +++++++++++++++++++
 .../ubiquity_motor_ros2/motor_parameters.h    |  4 +-
 launch/test.launch.py                         | 13 +++++
 src/motor_node.cc                             |  5 +-
 5 files changed, 71 insertions(+), 3 deletions(-)
 create mode 100644 cfg/test.yaml
 create mode 100644 launch/test.launch.py

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6c3f638..789e2f6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -193,6 +193,10 @@ install(PROGRAMS
   DESTINATION lib/${PROJECT_NAME}
 )
 
+install(DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch
+)
+
 # # Test dependencies
 # if(BUILD_TESTING)
 #   find_package(ament_lint_auto REQUIRED)
diff --git a/cfg/test.yaml b/cfg/test.yaml
new file mode 100644
index 0000000..4b3de02
--- /dev/null
+++ b/cfg/test.yaml
@@ -0,0 +1,48 @@
+# This is the configuration for the controller_manager in ROS 2
+controller_manager:
+  ros__parameters:
+    ubiquity_velocity_controller:
+      type: "diff_drive_controller/DiffDriveController"
+      
+      # Specify the joints connected to the wheels
+      left_wheel_names: ['left_wheel_joint']
+      right_wheel_names: ['right_wheel_joint']
+      
+      # Publish rate for odometry messages
+      publish_rate: 50.0  # Same as in ROS 1, rate of odom publishing
+
+      # Covariance for pose and twist (six elements representing the covariance matrix diagonal)
+      pose_covariance_diagonal: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2]
+      twist_covariance_diagonal: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2]
+
+      # Velocity command timeout (stop if no command received after this time)
+      cmd_vel_timeout: 0.25
+
+      # Enable publishing of the transform from odom to base_frame_id
+      enable_odom_tf: true
+
+      # Robot physical parameters
+      wheel_separation: 0.33
+      wheel_radius: 0.1015
+
+      # Base frame ID for the robot
+      base_frame_id: base_footprint
+
+      # Multipliers to adjust wheel separation and radius (usually left at 1.0)
+      wheel_separation_multiplier: 1.0
+      wheel_radius_multiplier: 1.0
+
+      # Velocity and acceleration limits for linear and angular motion
+      linear:
+        x:
+          has_velocity_limits: true
+          max_velocity: 1.0  # Maximum forward speed (m/s)
+          has_acceleration_limits: true
+          max_acceleration: 1.1  # Maximum acceleration (m/s^2)
+
+      angular:
+        z:
+          has_velocity_limits: true
+          max_velocity: 2.0  # Maximum rotational speed (rad/s)
+          has_acceleration_limits: true
+          max_acceleration: 5.0  # Maximum angular acceleration (rad/s^2)
diff --git a/include/ubiquity_motor_ros2/motor_parameters.h b/include/ubiquity_motor_ros2/motor_parameters.h
index 3063653..e7f10df 100644
--- a/include/ubiquity_motor_ros2/motor_parameters.h
+++ b/include/ubiquity_motor_ros2/motor_parameters.h
@@ -184,10 +184,10 @@ struct CommsParams {
     int32_t baud_rate;
 
     CommsParams()
-        : serial_port("/dev/ttyS0"), baud_rate(9600) {};
+        : serial_port("/dev/ttyS0"), baud_rate(38400) {};
 
     CommsParams(const std::shared_ptr<rclcpp::Node>& n)
-        : serial_port("/dev/ttyS0"), baud_rate(9600) {
+        : serial_port("/dev/ttyS0"), baud_rate(38400) {
         // clang-format off
         serial_port = getParamOrDefault(
             n, "ubiquity_motor_ros2/serial_port", serial_port);
diff --git a/launch/test.launch.py b/launch/test.launch.py
new file mode 100644
index 0000000..0c9e2e7
--- /dev/null
+++ b/launch/test.launch.py
@@ -0,0 +1,13 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='controller_manager',
+            executable='spawner',  # Correct executable for spawning controllers
+            arguments=['diff_drive_controller'],  # Controller name
+            parameters=['/home/ubuntu/ros2_ws/src/ubiquity_motor_ros2/cfg/test.yaml'],  # Path to your YAML config file
+            output='screen',
+        ),
+    ])
diff --git a/src/motor_node.cc b/src/motor_node.cc
index d622537..11c3f81 100644
--- a/src/motor_node.cc
+++ b/src/motor_node.cc
@@ -178,7 +178,8 @@ void MotorNode::run() {
         "--ros-args",
         // "--remap", "controller_manager:__node:=motor_hardware_node",
         "--log-level", "info", 
-        "--param", "update_rate:=" + std::to_string(controller_loop_rate)
+        "--param", "update_rate:=" + std::to_string(controller_loop_rate),
+        "--params-file", "/home/ubuntu/ros2_ws/src/ubiquity_motor_ros2/cfg/test.yaml"
         });
 
 
@@ -190,6 +191,8 @@ void MotorNode::run() {
     // Create the ResourceManager and register the actuator interface
     auto resource_manager = std::make_unique<hardware_interface::ResourceManager>(buffer.str(), get_node_clock_interface(), get_node_logging_interface(), true, controller_loop_rate);
     resource_manager->import_component(std::move(robot), getHwInfo());
+    // resource_manager->load_hardware_component(std::move(robot), getHwInfo());
+
 
     controller_manager::ControllerManager cm(std::move(resource_manager), executor, "controller_manager", get_namespace(), options);