Skip to content

update mecanum controller #12

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
rafal-gorecki merged 2 commits into from
May 13, 2025
Merged

update mecanum controller #12

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4573524
update mecanum controller
KmakD May 9, 2025
fa69500
add pre-commit
KmakD May 13, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 21 additions & 0 deletions .clang-format
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
Language: Cpp
BasedOnStyle: Google

AccessModifierOffset: -2
AlignAfterOpenBracket: AlwaysBreak
BraceWrapping:
AfterClass: true
AfterFunction: true
AfterNamespace: true
AfterStruct: true
BreakBeforeBraces: Custom
ColumnLimit: 100
ConstructorInitializerIndentWidth: 0
ContinuationIndentWidth: 2
DerivePointerAlignment: false
PointerAlignment: Middle
ReflowComments: true
IncludeBlocks: Preserve
AlignOperands: true
PenaltyBreakAssignment: 21
PenaltyBreakBeforeFirstCallParameter: 1
86 changes: 50 additions & 36 deletions mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,29 +31,30 @@
#include <string>
#include <vector>

#include "controller_interface/controller_interface.hpp"
#include <controller_interface/controller_interface.hpp>
#include <hardware_interface/handle.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/state.hpp>
#include <realtime_tools/realtime_box.hpp>
#include <realtime_tools/realtime_buffer.hpp>
#include <realtime_tools/realtime_publisher.hpp>

#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tf2_msgs/msg/tf_message.hpp>

#include "mecanum_drive_controller/mecanum_drive_controller_parameters.hpp"
#include "mecanum_drive_controller/odometry.hpp"
#include "mecanum_drive_controller/speed_limiter.hpp"
#include "mecanum_drive_controller/visibility_control.h"
#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "hardware_interface/handle.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "odometry.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "realtime_tools/realtime_box.hpp"
#include "realtime_tools/realtime_buffer.hpp"
#include "realtime_tools/realtime_publisher.hpp"
#include "tf2_msgs/msg/tf_message.hpp"

#include <mecanum_drive_controller/mecanum_drive_controller_parameters.hpp>

namespace mecanum_drive_controller
{
class MecanumDriveController : public controller_interface::ControllerInterface
{
using Twist = geometry_msgs::msg::TwistStamped;
using TwistStamped = geometry_msgs::msg::TwistStamped;

public:
MECANUM_DRIVE_CONTROLLER_PUBLIC
Expand All @@ -66,28 +67,35 @@ class MecanumDriveController : public controller_interface::ControllerInterface
controller_interface::InterfaceConfiguration state_interface_configuration() const override;

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

MECANUM_DRIVE_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_init() override;

MECANUM_DRIVE_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
controller_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state) override;

MECANUM_DRIVE_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
controller_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state) override;

MECANUM_DRIVE_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
controller_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state) override;

MECANUM_DRIVE_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override;
controller_interface::CallbackReturn on_cleanup(
const rclcpp_lifecycle::State & previous_state) override;

MECANUM_DRIVE_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_error(const rclcpp_lifecycle::State& previous_state) override;
controller_interface::CallbackReturn on_error(
const rclcpp_lifecycle::State & previous_state) override;

MECANUM_DRIVE_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state) override;
controller_interface::CallbackReturn on_shutdown(
const rclcpp_lifecycle::State & previous_state) override;

protected:
struct WheelHandle
Expand All @@ -96,9 +104,9 @@ class MecanumDriveController : public controller_interface::ControllerInterface
std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity;
};

const char* feedback_type() const;
controller_interface::CallbackReturn configure_wheel(const std::string& wheel_name,
std::unique_ptr<WheelHandle>& registered_handle);
const char * feedback_type() const;
controller_interface::CallbackReturn configure_wheel(
const std::string & wheel_name, std::unique_ptr<WheelHandle> & registered_handle);

std::string front_left_wheel_name_;
std::string front_right_wheel_name_;
Expand All @@ -117,42 +125,48 @@ class MecanumDriveController : public controller_interface::ControllerInterface
Odometry odometry_;

std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odometry_publisher_ = nullptr;
std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>> realtime_odometry_publisher_ = nullptr;
std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
realtime_odometry_publisher_ = nullptr;

std::shared_ptr<rclcpp::Publisher<tf2_msgs::msg::TFMessage>> odometry_transform_publisher_ = nullptr;
std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>> realtime_odometry_transform_publisher_ =
nullptr;
std::shared_ptr<rclcpp::Publisher<tf2_msgs::msg::TFMessage>> odometry_transform_publisher_ =
nullptr;
std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
realtime_odometry_transform_publisher_ = nullptr;

// Timeout to consider cmd_vel commands old
std::chrono::milliseconds cmd_vel_timeout_{ 500 };
std::chrono::milliseconds cmd_vel_timeout_{500};

bool subscriber_is_active_ = false;
rclcpp::Subscription<Twist>::SharedPtr velocity_command_subscriber_ = nullptr;
rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;

realtime_tools::RealtimeBox<std::shared_ptr<Twist>> received_velocity_msg_ptr_{ nullptr };
realtime_tools::RealtimeBuffer<std::shared_ptr<TwistStamped>> received_velocity_msg_ptr_{nullptr};

std::queue<Twist> previous_commands_; // last two commands
std::queue<TwistStamped> previous_commands_; // last two commands

// speed limiters
SpeedLimiter limiter_linear_x_;
SpeedLimiter limiter_linear_y_;
SpeedLimiter limiter_angular_;

bool publish_limited_velocity_ = false;
std::shared_ptr<rclcpp::Publisher<Twist>> limited_velocity_publisher_ = nullptr;
std::shared_ptr<realtime_tools::RealtimePublisher<Twist>> realtime_limited_velocity_publisher_ = nullptr;
std::shared_ptr<rclcpp::Publisher<TwistStamped>> limited_velocity_publisher_ = nullptr;
std::shared_ptr<realtime_tools::RealtimePublisher<TwistStamped>>
realtime_limited_velocity_publisher_ = nullptr;

rclcpp::Time previous_update_timestamp_{ 0 };
rclcpp::Time previous_update_timestamp_{0};

// publish rate limiter
double publish_rate_ = 50.0;
rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0);
rclcpp::Time previous_publish_timestamp_{ 0 };
rclcpp::Time previous_publish_timestamp_{0};

bool is_halted = false;

bool reset();
void halt();

private:
void reset_buffers();
};
} // namespace mecanum_drive_controller
#endif // MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_
Loading