Unstable odrive

rove.repos

@@ -1,8 +1,12 @@
 repositories:
-  ovis_description:
+  ovis_ros2:
     type: git
-    url: https://github.com/clubcapra/ovis_ros2.git
-    version: master
+    url: https://github.com/clubcapra/ovis_ros2
+    version: new-env-with-arm
+  ros_odrive:
+    type: git
+    url: https://github.com/IliTheButterfly/ros_odrive.git
+    version: 0fa84d3745a8c385076d29b7f54971e9f2d14de7
   vectornav:
     type: git
     url: https://github.com/dawonn/vectornav.git

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: 1.0
     scale_angular_turbo:
       pitch: 0.0
       roll: 0.0
       yaw: 1.0
     scale_linear:
-      x: 0.5
+      x: 1.0
       y: 0.0
       z: 0.0
     scale_linear_turbo:
-      x: 1.0
+      x: 2.0
       y: 0.0
-      z: 0.0
+      z: 0.0
+    # publish_stamped_twist: true

src/rove_bringup/launch/common.launch.py

@@ -18,7 +18,6 @@ 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')
@@ -84,7 +83,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"),
         ),
     )
 
@@ -102,8 +101,8 @@ def generate_launch_description():
     return LaunchDescription([
             robot_state_publisher,
             robot_localization_node_local,
-            robot_localization_node_global,
-            navsat_transform,
+            #robot_localization_node_global,
+            #navsat_transform,
             rviz,
             teleop,
             autonomy,

src/rove_bringup/launch/real.launch.py

@@ -2,17 +2,37 @@
 
 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, ExecuteProcess
 
 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, OnShutdown
 
 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_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [urdf_path]
+            ),
+#            " ",
+#            "use_mock_hardware:=",
+#            use_mock_hardware,
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    # Controllers
+    controller_nodes = ["diff_drive_controller"]
 
     common = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -23,22 +43,75 @@ def generate_launch_description():
         }.items(),
     )
 
-
-    ###### Sensor ######
-    vectornav = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_rove_bringup, "launch", "vectornav.launch.py"),
-        ),
+    ###### ROS2 control ######
+    controllers = PathJoinSubstitution(
+        [
+            pkg_rove_description,
+            "config",
+            "tracks_controllers.yaml",
+        ]
     )
-
-    velodyne = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_rove_bringup, "launch", "velodyne.launch.py"),
-        ),
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[
+            robot_description,
+            controllers,
+        ],
+        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])
+
+    # start_can_cmd = ExecuteProcess(
+    #     cmd=[[
+    #         'ip link set can0 type can bitrate 250000; ip link set up can0'
+    #     ]],
+    #     shell=True
+    # )
+
+    # stop_can_cmd = ExecuteProcess(
+    #     cmd=[[
+    #         'ip link set down can0'
+    #     ]],
+    #     shell=True
+    # )
+
+    # shutdown = RegisterEventHandler(
+    #     event_handler=OnShutdown(
+    #         on_shutdown=[stop_can_cmd]
+    #     )
+    # )
+
     return LaunchDescription([
+            control_node,
             common,
-            vectornav,
-            velodyne,
+            joint_state_broadcaster_spawner,
+            *delayed_controller_nodes,
+            # vectornav,
+            # velodyne,
             ])

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,9 @@ 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')
             ],
         ),
         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,13 @@ 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);
+
+        // 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 +57,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 +88,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 +100,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 +122,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);
     }
 
@@ -127,10 +145,12 @@ class RoveController : public rclcpp::Node {
     }
 
     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_;
     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) {