Joy or cmd_vel are lagging

src/rove_bringup/CMakeLists.txt

@@ -6,12 +6,13 @@ project(rove_bringup)
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(Qt5 COMPONENTS Widgets REQUIRED)
 
 add_executable(rove_controller_node src/rove_controller_node.cpp)
 
 # Link dependencies
-ament_target_dependencies(rove_controller_node rclcpp sensor_msgs)
+ament_target_dependencies(rove_controller_node rclcpp sensor_msgs geometry_msgs)
 
 # Install executable
 install(TARGETS

src/rove_bringup/config/teleop_joy_params.yaml

@@ -14,16 +14,17 @@
     scale_angular:
       pitch: 0.0
       roll: 0.0
-      yaw: 0.5
+      yaw: 4.0
     scale_angular_turbo:
       pitch: 0.0
       roll: 0.0
       yaw: 1.0
     scale_linear:
-      x: 0.5
+      x: 15.0
       y: 0.0
       z: 0.0
     scale_linear_turbo:
       x: 1.0
       y: 0.0
-      z: 0.0
+      z: 0.0
+    publish_stamped_twist: true

src/rove_bringup/launch/common.launch.py

@@ -15,24 +15,12 @@ def generate_launch_description():
     pkg_rove_bringup = get_package_share_directory('rove_bringup')
     pkg_rove_description = get_package_share_directory('rove_description')
     pkg_rove_slam = get_package_share_directory('rove_slam')
-    bringup_pkg_path = get_package_share_directory('rove_bringup')
 
     # Get the URDF file
     urdf_path = os.path.join(pkg_rove_description, 'urdf', 'rove.urdf.xacro')
     robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str)
 
-    # Takes the description and joint angles as inputs and publishes
-    # the 3D poses of the robot links
-    robot_state_publisher = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        output='both',
-        parameters=[
-            {'robot_description': robot_desc},
-            {"use_sim_time": True, }
-        ]
-    )
+
 
     # Visualize in RViz
     rviz = Node(
@@ -83,7 +71,7 @@ def generate_launch_description():
 
     teleop = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(bringup_pkg_path, "launch", "rove_controller_usb.launch.py"),
+            os.path.join(pkg_rove_bringup, "launch", "rove_controller_usb.launch.py"),
         ),
     )
 
@@ -99,11 +87,11 @@ def generate_launch_description():
     )
 
     return LaunchDescription([
-            robot_state_publisher,
-            robot_localization_node_local,
-            robot_localization_node_global,
-            navsat_transform,
+            # robot_state_publisher,
+            # robot_localization_node_local,
+            # robot_localization_node_global,
+            # navsat_transform,
             rviz,
             teleop,
-            autonomy,
+            # autonomy,
             ])

src/rove_bringup/launch/real.launch.py

@@ -2,33 +2,130 @@
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
+from launch.actions import IncludeLaunchDescription, RegisterEventHandler
 
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import Command
-from launch_ros.actions import Node
+from launch.substitutions import Command, PathJoinSubstitution, FindExecutable
+from launch_ros.actions import Node, SetRemap
 from launch_ros.parameter_descriptions import ParameterValue
-
+from launch.event_handlers import OnProcessExit
 
 def generate_launch_description():
     # Get the launch directory
     pkg_rove_bringup = get_package_share_directory('rove_bringup')
+    pkg_rove_description = get_package_share_directory('rove_description')
+
+    # Get the URDF file
+    urdf_path = os.path.join(pkg_rove_description, 'urdf', 'rove.urdf.xacro')
+    robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str)
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [urdf_path]
+            ),
+#            " ",
+#            "use_mock_hardware:=",
+#            use_mock_hardware,
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    # Takes the description and joint angles as inputs and publishes
+    # the 3D poses of the robot links
+    robot_state_pub_node = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        # name='robot_state_publisher',
+        output='both',
+        parameters=[
+            {'robot_description': robot_desc},
+            {"use_sim_time": True, }
+        ],
+        remappings=[
+            # ("/diff_drive_controller/cmd_vel_unstamped", "/cmd_vel"),
+            # ("/diff_drive_controller/cmd_vel", "/cmd_vel"),
+            # ("/cmd_vel", "/diff_drive_controller/cmd_vel_unstamped"),
+            # ("/cmd_vel", "/diff_drive_controller/cmd_vel"),
+        ],
+    )
+
+    # Controllers
+    # controller_nodes = ["left_track_controller", "right_track_controller", "diff_drive_controller"]
+    controller_nodes = ["diff_drive_controller"]
+    # controller_nodes = ["velocity_controller"]
+    # controller_nodes = ["forward_controller"]
 
     common = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(pkg_rove_bringup, "launch", "common.launch.py"),
         ),
     )
 
