Class SwerveDriveController

Inheritance Relationships

Base Type

  • public controller_interface::ControllerInterface

Class Documentation

class SwerveDriveController : public controller_interface::ControllerInterface

Public Functions

SwerveDriveController()

Constructor.

CallbackReturn on_init() override

Initialization function see @ControllerInterfaceon_init.

CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override

Configuration function see @ControllerInterfaceon_configure.

CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override

Activation function see @ControllerInterfaceon_activate.

CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override

Deactivation function see @ControllerInterfaceon_deactivate.

controller_interface::InterfaceConfiguration command_interface_configuration() const override

Command interface configuration function see @ControllerInterfacecommand_interface_configuration.

controller_interface::InterfaceConfiguration state_interface_configuration() const override

State interface configuration function see @ControllerInterfacestate_interface_configuration.

controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override

Update function see @ControllerInterfaceupdate.

void reference_callback(const std::shared_ptr<CmdVelMsg> msg)

Callback for the cmd_vel subscriber.

Public Members

std::shared_ptr<ParamListener> param_listener_
Params params_

Protected Types

using CommandedJointStatePublisher = realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>

Protected Functions

double shortest_angular_distance(double from, double to)

Calculate the shortest angular distance between two angles (result in [-pi, pi]).

double normalize_angle_positive(double angle)
void reset_controller_reference_msg(const std::shared_ptr<geometry_msgs::msg::Twist> &msg)

Protected Attributes

std::vector<std::string> steering_joint_names_
std::vector<std::string> wheel_joint_names_
double wheel_radius_
std::vector<double> module_x_offsets_
std::vector<double> module_y_offsets_
std::vector<double> module_angle_offsets_
std::vector<double> module_steering_limit_lower_
std::vector<double> module_steering_limit_upper_
std::vector<double> module_wheel_speed_limit_lower_
std::vector<double> module_wheel_speed_limit_upper_
bool enabled_steering_flip_
bool enabled_steering_angular_velocity_limit_
bool enabled_steering_angular_limit_
bool enabled_open_loop_
uint is_rotation_direction_
std::vector<double> previoud_steering_commands_
double steering_angular_velocity_limit_
double steering_alignment_angle_error_threshold_
double steering_alignment_start_angle_error_threshold_
double steering_alignment_start_speed_error_threshold_
std::string odom_solver_method_str_
std::string cmd_vel_topic_
bool use_stamped_cmd_vel_
double cmd_vel_timeout_
rclcpp::Duration ref_timeout_
size_t num_modules_ = 0
realtime_tools::RealtimeBuffer<std::shared_ptr<CmdVelMsg>> cmd_vel_buffer_
rclcpp::Subscription<CmdVelMsg>::SharedPtr cmd_vel_subscriber_ = nullptr
std::vector<ModuleHandles> module_handles_
double target_vx_ = 0.0
double target_vy_ = 0.0
double target_wz_ = 0.0
rclcpp::Time last_cmd_vel_time_
Odometry odometry_
rclcpp::Publisher<OdomStateMsg>::SharedPtr odom_s_publisher_ = nullptr
std::unique_ptr<OdomStatePublisher> rt_odom_state_publisher_ = nullptr
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr commanded_joint_state_publisher_ = nullptr
std::unique_ptr<CommandedJointStatePublisher> rt_commanded_joint_state_publisher_ = nullptr
std::string odom_frame_id_
std::string base_frame_id_
bool enable_odom_tf_
std::vector<double> pose_covariance_diagonal_
std::vector<double> twist_covariance_diagonal_
int velocity_rolling_window_size_
std::string odom_source_
rclcpp::Publisher<TfStateMsg>::SharedPtr tf_odom_s_publisher_
std::unique_ptr<TfStatePublisher> rt_tf_odom_state_publisher_
bool enable_visualization_
std::string visualization_marker_topic_
std::unique_ptr<MarkerVisualize> visualizer_ = nullptr
double visualization_update_time_
rclcpp::Time last_visualization_publish_time_
bool enabled_speed_limits_
bool publish_limited_velocity_
std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::Twist>> limited_velocity_publisher_ = nullptr
std::shared_ptr<realtime_tools::RealtimePublisher<geometry_msgs::msg::Twist>> realtime_limited_velocity_publisher_ = nullptr
std::queue<Twist> previous_commands_
SpeedLimiter limiter_linear_x_
SpeedLimiter limiter_linear_y_
SpeedLimiter limiter_angular_z_
bool enable_direct_joint_commands_ = false
double wheel_saturation_scale_factor_ = 1.0
bool enabled_wheel_saturation_scaling_ = false
std::vector<double> previous_wheel_directions_

Protected Static Functions

static double normalize_angle(double angle_rad)

Normalize angle to be within the range [-pi, pi].