diff --git a/rove.repos b/rove.repos
index 900658a..658a1cd 100644
--- a/rove.repos
+++ b/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
diff --git a/src/rove_bringup/CMakeLists.txt b/src/rove_bringup/CMakeLists.txt
index fcf4c58..8e81165 100644
--- a/src/rove_bringup/CMakeLists.txt
+++ b/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
diff --git a/src/rove_bringup/config/teleop_joy_params.yaml b/src/rove_bringup/config/teleop_joy_params.yaml
index 2b37113..4195ba7 100644
--- a/src/rove_bringup/config/teleop_joy_params.yaml
+++ b/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
\ No newline at end of file
+ z: 0.0
+ # publish_stamped_twist: true
\ No newline at end of file
diff --git a/src/rove_bringup/launch/common.launch.py b/src/rove_bringup/launch/common.launch.py
index 00a1b5d..87781ff 100644
--- a/src/rove_bringup/launch/common.launch.py
+++ b/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,
diff --git a/src/rove_bringup/launch/real.launch.py b/src/rove_bringup/launch/real.launch.py
index a9d6bf0..4205eff 100644
--- a/src/rove_bringup/launch/real.launch.py
+++ b/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,
])
diff --git a/src/rove_bringup/launch/rove_controller_bluetooth.launch.py b/src/rove_bringup/launch/rove_controller_bluetooth.launch.py
index ddf043c..43dc67e 100644
--- a/src/rove_bringup/launch/rove_controller_bluetooth.launch.py
+++ b/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')
diff --git a/src/rove_bringup/launch/rove_controller_usb.launch.py b/src/rove_bringup/launch/rove_controller_usb.launch.py
index 846e720..61153ce 100644
--- a/src/rove_bringup/launch/rove_controller_usb.launch.py
+++ b/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(
diff --git a/src/rove_bringup/package.xml b/src/rove_bringup/package.xml
index 140cc49..808c303 100644
--- a/src/rove_bringup/package.xml
+++ b/src/rove_bringup/package.xml
@@ -9,6 +9,8 @@
joy
teleop_twist_joy
+ std_msgs
+ geometry_msgs
joy
teleop_twist_joy
diff --git a/src/rove_bringup/src/rove_controller_node.cpp b/src/rove_bringup/src/rove_controller_node.cpp
index 8baedae..3086ea0 100644
--- a/src/rove_bringup/src/rove_controller_node.cpp
+++ b/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(
"/joy", 10, std::bind(&RoveController::joyCallback, this, std::placeholders::_1)
);
+ // cmd_vel_sub_ = create_subscription("/cmd_vel", 1, std::bind(&RoveController::cmdvelCallback, this, std::placeholders::_1));
- joy_pub_ = create_publisher("/rove/joy", 10);
+ joy_pub_ = create_publisher("/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::SharedPtr joy_sub_;
+ // rclcpp::Subscription::SharedPtr cmd_vel_sub_;
rclcpp::Publisher::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) {
diff --git a/src/rove_description/config/tracks_controllers.yaml b/src/rove_description/config/tracks_controllers.yaml
new file mode 100644
index 0000000..8fced65
--- /dev/null
+++ b/src/rove_description/config/tracks_controllers.yaml
@@ -0,0 +1,73 @@
+# This config file is used by ros2_control
+controller_manager:
+ ros__parameters:
+ update_rate: 20 # Hz
+
+ joint_state_broadcaster:
+ type: joint_state_broadcaster/JointStateBroadcaster
+
+ # left_track_controller:
+ # type: rove_tracks/ChainedController
+
+ # right_track_controller:
+ # type: rove_tracks/ChainedController
+
+ diff_drive_controller:
+ type: diff_drive_controller/DiffDriveController
+ # forward_controller:
+ # type: forward_command_controller/ForwardCommandController
+
+# forward_controller:
+# ros__parameters:
+# joints:
+# - track_fl_j
+# interface_name: velocity
+
+diff_drive_controller:
+ ros__parameters:
+ # left_wheel_names: ["track_l_j"]
+ right_wheel_names: ["track_rr_j", "track_fr_j"]
+ left_wheel_names: ["track_fl_j", "track_rl_j"]
+ # right_wheel_names: ["track_fr_j"]
+
+ wheel_separation: 0.230
+ wheel_radius: 0.11
+
+ wheel_separation_multiplier: 10.0
+ left_wheel_radius_multiplier: -0.0325
+ right_wheel_radius_multiplier: 0.0325
+
+ publish_rate: 20.0
+ odom_frame_id: odom
+ # allow_multiple_cmd_vel_publishers: true
+ base_frame_id: base_link
+ pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+ twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+
+ open_loop: true
+ enable_odom_tf: true
+
+ cmd_vel_timeout: 0.5
+ # publish_limited_velocity: true
+ # publish_cmd: true
+ # velocity_rolling_window_size: 10
+ use_stamped_vel: false
+
+ linear.x.has_velocity_limits: true
+ linear.x.has_acceleration_limits: true
+ linear.x.has_jerk_limits: false
+ linear.x.max_velocity: 5.0
+ linear.x.min_velocity: -5.0
+ linear.x.max_acceleration: 2.0
+ linear.x.max_jerk: 0.0
+ linear.x.min_jerk: 0.0
+
+ angular.z.has_velocity_limits: true
+ angular.z.has_acceleration_limits: true
+ angular.z.has_jerk_limits: false
+ angular.z.max_velocity: 1.0
+ angular.z.min_velocity: -1.0
+ angular.z.max_acceleration: 1.0
+ angular.z.min_acceleration: -1.0
+ angular.z.max_jerk: 0.0
+ angular.z.min_jerk: 0.0
\ No newline at end of file
diff --git a/src/rove_description/urdf/rove.ros2_control.urdf.xacro b/src/rove_description/urdf/rove.ros2_control.urdf.xacro
new file mode 100644
index 0000000..45461da
--- /dev/null
+++ b/src/rove_description/urdf/rove.ros2_control.urdf.xacro
@@ -0,0 +1,108 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ odrive_ros2_control_plugin/ODriveHardwareInterface
+ can0
+
+
+ 23
+
+
+
+
+
+
+ 24
+
+
+
+
+
+
+
+ 21
+
+
+
+
+
+
+ 22
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/rove_description/urdf/rove.urdf.xacro b/src/rove_description/urdf/rove.urdf.xacro
index 10e11c2..9d1de3c 100644
--- a/src/rove_description/urdf/rove.urdf.xacro
+++ b/src/rove_description/urdf/rove.urdf.xacro
@@ -16,8 +16,10 @@
+
-
+
+
@@ -42,10 +44,14 @@
+
+
+
+
-
+
diff --git a/src/rove_description/urdf/track.urdf.xacro b/src/rove_description/urdf/track.urdf.xacro
index 00685ee..e9fe3f3 100644
--- a/src/rove_description/urdf/track.urdf.xacro
+++ b/src/rove_description/urdf/track.urdf.xacro
@@ -19,6 +19,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
@@ -52,6 +64,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
track_${suffix}
diff --git a/utils/odrive_can_test.py b/utils/odrive_can_test.py
new file mode 100644
index 0000000..72f453f
--- /dev/null
+++ b/utils/odrive_can_test.py
@@ -0,0 +1,53 @@
+import asyncio
+from odrive_can import ODriveCAN, CanMsg
+from odrive_can.tools import UDP_Client
+
+AXIS_ID = 0
+INTERFACE = "can0"
+SETPOINT = 50
+
+udp = UDP_Client() # send data to UDP server for plotting
+
+def feedback_callback_fcn(msg: CanMsg, caller: ODriveCAN):
+ """called on position estimate"""
+ print(msg)
+ udp.send(msg.data)
+
+async def main():
+ """connect to odrive"""
+ try:
+ drv = ODriveCAN(axis_id=AXIS_ID, interface=INTERFACE)
+
+ # set up callback (optional)
+ drv.feedback_callback = feedback_callback_fcn
+
+ # start
+ await drv.start()
+
+ # check errors (raises exception if any)
+ await drv.check_errors() # Ensure this is awaited
+
+ # set controller mode
+ await drv.set_controller_mode("POSITION_CONTROL", "POS_FILTER")
+
+ # reset encoder
+ await drv.set_linear_count(0)
+
+ # set axis state
+ await drv.set_axis_state("CLOSED_LOOP_CONTROL")
+
+ # set position gain
+ await drv.set_pos_gain(3.0)
+
+ for _ in range(2):
+ # setpoint
+ await drv.set_input_pos(SETPOINT)
+ await asyncio.sleep(5.0)
+ await drv.set_input_pos(-SETPOINT)
+ await asyncio.sleep(5.0)
+
+ await drv.set_input_pos(0.0)
+ except Exception as e:
+ print(f"Error in main function: {e}")
+
+asyncio.run(main())
diff --git a/utils/odrive_usb_debug.py b/utils/odrive_usb_debug.py
new file mode 100644
index 0000000..d52adbb
--- /dev/null
+++ b/utils/odrive_usb_debug.py
@@ -0,0 +1,24 @@
+import odrive
+import time
+import math
+from odrive.enums import *
+
+
+odrv0 = odrive.find_any()
+odrv = odrv0
+
+#odrv.axis0.controller.config.spinout_electrical_power_threshold = 1000
+#odrv.axis0.controller.config.spinout_mechanical_power_threshold = -1000
+#odrv.axis0.config.can.error_msg_rate_ms = 100
+#odrv.axis0.config.can.node_id = 21
+#odrv.axis0.config.enable_watchdog = True
+
+print(odrv.axis0.config.can.node_id)
+print(odrv.axis0.config.enable_watchdog)
+print(odrv.axis0.config.watchdog_timeout)
+print(odrive.utils.dump_errors(odrv0))
+print(odrv.axis0.disarm_reason)
+print(odrv.axis0.active_errors)
+
+#odrv.axis0.config.can.node_id = 0
+#odrv.save_configuration()
\ No newline at end of file