-
Notifications
You must be signed in to change notification settings - Fork 333
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Use the .hpp headers from
realtime_tools package (#1406)
(cherry picked from commit cd73055) # Conflicts: # diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp # joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp # mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp # mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp # parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp # steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp # tricycle_controller/include/tricycle_controller/tricycle_controller.hpp
- Loading branch information
1 parent
ac31eae
commit 11b20e1
Showing
17 changed files
with
475 additions
and
16 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
162 changes: 162 additions & 0 deletions
162
mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,162 @@ | ||
| // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #ifndef MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ | ||
| #define MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ | ||
|
|
||
| #include <chrono> | ||
| #include <cmath> | ||
| #include <memory> | ||
| #include <queue> | ||
| #include <string> | ||
| #include <utility> | ||
| #include <vector> | ||
|
|
||
| #include "controller_interface/chainable_controller_interface.hpp" | ||
| #include "mecanum_drive_controller/odometry.hpp" | ||
| #include "mecanum_drive_controller/visibility_control.h" | ||
| #include "mecanum_drive_controller_parameters.hpp" | ||
| #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" | ||
| #include "rclcpp_lifecycle/state.hpp" | ||
| #include "realtime_tools/realtime_buffer.hpp" | ||
| #include "realtime_tools/realtime_publisher.hpp" | ||
| #include "std_srvs/srv/set_bool.hpp" | ||
|
|
||
| #include "control_msgs/msg/mecanum_drive_controller_state.hpp" | ||
| #include "geometry_msgs/msg/twist_stamped.hpp" | ||
| #include "nav_msgs/msg/odometry.hpp" | ||
| #include "tf2_msgs/msg/tf_message.hpp" | ||
| namespace mecanum_drive_controller | ||
| { | ||
| // name constants for state interfaces | ||
| static constexpr size_t NR_STATE_ITFS = 4; | ||
|
|
||
| // name constants for command interfaces | ||
| static constexpr size_t NR_CMD_ITFS = 4; | ||
|
|
||
| // name constants for reference interfaces | ||
| static constexpr size_t NR_REF_ITFS = 3; | ||
|
|
||
| class MecanumDriveController : public controller_interface::ChainableControllerInterface | ||
| { | ||
| public: | ||
| MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC | ||
| MecanumDriveController(); | ||
|
|
||
| MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC | ||
| controller_interface::CallbackReturn on_init() override; | ||
|
|
||
| MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC | ||
| controller_interface::InterfaceConfiguration command_interface_configuration() const override; | ||
|
|
||
| MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC | ||
| controller_interface::InterfaceConfiguration state_interface_configuration() const override; | ||
|
|
||
| MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC | ||
| controller_interface::CallbackReturn on_configure( | ||
| const rclcpp_lifecycle::State & previous_state) override; | ||
|
|
||
| MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC | ||
| controller_interface::CallbackReturn on_activate( | ||
| const rclcpp_lifecycle::State & previous_state) override; | ||
|
|
||
| MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC | ||
| controller_interface::CallbackReturn on_deactivate( | ||
| const rclcpp_lifecycle::State & previous_state) override; | ||
|
|
||
| MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC | ||
| controller_interface::return_type update_reference_from_subscribers( | ||
| const rclcpp::Time & time, const rclcpp::Duration & period) override; | ||
|
|
||
| MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC | ||
| controller_interface::return_type update_and_write_commands( | ||
| const rclcpp::Time & time, const rclcpp::Duration & period) override; | ||
|
|
||
| using ControllerReferenceMsg = geometry_msgs::msg::TwistStamped; | ||
| using OdomStateMsg = nav_msgs::msg::Odometry; | ||
| using TfStateMsg = tf2_msgs::msg::TFMessage; | ||
| using ControllerStateMsg = control_msgs::msg::MecanumDriveControllerState; | ||
|
|
||
| protected: | ||
| std::shared_ptr<mecanum_drive_controller::ParamListener> param_listener_; | ||
| mecanum_drive_controller::Params params_; | ||
|
|
||
| /** | ||
| * The list is sorted in the following order: | ||
| * - front left wheel | ||
| * - front right wheel | ||
| * - back right wheel | ||
| * - back left wheel | ||
| */ | ||
| enum WheelIndex : std::size_t | ||
| { | ||
| FRONT_LEFT = 0, | ||
| FRONT_RIGHT = 1, | ||
| REAR_RIGHT = 2, | ||
| REAR_LEFT = 3 | ||
| }; | ||
|
|
||
| /** | ||
| * Internal lists with joint names sorted as in `WheelIndex` enum. | ||
| */ | ||
| std::vector<std::string> command_joint_names_; | ||
|
|
||
| /** | ||
| * Internal lists with joint names sorted as in `WheelIndex` enum. | ||
| * If parameters for state joint names are *not* defined, this list is the same as | ||
| * `command_joint_names_`. | ||
| */ | ||
| std::vector<std::string> state_joint_names_; | ||
|
|
||
| // Names of the references, ex: high level vel commands from MoveIt, Nav2, etc. | ||
| // used for preceding controller | ||
| std::vector<std::string> reference_names_; | ||
|
|
||
| // Command subscribers and Controller State, odom state, tf state publishers | ||
| rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr; | ||
| realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> input_ref_; | ||
| rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); | ||
|
|
||
| using OdomStatePublisher = realtime_tools::RealtimePublisher<OdomStateMsg>; | ||
| rclcpp::Publisher<OdomStateMsg>::SharedPtr odom_s_publisher_; | ||
| std::unique_ptr<OdomStatePublisher> rt_odom_state_publisher_; | ||
|
|
||
| using TfStatePublisher = realtime_tools::RealtimePublisher<TfStateMsg>; | ||
| rclcpp::Publisher<TfStateMsg>::SharedPtr tf_odom_s_publisher_; | ||
| std::unique_ptr<TfStatePublisher> rt_tf_odom_state_publisher_; | ||
|
|
||
| using ControllerStatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>; | ||
| rclcpp::Publisher<ControllerStateMsg>::SharedPtr controller_s_publisher_; | ||
| std::unique_ptr<ControllerStatePublisher> controller_state_publisher_; | ||
|
|
||
| // override methods from ChainableControllerInterface | ||
| std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override; | ||
|
|
||
| bool on_set_chained_mode(bool chained_mode) override; | ||
|
|
||
| Odometry odometry_; | ||
|
|
||
| private: | ||
| // callback for topic interface | ||
| MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL | ||
| void reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg); | ||
|
|
||
| double velocity_in_center_frame_linear_x_; // [m/s] | ||
| double velocity_in_center_frame_linear_y_; // [m/s] | ||
| double velocity_in_center_frame_angular_z_; // [rad/s] | ||
| }; | ||
|
|
||
| } // namespace mecanum_drive_controller | ||
|
|
||
| #endif // MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ |
104 changes: 104 additions & 0 deletions
104
mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,104 @@ | ||
| // Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #ifndef MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ | ||
| #define MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ | ||
|
|
||
| #include "geometry_msgs/msg/twist.hpp" | ||
| #include "realtime_tools/realtime_buffer.hpp" | ||
| #include "realtime_tools/realtime_publisher.hpp" | ||
|
|
||
| #define PLANAR_POINT_DIM 3 | ||
|
|
||
| namespace mecanum_drive_controller | ||
| { | ||
| /// \brief The Odometry class handles odometry readings | ||
| /// (2D pose and velocity with related timestamp) | ||
| class Odometry | ||
| { | ||
| public: | ||
| /// Integration function, used to integrate the odometry: | ||
| typedef std::function<void(double, double, double)> IntegrationFunction; | ||
|
|
||
| /// \brief Constructor | ||
| /// Timestamp will get the current time value | ||
| /// Value will be set to zero | ||
| Odometry(); | ||
|
|
||
| /// \brief Initialize the odometry | ||
| /// \param time Current time | ||
| void init(const rclcpp::Time & time, std::array<double, PLANAR_POINT_DIM> base_frame_offset); | ||
|
|
||
| /// \brief Updates the odometry class with latest wheels position | ||
| /// \param wheel_front_left_vel Wheel velocity [rad/s] | ||
| /// \param wheel_rear_left_vel Wheel velocity [rad/s] | ||
| /// \param wheel_rear_right_vel Wheel velocity [rad/s] | ||
| /// \param wheel_front_right_vel Wheel velocity [rad/s] | ||
| /// \param time Current time | ||
| /// \return true if the odometry is actually updated | ||
| bool update( | ||
| const double wheel_front_left_vel, const double wheel_rear_left_vel, | ||
| const double wheel_rear_right_vel, const double wheel_front_right_vel, const double dt); | ||
|
|
||
| /// \return position (x component) [m] | ||
| double getX() const { return position_x_in_base_frame_; } | ||
| /// \return position (y component) [m] | ||
| double getY() const { return position_y_in_base_frame_; } | ||
| /// \return orientation (z component) [m] | ||
| double getRz() const { return orientation_z_in_base_frame_; } | ||
| /// \return body velocity of the base frame (linear x component) [m/s] | ||
| double getVx() const { return velocity_in_base_frame_linear_x; } | ||
| /// \return body velocity of the base frame (linear y component) [m/s] | ||
| double getVy() const { return velocity_in_base_frame_linear_y; } | ||
| /// \return body velocity of the base frame (angular z component) [m/s] | ||
| double getWz() const | ||
| { | ||
| return velocity_in_base_frame_angular_z; | ||
| ; | ||
| } | ||
|
|
||
| /// \brief Sets the wheels parameters: mecanum geometric param and radius | ||
| /// \param sum_of_robot_center_projection_on_X_Y_axis Wheels geometric param | ||
| /// (used in mecanum wheels' ik) [m] | ||
| /// \param wheels_radius Wheels radius [m] | ||
| void setWheelsParams( | ||
| const double sum_of_robot_center_projection_on_X_Y_axis, const double wheels_radius); | ||
|
|
||
| private: | ||
| /// Current timestamp: | ||
| rclcpp::Time timestamp_; | ||
|
|
||
| /// Reference frame (wrt to center frame). [x, y, theta] | ||
| std::array<double, PLANAR_POINT_DIM> base_frame_offset_; | ||
|
|
||
| /// Current pose: | ||
| double position_x_in_base_frame_; // [m] | ||
| double position_y_in_base_frame_; // [m] | ||
| double orientation_z_in_base_frame_; // [rad] | ||
|
|
||
| double velocity_in_base_frame_linear_x; // [m/s] | ||
| double velocity_in_base_frame_linear_y; // [m/s] | ||
| double velocity_in_base_frame_angular_z; // [rad/s] | ||
|
|
||
| /// Wheels kinematic parameters [m]: | ||
| /// lx and ly represent the distance from the robot's center to the wheels | ||
| /// projected on the x and y axis with origin at robots center respectively, | ||
| /// sum_of_robot_center_projection_on_X_Y_axis_ = lx+ly | ||
| double sum_of_robot_center_projection_on_X_Y_axis_; | ||
| double wheels_radius_; // [m] | ||
| }; | ||
|
|
||
| } // namespace mecanum_drive_controller | ||
|
|
||
| #endif // MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ */ |
Oops, something went wrong.