Fixes, launch from launch file instead from motor_node

CMakeLists.txt

@@ -148,42 +148,42 @@ install(TARGETS ${PROJECT_NAME}
 )
 
 
-# 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>
-    ${CMAKE_CURRENT_SOURCE_DIR}/serial/include
-    ${controller_manager_INCLUDE_DIRS}
-)
+# # 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>
+#     ${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"
-  "controller_manager"
-  "ubiquity_motor_ros2_msgs"
-)
+# 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 ${PROJECT_NAME})
+# target_link_libraries(motor_node Boost::thread ur_serial ${PROJECT_NAME})
 
-# Export the pluginlib library
+# # Export the pluginlib library
 
 
-# Install targets
-install(TARGETS motor_node
-  DESTINATION lib/${PROJECT_NAME}
-)
+# # Install targets
+# install(TARGETS motor_node
+#   DESTINATION lib/${PROJECT_NAME}
+# )
 
 # Install scripts
 install(PROGRAMS
@@ -197,6 +197,11 @@ install(DIRECTORY launch/
   DESTINATION share/${PROJECT_NAME}/launch
 )
 
+install(DIRECTORY cfg/
+  DESTINATION share/${PROJECT_NAME}/cfg
+)
+
+
 # # Test dependencies
 # if(BUILD_TESTING)
 #   find_package(ament_lint_auto REQUIRED)

README.md

@@ -1,4 +1,12 @@
 # Ubiquity motor ROS 2
 
 ## Build the motor node and its msgs
-colcon build --base-paths src/ubiquity_motor_ros2 src/ubiquity_motor_ros2/ubiquity_motor_ros2_msgs --cmake-args --event-handlers console_direct+
+colcon build --base-paths src/ubiquity_motor_ros2 src/ubiquity_motor_ros2/ubiquity_motor_ros2_msgs --cmake-args --event-handlers console_direct+
+
+## Run motor node
+
+ros2 launch ubiquity_motor_ros2 motors.launch.py
+
+## Teleop 
+
+ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true --remap cmd_vel:=/ubiquity_velocity_controller/cmd_vel

cfg/conf.yaml

@@ -0,0 +1,55 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 20
+
+    ubiquity_velocity_controller:
+    # ros__parameters:
+      type: "diff_drive_controller/DiffDriveController"
+
+
+ubiquity_velocity_controller:
+  ros__parameters:
+    # 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
+
+    # 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
+    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
+    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
+        has_acceleration_limits: true
+        max_acceleration: 1.1
+
+    angular:
+      z:
+        has_velocity_limits: true
+        max_velocity: 2.0
+        has_acceleration_limits: true
+        max_acceleration: 5.0
+
+

include/ubiquity_motor_ros2/motor_hardware.h

@@ -65,7 +65,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 // Until we have a holdoff for MCB message overruns we do this delay to be cautious
 // Twice the period for status reports from MCB
 // auto mcbStatusSleepPeriodNs = rclcpp::Duration::from_seconds(0.02).to_chrono<std::chrono::nanoseconds>();
-
+namespace ubiquity_motor_ros2
+{
 struct MotorDiagnostics {
     MotorDiagnostics()
         : odom_update_status(
@@ -161,6 +162,8 @@ class MotorHardware : public hardware_interface::SystemInterface {
 
     hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
 
+    hardware_interface::CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override;
+
 
     // hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override
     // {
@@ -358,4 +361,5 @@ class MotorHardware : public hardware_interface::SystemInterface {
     FRIEND_TEST(MotorHardwareTests, odomUpdatesPositionMax);
 };
 
+}
 #endif

include/ubiquity_motor_ros2/motor_node.h

@@ -10,6 +10,7 @@
 #include <ubiquity_motor_ros2/motor_message.h>
 // #include <mutex>
 
+using namespace ubiquity_motor_ros2;
 
 static const double BILLION = 1000000000.0;
 

include/ubiquity_motor_ros2/motor_parameters.h

@@ -86,19 +86,19 @@ struct FirmwareParams {
     float battery_voltage_critical;
 
     FirmwareParams()
-        : pid_proportional(4000),
+        : pid_proportional(5000),
           pid_integral(5),
-          pid_derivative(-200),
+          pid_derivative(-110),
           pid_velocity(0),
           pid_denominator(1000),
-          pid_moving_buffer_size(10),
-	  pid_control(0),
+          pid_moving_buffer_size(70),
+	      pid_control(0),
           controller_board_version(51),
           estop_detection(1),
           estop_pid_threshold(1500),
-          max_speed_fwd(80),
-          max_speed_rev(-80),
-          max_pwm(250),
+          max_speed_fwd(104),
+          max_speed_rev(-104),
+          max_pwm(325),
           deadman_timer(2400000),
           deadzone_enable(0),
           hw_options(0),

launch/motors.launch.py

@@ -0,0 +1,71 @@
+import os
+import launch
+import launch_ros.actions
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, LogInfo
+from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+    # Define the path to the xacro file
+    xacro_file = PathJoinSubstitution(
+        [FindPackageShare('magni_description'), 'urdf', 'magni.urdf.xacro']
+    )
+
+    # Define the command to run xacro as a subprocess
+    xacro_command = Command(
+        [
+            'xacro ', xacro_file,
+        ]
+    )
+
+    # Use ExecuteProcess to run the xacro command
+    run_xacro = ExecuteProcess(
+        cmd=['xacro', xacro_file],
+        output='screen',
+        shell=True
+    )
+
+    # Use the robot_description generated from the xacro command
+    robot_state_publisher = launch_ros.actions.Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[{'robot_description': xacro_command}]
+    )
+
+    # ros2_control_node
+    # Step 1: Run the stty command to configure the serial port
+    serial_config = ExecuteProcess(
+        cmd=['sudo', 'stty', '-F', '/dev/ttyS0', 'sane'],
+        shell=True
+    )
+
+        # Path to the test.yaml configuration file
+    config_file = PathJoinSubstitution(
+        [FindPackageShare('ubiquity_motor_ros2'), 'cfg', 'conf.yaml']
+    )
+
+    # Step 2: Run the ros2_control_node with parameters
+    controller_node = launch_ros.actions.Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        output='screen',
+        parameters=[config_file],
+    )
+
+    # Spawning the controller using spawner command
+    spawn_controller = ExecuteProcess(
+        cmd=[
+            'ros2', 'run', 'controller_manager', 'spawner', 'ubiquity_velocity_controller'
+        ],
+        output='screen'
+    )
+
+    # Return the launch description without ros2_control_node
+    return launch.LaunchDescription([
+        run_xacro,  # Runs the xacro process
+        robot_state_publisher,  # Starts the robot_state_publisher with xacro output
+        serial_config,
+        controller_node,
+        spawn_controller
+    ])

plugin_description.xml

@@ -1,5 +1,5 @@
 <library path="ubiquity_motor_ros2">
-  <class name="ubiquity_motor_ros2/MotorHardware" type="MotorHardware" base_class_type="hardware_interface::SystemInterface">
+  <class name="ubiquity_motor_ros2/MotorHardware" type="ubiquity_motor_ros2::MotorHardware" base_class_type="hardware_interface::SystemInterface">
     <description>
       This is the motor hardware plugin for the ubiquity_motor_ros2 package.
     </description>