From 0dcecd1c4ccee5093e8f1a368a2565ffd3bc2014 Mon Sep 17 00:00:00 2001 From: ilithebutterfly Date: Wed, 5 Jun 2024 14:59:40 -0400 Subject: [PATCH] Joy or cmd_vel are lagging --- src/rove_bringup/CMakeLists.txt | 3 +- .../config/teleop_joy_params.yaml | 7 +- src/rove_bringup/launch/common.launch.py | 26 +-- src/rove_bringup/launch/real.launch.py | 107 ++++++++- .../rove_controller_bluetooth.launch.py | 4 +- .../launch/rove_controller_usb.launch.py | 6 +- src/rove_bringup/package.xml | 2 + src/rove_bringup/src/rove_controller_node.cpp | 38 ++- .../config/test_controllers.yaml | 14 ++ .../config/tracks_controllers.yaml | 87 +++++++ .../urdf/rove.ros2_control.urdf.xacro | 111 +++++++++ src/rove_description/urdf/rove.urdf.xacro | 12 +- src/rove_description/urdf/track.urdf.xacro | 34 +++ src/rove_tracks/CMakeLists.txt | 24 +- .../rove_tracks/chained_controller.hpp | 53 +++-- ...validate_chained_controller_parameters.hpp | 1 + src/rove_tracks/src/chained_controller.cpp | 219 +++++++++++------- src/rove_tracks/src/chained_controller.yaml | 45 ++-- .../test/chained_controller_params.yaml | 7 +- .../test/test_chained_controller.cpp | 7 +- src/rove_tracks/urdf/rove_track.urdf.xacro | 37 +++ 21 files changed, 671 insertions(+), 173 deletions(-) create mode 100644 src/rove_description/config/test_controllers.yaml create mode 100644 src/rove_description/config/tracks_controllers.yaml create mode 100644 src/rove_description/urdf/rove.ros2_control.urdf.xacro create mode 100644 src/rove_tracks/urdf/rove_track.urdf.xacro 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..c5aff47 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: 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 \ 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 b88bcd7..6b18040 100644 --- a/src/rove_bringup/launch/common.launch.py +++ b/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, ]) diff --git a/src/rove_bringup/launch/real.launch.py b/src/rove_bringup/launch/real.launch.py index ef14e0b..f08fa88 100644 --- a/src/rove_bringup/launch/real.launch.py +++ b/src/rove_bringup/launch/real.launch.py @@ -2,17 +2,60 @@ 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( @@ -20,6 +63,55 @@ def generate_launch_description(): ), ) + ###### 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( @@ -27,8 +119,13 @@ def generate_launch_description(): 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, ]) 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..7241cb8 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,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( 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..7ea6902 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,14 @@ 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); + // cmd_vel_stamped_pub_ = create_publisher("/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::SharedPtr joy_sub_; + // rclcpp::Subscription::SharedPtr cmd_vel_sub_; rclcpp::Publisher::SharedPtr joy_pub_; + // rclcpp::Publisher::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) { diff --git a/src/rove_description/config/test_controllers.yaml b/src/rove_description/config/test_controllers.yaml new file mode 100644 index 0000000..43b5fdd --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/rove_description/config/tracks_controllers.yaml b/src/rove_description/config/tracks_controllers.yaml new file mode 100644 index 0000000..92d4dd3 --- /dev/null +++ b/src/rove_description/config/tracks_controllers.yaml @@ -0,0 +1,87 @@ +# This config file is used by ros2_control +controller_manager: + ros__parameters: + update_rate: 50 # 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.419 + wheel_radius: 0.0855 + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: -1.0 + right_wheel_radius_multiplier: 1.0 + + publish_rate: 50.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 + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear.x.has_velocity_limits: true + linear.x.has_acceleration_limits: true + linear.x.has_jerk_limits: false + linear.x.max_velocity: 100.0 + # linear.x.min_velocity: -1.0 + linear.x.max_acceleration: 100.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: 100.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 + +# left_track_controller: +# ros__parameters: +# track_joint_name: track_l_j +# front_joint: track_fl_j +# back_joint: track_rl_j + +# right_track_controller: +# ros__parameters: +# track_joint_name: track_r_j +# front_joint: track_fr_j +# back_joint: track_rr_j \ 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..882deec --- /dev/null +++ b/src/rove_description/urdf/rove.ros2_control.urdf.xacro @@ -0,0 +1,111 @@ + + + + + + + + + + + + + + + + + + + + odrive_ros2_control_plugin/ODriveHardwareInterface + can0 + + + 23 + + + + + + + 24 + + + + + + + + 21 + + + + + + + 22 + + + + + + + + + + + mock_components/GenericSystem + true + + + + + + + + + + + + + + \ 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 48a80b8..0248842 100644 --- a/src/rove_description/urdf/rove.urdf.xacro +++ b/src/rove_description/urdf/rove.urdf.xacro @@ -16,8 +16,10 @@ + - + + @@ -48,10 +50,14 @@ + + + + - + diff --git a/src/rove_description/urdf/track.urdf.xacro b/src/rove_description/urdf/track.urdf.xacro index bb8c100..328db4d 100644 --- a/src/rove_description/urdf/track.urdf.xacro +++ b/src/rove_description/urdf/track.urdf.xacro @@ -19,6 +19,24 @@ + + + + + + + + + + + + + + @@ -52,6 +70,22 @@ + + + + + + + + + + + + + + + + track_${suffix} diff --git a/src/rove_tracks/CMakeLists.txt b/src/rove_tracks/CMakeLists.txt index 85993ea..d045986 100644 --- a/src/rove_tracks/CMakeLists.txt +++ b/src/rove_tracks/CMakeLists.txt @@ -29,23 +29,23 @@ generate_parameter_library(chained_controller_parameters include/rove_tracks/validate_chained_controller_parameters.hpp ) add_library( - chained_controller + rove_tracks SHARED src/chained_controller.cpp ) -target_include_directories(chained_controller PUBLIC +target_include_directories(rove_tracks PUBLIC "$" "$") -target_link_libraries(chained_controller chained_controller_parameters) -ament_target_dependencies(chained_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_definitions(chained_controller PRIVATE "CHAINED_CONTROLLER_BUILDING_DLL") +target_link_libraries(rove_tracks chained_controller_parameters) +ament_target_dependencies(rove_tracks ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(rove_tracks PRIVATE "CHAINED_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface rove_tracks.xml) install( TARGETS - chained_controller + rove_tracks RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -56,7 +56,13 @@ install( DESTINATION include/${PROJECT_NAME} ) -if(BUILD_TESTING) +install( + DIRECTORY urdf + DESTINATION share/${PROJECT_NAME} +) + +# if(BUILD_TESTING) +if(0) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights # comment the line when a copyright and license is added to all source files @@ -88,7 +94,7 @@ if(BUILD_TESTING) hardware_interface ) - add_rostest_with_parameters_gmock(test_chained_controller_preceeding test/test_chained_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/chained_controller_preceeding_params.yaml) + add_rostest_with_parameters_gchained_controllermock(test_chained_controller_preceeding test/test_chained_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/chained_controller_preceeding_params.yaml) target_include_directories(test_chained_controller_preceeding PRIVATE include) target_link_libraries(test_chained_controller_preceeding chained_controller) ament_target_dependencies( @@ -105,7 +111,7 @@ ament_export_dependencies( ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_export_libraries( - chained_controller + rove_tracks ) ament_package() diff --git a/src/rove_tracks/include/rove_tracks/chained_controller.hpp b/src/rove_tracks/include/rove_tracks/chained_controller.hpp index b600803..71b5a00 100644 --- a/src/rove_tracks/include/rove_tracks/chained_controller.hpp +++ b/src/rove_tracks/include/rove_tracks/chained_controller.hpp @@ -35,19 +35,39 @@ namespace rove_tracks { -// name constants for state interfaces -static constexpr size_t STATE_MY_ITFS = 0; -// name constants for command interfaces -static constexpr size_t CMD_MY_ITFS = 0; +enum track_cmd_id : int +{ + TRK_CMD_FRONT_VELOCITY = 0, + TRK_CMD_BACK_VELOCITY, + TRK_CMD_FRONT_EFFORT, + TRK_CMD_BACK_EFFORT, + TRK_CMD_COUNT, + TRK_CMD_TRACK_VELOCITY = TRK_CMD_COUNT, + TRK_CMD_TOTAL_COUNT, + TRK_CMD_NONE = -1, +}; -// TODO(anyone: example setup for control mode (usually you will use some enums defined in messages) -enum class control_mode_type : std::uint8_t +enum track_state_id : int { - FAST = 0, - SLOW = 1, + TRK_ST_FRONT_POSITION = 0, + TRK_ST_BACK_POSITION, + TRK_ST_FRONT_VELOCITY, + TRK_ST_BACK_VELOCITY, + TRK_ST_FRONT_EFFORT, + TRK_ST_BACK_EFFORT, + TRK_ST_COUNT, + TRK_ST_TRACK_VELOCITY = TRK_ST_COUNT, + TRK_ST_TOTAL_COUNT, + TRK_ST_NONE = -1, }; +// name constants for command interfaces +static constexpr size_t CMD_MY_ITFS = TRK_CMD_COUNT; + +// name constants for state interfaces +static constexpr size_t STATE_MY_ITFS = TRK_ST_COUNT; + class ChainedController : public controller_interface::ChainableControllerInterface { public: @@ -88,18 +108,21 @@ class ChainedController : public controller_interface::ChainableControllerInterf using ControllerStateMsg = control_msgs::msg::JointControllerState; protected: - std::shared_ptr param_listener_; - chained_controller::Params params_; + std::shared_ptr param_listener_; + rove_tracks::Params params_; - std::vector state_joints_; + std::string front_joint_; + std::string back_joint_; + std::string track_joint_; + int direction_ = 0; + int coasting_direction_ = 0; + const double inrush_torque_ = 0; + bool caught_up_ = false; // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; - rclcpp::Service::SharedPtr set_slow_control_mode_service_; - realtime_tools::RealtimeBuffer control_mode_; - using ControllerStatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr s_publisher_; @@ -114,6 +137,8 @@ class ChainedController : public controller_interface::ChainableControllerInterf // callback for topic interface ROVE_TRACKS__VISIBILITY_LOCAL void reference_callback(const std::shared_ptr msg); + + rclcpp::Logger logger() const; }; } // namespace rove_tracks diff --git a/src/rove_tracks/include/rove_tracks/validate_chained_controller_parameters.hpp b/src/rove_tracks/include/rove_tracks/validate_chained_controller_parameters.hpp index caf9569..10a8db2 100644 --- a/src/rove_tracks/include/rove_tracks/validate_chained_controller_parameters.hpp +++ b/src/rove_tracks/include/rove_tracks/validate_chained_controller_parameters.hpp @@ -18,6 +18,7 @@ #include +#include #include "parameter_traits/parameter_traits.hpp" namespace parameter_traits diff --git a/src/rove_tracks/src/chained_controller.cpp b/src/rove_tracks/src/chained_controller.cpp index 2a3d9ad..4597465 100644 --- a/src/rove_tracks/src/chained_controller.cpp +++ b/src/rove_tracks/src/chained_controller.cpp @@ -20,6 +20,7 @@ #include #include +#include #include "controller_interface/helpers.hpp" namespace @@ -58,14 +59,13 @@ ChainedController::ChainedController() : controller_interface::ChainableControll controller_interface::CallbackReturn ChainedController::on_init() { - control_mode_.initRT(control_mode_type::FAST); - try { - param_listener_ = std::make_shared(get_node()); + param_listener_ = std::make_shared(get_node()); } catch (const std::exception & e) { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); return controller_interface::CallbackReturn::ERROR; } @@ -78,58 +78,24 @@ controller_interface::CallbackReturn ChainedController::on_configure( { params_ = param_listener_->get_params(); - if (!params_.state_joints.empty()) - { - state_joints_ = params_.state_joints; - } - else - { - state_joints_ = params_.joints; - } - - if (params_.joints.size() != state_joints_.size()) - { - RCLCPP_FATAL( - get_node()->get_logger(), - "Size of 'joints' (%zu) and 'state_joints' (%zu) parameters has to be the same!", - params_.joints.size(), state_joints_.size()); - return CallbackReturn::FAILURE; - } + front_joint_ = params_.front_joint; + back_joint_ = params_.back_joint; + track_joint_ = params_.track_joint_name; - // topics QoS + // // topics QoS auto subscribers_qos = rclcpp::SystemDefaultsQoS(); subscribers_qos.keep_last(1); subscribers_qos.best_effort(); - // Reference Subscriber + // // Reference Subscriber ref_subscriber_ = get_node()->create_subscription( "~/reference", subscribers_qos, std::bind(&ChainedController::reference_callback, this, std::placeholders::_1)); std::shared_ptr msg = std::make_shared(); - reset_controller_reference_msg(msg, params_.joints); + reset_controller_reference_msg(msg, {params_.track_joint_name}); input_ref_.writeFromNonRT(msg); - auto set_slow_mode_service_callback = - [&]( - const std::shared_ptr request, - std::shared_ptr response) - { - if (request->data) - { - control_mode_.writeFromNonRT(control_mode_type::SLOW); - } - else - { - control_mode_.writeFromNonRT(control_mode_type::FAST); - } - response->success = true; - }; - - set_slow_control_mode_service_ = get_node()->create_service( - "~/set_slow_control_mode", set_slow_mode_service_callback, - rmw_qos_profile_services_hist_keep_all); - try { // State publisher @@ -147,7 +113,7 @@ controller_interface::CallbackReturn ChainedController::on_configure( // TODO(anyone): Reserve memory in state publisher depending on the message type state_publisher_->lock(); - state_publisher_->msg_.header.frame_id = params_.joints[0]; + state_publisher_->msg_.header.frame_id = params_.track_joint_name; state_publisher_->unlock(); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); @@ -159,11 +125,16 @@ controller_interface::InterfaceConfiguration ChainedController::command_interfac controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names.reserve(params_.joints.size()); - for (const auto & joint : params_.joints) - { - command_interfaces_config.names.push_back(joint + "/" + params_.interface_name); - } + // command_interfaces_config.names.reserve(TRK_CMD_TOTAL_COUNT); + command_interfaces_config.names.reserve(TRK_CMD_COUNT); + // command_interfaces_config.names.push_back(front_joint_ + "/" + hardware_interface::HW_IF_POSITION); + // command_interfaces_config.names.push_back(back_joint_ + "/" + hardware_interface::HW_IF_POSITION); + command_interfaces_config.names.push_back(front_joint_ + "/" + hardware_interface::HW_IF_VELOCITY); + command_interfaces_config.names.push_back(back_joint_ + "/" + hardware_interface::HW_IF_VELOCITY); + command_interfaces_config.names.push_back(front_joint_ + "/" + hardware_interface::HW_IF_EFFORT); + command_interfaces_config.names.push_back(back_joint_ + "/" + hardware_interface::HW_IF_EFFORT); + // command_interfaces_config.names.push_back(track_joint_ + "/" + hardware_interface::HW_IF_VELOCITY); + // command_interfaces_config.names.push_back(track_joint_ + "/" + hardware_interface::HW_IF_VELOCITY); return command_interfaces_config; } @@ -173,49 +144,71 @@ controller_interface::InterfaceConfiguration ChainedController::state_interface_ controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.reserve(state_joints_.size()); - for (const auto & joint : state_joints_) - { - state_interfaces_config.names.push_back(joint + "/" + params_.interface_name); - } + // state_interfaces_config.names.reserve(TRK_ST_TOTAL_COUNT); + state_interfaces_config.names.reserve(TRK_ST_COUNT); + state_interfaces_config.names.push_back(front_joint_ + "/" + hardware_interface::HW_IF_POSITION); + state_interfaces_config.names.push_back(back_joint_ + "/" + hardware_interface::HW_IF_POSITION); + state_interfaces_config.names.push_back(front_joint_ + "/" + hardware_interface::HW_IF_VELOCITY); + state_interfaces_config.names.push_back(back_joint_ + "/" + hardware_interface::HW_IF_VELOCITY); + state_interfaces_config.names.push_back(front_joint_ + "/" + hardware_interface::HW_IF_EFFORT); + state_interfaces_config.names.push_back(back_joint_ + "/" + hardware_interface::HW_IF_EFFORT); + // state_interfaces_config.names.push_back(track_joint_ + "/" + hardware_interface::HW_IF_VELOCITY); + // for (const auto & joint : state_joints_) + // { + // state_interfaces_config.names.push_back(joint + "/" + params_.interface_name); + // } return state_interfaces_config; } void ChainedController::reference_callback(const std::shared_ptr msg) { - if (msg->joint_names.size() == params_.joints.size()) + if (msg->joint_names.size() == 1) { input_ref_.writeFromNonRT(msg); } else { RCLCPP_ERROR( - get_node()->get_logger(), + logger(), "Received %zu , but expected %zu joints in command. Ignoring message.", - msg->joint_names.size(), params_.joints.size()); + msg->joint_names.size(), 1); } } + +rclcpp::Logger ChainedController::logger() const +{ + return get_node()->get_logger(); +} + std::vector ChainedController::on_export_reference_interfaces() { - reference_interfaces_.resize(state_joints_.size(), std::numeric_limits::quiet_NaN()); + // reference_interfaces_.resize(state_joints_.size(), std::numeric_limits::quiet_NaN()); + reference_interfaces_.resize(TRK_CMD_TOTAL_COUNT - TRK_CMD_COUNT, std::numeric_limits::quiet_NaN()); + std::vector reference_interfaces; reference_interfaces.reserve(reference_interfaces_.size()); + RCLCPP_INFO(logger(), "Interface count: %d", reference_interfaces_.size()); - for (size_t i = 0; i < reference_interfaces_.size(); ++i) - { - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), state_joints_[i] + "/" + params_.interface_name, - &reference_interfaces_[i])); - } + reference_interfaces.push_back(hardware_interface::CommandInterface(get_node()->get_name(), track_joint_ + "/" + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[0])); + RCLCPP_INFO(logger(), "Interface exported: %d", reference_interfaces.size()); + + // for (size_t i = 0; i < reference_interfaces_.size(); ++i) + // { + // reference_interfaces.push_back(hardware_interface::CommandInterface( + // get_node()->get_name(), state_joints_[i] + "/" + params_.interface_name, + // &reference_interfaces_[i])); + // } return reference_interfaces; } bool ChainedController::on_set_chained_mode(bool chained_mode) { + RCLCPP_INFO(logger(), "Set chained mode to: %d", chained_mode); // Always accept switch to/from chained mode return true || chained_mode; } @@ -224,7 +217,7 @@ controller_interface::CallbackReturn ChainedController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // Set default value in command - reset_controller_reference_msg(*(input_ref_.readFromRT()), state_joints_); + reset_controller_reference_msg(*(input_ref_.readFromRT()), {track_joint_}); return controller_interface::CallbackReturn::SUCCESS; } @@ -262,34 +255,98 @@ controller_interface::return_type ChainedController::update_reference_from_subsc controller_interface::return_type ChainedController::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, - // instead of a loop - for (size_t i = 0; i < command_interfaces_.size(); ++i) + static rclcpp::Clock clk{}; + if (!std::isnan(reference_interfaces_[0])) { - if (!std::isnan(reference_interfaces_[i])) - { - if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) - { - reference_interfaces_[i] /= 2; - } - command_interfaces_[i].set_value(reference_interfaces_[i]); + RCLCPP_INFO_STREAM_THROTTLE(logger(), clk, 1000, "Ref value: " << reference_interfaces_[0]); + direction_ = reference_interfaces_[0] == 0 ? 0 : ( + reference_interfaces_[0] < 0 ? -1 : 1 + ); + } - reference_interfaces_[i] = std::numeric_limits::quiet_NaN(); + track_cmd_id leading_cmd = track_cmd_id::TRK_CMD_NONE; + track_cmd_id trailing_cmd = track_cmd_id::TRK_CMD_NONE; + track_state_id leading_st_velocity = track_state_id::TRK_ST_NONE; + track_state_id leading_st_effort = track_state_id::TRK_ST_NONE; + + switch (direction_) + { + case -1: + leading_cmd = track_cmd_id::TRK_CMD_BACK_VELOCITY; + trailing_cmd = track_cmd_id::TRK_CMD_FRONT_EFFORT; + leading_st_velocity = track_state_id::TRK_ST_BACK_VELOCITY; + leading_st_effort = track_state_id::TRK_ST_BACK_EFFORT; + break; + case 0: + if (coasting_direction_ < 0) + { + leading_cmd = track_cmd_id::TRK_CMD_BACK_VELOCITY; + trailing_cmd = track_cmd_id::TRK_CMD_FRONT_EFFORT; + leading_st_velocity = track_state_id::TRK_ST_BACK_VELOCITY; + leading_st_effort = track_state_id::TRK_ST_BACK_EFFORT; } + if (coasting_direction_ > 0) + { + leading_cmd = track_cmd_id::TRK_CMD_FRONT_VELOCITY; + trailing_cmd = track_cmd_id::TRK_CMD_BACK_EFFORT; + leading_st_velocity = track_state_id::TRK_ST_FRONT_VELOCITY; + leading_st_effort = track_state_id::TRK_ST_FRONT_EFFORT; + } + break; + case 1: + leading_cmd = track_cmd_id::TRK_CMD_FRONT_VELOCITY; + trailing_cmd = track_cmd_id::TRK_CMD_BACK_EFFORT; + leading_st_velocity = track_state_id::TRK_ST_FRONT_VELOCITY; + leading_st_effort = track_state_id::TRK_ST_FRONT_EFFORT; + break; + default: + leading_cmd = track_cmd_id::TRK_CMD_NONE; + trailing_cmd = track_cmd_id::TRK_CMD_NONE; + leading_st_velocity = track_state_id::TRK_ST_NONE; + leading_st_effort = track_state_id::TRK_ST_NONE; + break; + } + + if (!std::isnan(reference_interfaces_[0])) + { + command_interfaces_[leading_cmd].set_value(reference_interfaces_[0]); + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + } + + double leading_vel = 0; + double leading_torque = 0; + if (leading_st_velocity != track_state_id::TRK_ST_NONE) + { + leading_vel = state_interfaces_[leading_st_velocity].get_value(); + coasting_direction_ = /*TODO close to*/leading_vel == 0 ? 0 : ( + leading_vel > 0 ? 1 : -1 + ); } - if (state_publisher_ && state_publisher_->trylock()) + if (leading_st_effort != track_state_id::TRK_ST_NONE) { - state_publisher_->msg_.header.stamp = time; - state_publisher_->msg_.set_point = command_interfaces_[CMD_MY_ITFS].get_value(); + leading_torque = state_interfaces_[leading_st_effort].get_value(); + } - state_publisher_->unlockAndPublish(); + if (coasting_direction_ < direction_ || coasting_direction_ > direction_) caught_up_ = false; + if (abs(leading_torque) > inrush_torque_) caught_up_ = true; + + double torque = caught_up_ ? leading_torque : (std::min(inrush_torque_, abs(leading_torque)) * coasting_direction_); + + if (coasting_direction_ != 0) + { + command_interfaces_[trailing_cmd].set_value(torque); } + RCLCPP_INFO_STREAM_THROTTLE(logger(), clk, 1000, "Ref value: " << reference_interfaces_[0] << + " Direction: " << direction_ << + " Leading vel: " << leading_vel << + " Leading torque: " << leading_torque << + " Coasting direction: " << coasting_direction_); return controller_interface::return_type::OK; } -} // namespace rove_tracks +} // namespace rove_tracks #include "pluginlib/class_list_macros.hpp" diff --git a/src/rove_tracks/src/chained_controller.yaml b/src/rove_tracks/src/chained_controller.yaml index aa92704..f58782a 100644 --- a/src/rove_tracks/src/chained_controller.yaml +++ b/src/rove_tracks/src/chained_controller.yaml @@ -1,31 +1,30 @@ -chained_controller: - joints: { - type: string_array, - default_value: [], - description: "Specifies joints used by the controller. If state joints parameter is defined, then only command joints are defined with this parameter.", +rove_tracks: + track_joint_name: { + type: string, + default_value: "", + description: "The name of the track joint.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } + # validation: { + # not_empty<>: null, + # } } - state_joints: { - type: string_array, - default_value: [], - description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + front_joint: { + type: string, + default_value: "", + description: "Specifies the joint at the front of the track.", read_only: true, - validation: { - unique<>: null, - } + # validation: { + # not_empty<>: null, + # # unique<>: null, + # } } - interface_name: { + back_joint: { type: string, default_value: "", - description: "Name of the interface used by the controller on joints and command_joints.", + description: "Specifies the joint at the back of the track.", read_only: true, - validation: { - not_empty<>: null, - one_of<>: [["position", "velocity", "acceleration", "effort",]], - forbidden_interface_name_prefix: null - } + # validation: { + # not_empty<>: null, + # # unique<>: null, + # } } diff --git a/src/rove_tracks/test/chained_controller_params.yaml b/src/rove_tracks/test/chained_controller_params.yaml index a2acc4c..91fc7d6 100644 --- a/src/rove_tracks/test/chained_controller_params.yaml +++ b/src/rove_tracks/test/chained_controller_params.yaml @@ -1,7 +1,6 @@ test_chained_controller: ros__parameters: + track_joint_name: joint1 + front_joint: joint2 + back_joint: joint3 - joints: - - joint1 - - interface_name: acceleration diff --git a/src/rove_tracks/test/test_chained_controller.cpp b/src/rove_tracks/test/test_chained_controller.cpp index d980b7e..f3cb7f5 100644 --- a/src/rove_tracks/test/test_chained_controller.cpp +++ b/src/rove_tracks/test/test_chained_controller.cpp @@ -22,7 +22,6 @@ #include using rove_tracks::CMD_MY_ITFS; -using rove_tracks::control_mode_type; using rove_tracks::STATE_MY_ITFS; class ChainedControllerTest : public ChainedControllerFixture @@ -33,9 +32,9 @@ TEST_F(ChainedControllerTest, all_parameters_set_configure_success) { SetUpController(); - ASSERT_TRUE(controller_->params_.joints.empty()); - ASSERT_TRUE(controller_->params_.state_joints.empty()); - ASSERT_TRUE(controller_->params_.interface_name.empty()); + ASSERT_TRUE(controller_->params_.front_joint == ""); + ASSERT_TRUE(controller_->params_.back_joint == ""); + ASSERT_TRUE(controller_->params_.track_joint_name == ""); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); diff --git a/src/rove_tracks/urdf/rove_track.urdf.xacro b/src/rove_tracks/urdf/rove_track.urdf.xacro new file mode 100644 index 0000000..06da19d --- /dev/null +++ b/src/rove_tracks/urdf/rove_track.urdf.xacro @@ -0,0 +1,37 @@ + + + + + + ${nodeid} + + + + + + + + + + + + + + + + + + + + mock_components/GenericSystem + + + + + + + + + + +