+    ###### ROS2 control ######
+    controllers = PathJoinSubstitution(
+        [
+            pkg_rove_description,
+            "config",
+            "tracks_controllers.yaml",
+            # "test_controllers.yaml",
+        ]
+    )
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[
+            # robot_desc,
+            robot_description,
+            controllers,
+        ],
+        # remappings=[
+        #     ('/cmd_vel', '/diff_drive_controller/cmd_vel_unstamped'),
+        # ],
+        output="both",
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+
+
+    def create_controller_node(node_name:str):
+        robot_controller_spawner = Node(
+            package="controller_manager",
+            executable="spawner",
+            arguments=[node_name, "--controller-manager", "/controller_manager"],
+        )
+
+        # Delay start of robot_controller after `joint_state_broadcaster`
+        delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=joint_state_broadcaster_spawner,
+                on_exit=[robot_controller_spawner],
+            )
+        )
+        return delay_robot_controller_spawner_after_joint_state_broadcaster_spawner
+
+    delayed_controller_nodes = list([create_controller_node(node_name) for node_name in controller_nodes])
+
 
     ###### Sensor ######
     vectornav = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(pkg_rove_bringup, "launch", "vectornav.launch.py"),
         ),
     )
+
 
     return LaunchDescription([
+            robot_state_pub_node,
+            control_node,
             common,
-            vectornav,
+            joint_state_broadcaster_spawner,
+            *delayed_controller_nodes,
+            # vectornav,
             ])

src/rove_bringup/launch/rove_controller_bluetooth.launch.py

@@ -7,7 +7,7 @@ def generate_launch_description():
     dir = get_package_share_directory(package_name)
     # Get params files
     joy_params_file = dir + '/config/joy_params.yaml'
-    teleope_joy_params_file = dir + '/config/teleop_joy_params.yaml'
+    teleop_joy_params_file = dir + '/config/teleop_joy_params.yaml'
     bluetooth_mapping_file = dir + '/config/bluetooth_mapping.yaml'
 
     return LaunchDescription([
@@ -22,7 +22,7 @@ def generate_launch_description():
             package='teleop_twist_joy',
             executable='teleop_node',
             name='teleop_twist_joy_node',
-            parameters=[teleope_joy_params_file],
+            parameters=[teleop_joy_params_file],
             remappings=[
                 ('/joy', '/joy'),
                 # ('/cmd_vel', '/model/rove/cmd_vel')

src/rove_bringup/launch/rove_controller_usb.launch.py

@@ -7,7 +7,7 @@ def generate_launch_description():
     dir = get_package_share_directory(package_name)
     # Get params files
     joy_params_file = dir + '/config/joy_params.yaml'
-    teleope_joy_params_file = dir + '/config/teleop_joy_params.yaml'
+    teleop_joy_params_file = dir + '/config/teleop_joy_params.yaml'
     usb_mapping_file = dir + '/config/usb_mapping.yaml'
 
     return LaunchDescription([
@@ -22,10 +22,10 @@ def generate_launch_description():
             package='teleop_twist_joy',
             executable='teleop_node',
             name='teleop_twist_joy_node',
-            parameters=[teleope_joy_params_file],
+            parameters=[teleop_joy_params_file],
             remappings=[
                 ('/joy', '/joy'),
-                # ('/cmd_vel', '/model/rove/cmd_vel')
+                ('/cmd_vel', '/diff_drive_controller/cmd_vel_unstamped')
             ],
         ),
         Node(

src/rove_bringup/package.xml

@@ -9,6 +9,8 @@
 
   <build_depend>joy</build_depend>
   <build_depend>teleop_twist_joy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
 
   <exec_depend>joy</exec_depend>
   <exec_depend>teleop_twist_joy</exec_depend>

src/rove_bringup/src/rove_controller_node.cpp

@@ -1,6 +1,9 @@
 // rove_controller.cpp
 #include "rclcpp/rclcpp.hpp"
 #include "sensor_msgs/msg/joy.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "std_msgs/msg/float64_multi_array.hpp"
 
 enum ControllerMode{
     FLIPPER_MODE = 0,
@@ -39,8 +42,14 @@ class RoveController : public rclcpp::Node {
         joy_sub_ = create_subscription<sensor_msgs::msg::Joy>(
             "/joy", 10, std::bind(&RoveController::joyCallback, this, std::placeholders::_1)
         );
+        // cmd_vel_sub_ = create_subscription<geometry_msgs::msg::Twist>("/cmd_vel", 1, std::bind(&RoveController::cmdvelCallback, this, std::placeholders::_1));
 
-        joy_pub_ = create_publisher<sensor_msgs::msg::Joy>("/rove/joy", 10);
+        joy_pub_ = create_publisher<sensor_msgs::msg::Joy>("/rove/joy", 1);
+        // cmd_vel_stamped_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("/diff_drive_controller/cmd_vel", 1);
+
+        // auto x = create_wall_timer(
+        //     std::chrono::milliseconds(1000/20), std::bind(&RoveController::cmdvelTimerCallback, this, std::placeholders::_1)
+        // );
 
         previous_msg_ = sensor_msgs::msg::Joy();
         previous_msg_.axes.resize(6, 0.0);
@@ -49,6 +58,9 @@ class RoveController : public rclcpp::Node {
         teleop_msg_ = sensor_msgs::msg::Joy();
         teleop_msg_.axes.resize(2,0);
         teleop_msg_.buttons.resize(1,0);
+
+        // cmd_vel_msg_ = geometry_msgs::msg::Twist();
+        // cmd_vel_stamped_msg_ = geometry_msgs::msg::TwistStamped();
     }
 
 private:
@@ -77,6 +89,7 @@ class RoveController : public rclcpp::Node {
     }
 
     void arm_action(const sensor_msgs::msg::Joy::SharedPtr joy_msg) {
+        log("Arm");
         if(button_pressed(joy_msg, B)){
             log("Arm B pressed");
         }
@@ -88,6 +101,7 @@ class RoveController : public rclcpp::Node {
         if(buttton_down(joy_msg, X)){
             log("Arm X down");
         }
+
     }
 
     void flipper_action(const sensor_msgs::msg::Joy::SharedPtr joy_msg) {
@@ -109,6 +123,11 @@ class RoveController : public rclcpp::Node {
     }
 
     void log(const char *text){
+        static rclcpp::Clock clk{};
+        static const char* last = nullptr;
+        if (last == text) return;
+        last = text;
+        // RCLCPP_INFO_THROTTLE(get_logger(), clk, 2000, text);
         RCLCPP_INFO(get_logger(), text);
     }
 
@@ -126,11 +145,26 @@ class RoveController : public rclcpp::Node {
         previous_msg_ = *joy_msg;
     }
 
+    // void cmdvelCallback(const geometry_msgs::msg::Twist::SharedPtr cmd_vel_msg) {
+    //     cmd_vel_msg_ = *cmd_vel_msg;
+    //     cmd_vel_stamped_msg_.twist = cmd_vel_msg_;
+    //     auto header = std_msgs::msg::Header();
+    //     header.stamp = this->get_clock()->now();
+    //     cmd_vel_stamped_msg_.header = header;
+    // }
+
+    // void cmdvelTimerCallback() {
+    //     cmd_vel_stamped_pub_->publish(cmd_vel_stamped_msg_);
+    // }
+
     rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
+    // rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
     rclcpp::Publisher<sensor_msgs::msg::Joy>::SharedPtr joy_pub_;
+    // rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_vel_stamped_pub_;
     sensor_msgs::msg::Joy previous_msg_; 
     sensor_msgs::msg::Joy teleop_msg_;
-
+    // geometry_msgs::msg::Twist cmd_vel_msg_;
+    // geometry_msgs::msg::TwistStamped cmd_vel_stamped_msg_;
 };
 
 int main(int argc, char **argv) {

src/rove_description/config/test_controllers.yaml

@@ -0,0 +1,14 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 10  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    velocity_controller:
+      type: velocity_controllers/JointGroupVelocityController
+
+velocity_controller:
+  ros__parameters:
+    joints:
+      - track_fl_j