forked from ros-controls/ros2_controllers
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.
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
mecanum drive controller adding and ported to ros2 #4
Closed
Closed
Changes from all commits
Commits
Show all changes
48 commits
Select commit
Hold shift + click to select a range
27ed35d
compiling without error
Robotgir d327065
all tests working
Robotgir 6adb982
reorganized publishers added code to print values for debug
Robotgir 1d5f5e3
added meaning full param values
Robotgir ebe3525
next line in comment
Robotgir d2db9e4
Automated formatting files.
destogl 3957f77
Manual changes of formatting.
destogl 34845d5
Delete LICENSE
destogl a08a307
Apply suggestions from code review
Robotgir 8f4d6a6
Apply suggestions from code review
Robotgir 9d78f16
used the suggested structure
Robotgir 8e6f7a6
general changes
Robotgir 6e56813
removed bacc accumulators
Robotgir 1e99467
renaming to more meaningful and readable variable names
Robotgir 0f75e63
code review suggestions implementing
Robotgir dbc770f
Apply suggestions from code review
Robotgir 23ef0ce
added description
Robotgir c3b211f
checked more values against the behavior of controller methods
Robotgir a535f3b
changed controller type to CtrlType
Robotgir 4b8e759
Update mecanum_drive_controller/test/test_mecanum_drive_controller.cpp
destogl 57a96ee
updated cmakelists structure
Robotgir c1fd834
renaming variables
Robotgir 6aa2dde
changed description
Robotgir dfb960f
resetting msg if msg is valid after receiving a to old msg
Robotgir 2c23123
estructuring parameters in yaml
Robotgir 2382c10
comparing float values
Robotgir f58d12e
Apply suggestions from code review
Robotgir b8c5f48
added description for each test as comment
Robotgir 100b3c4
added preceding controller tests
Robotgir 4638622
reorganized reference_interfaces configuration method
Robotgir 5db9123
configuration of interfaces in loop
Robotgir db9482c
final fixups
Robotgir 163b95a
using control_msgs::msg::MecanumDriveControllerState and removing cus…
Robotgir b3cddbb
using control_msgs::msg::MecanumDriveControllerState and removing cus…
Robotgir 32cd3b6
added chained mode test for update logic
Robotgir e71d025
changed wheel vl names to more descriptive
Robotgir 622e60d
renamed rot and traslation matrix variable names
Robotgir 8c5062e
renamed vl variable to more descriptive
Robotgir 85278e9
ran pre-commit formatter
Robotgir dc77a31
set ref_itfces to 0 to stop vehicle in case of timeout
Robotgir 4c11687
added description
Robotgir 15c2430
check nan
Robotgir ce10fde
odom vel naming descriptive
Robotgir 976f64c
removed outdated nomenclature
Robotgir eadabde
add comment
Robotgir 8935e78
Apply suggestions from code review
destogl b77ee49
Update mecanum_drive_controller/src/mecanum_drive_controller.cpp
destogl a22266c
Update mecanum_drive_controller/src/mecanum_drive_controller.cpp
destogl File filter
Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
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
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,112 @@ | ||
cmake_minimum_required(VERSION 3.8) | ||
project(mecanum_drive_controller) | ||
|
||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") | ||
add_compile_options(-Wall -Wextra -Wpedantic) | ||
endif() | ||
|
||
# find dependencies | ||
set(THIS_PACKAGE_INCLUDE_DEPENDS | ||
control_msgs | ||
controller_interface | ||
geometry_msgs | ||
hardware_interface | ||
nav_msgs | ||
pluginlib | ||
rclcpp | ||
rclcpp_lifecycle | ||
realtime_tools | ||
std_srvs | ||
tf2 | ||
tf2_msgs | ||
tf2_geometry_msgs | ||
) | ||
|
||
find_package(ament_cmake REQUIRED) | ||
find_package(generate_parameter_library REQUIRED) | ||
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) | ||
find_package(${Dependency} REQUIRED) | ||
endforeach() | ||
|
||
# Add mecanum_drive_controller library related compile commands | ||
generate_parameter_library(mecanum_drive_controller_parameters | ||
src/mecanum_drive_controller.yaml | ||
) | ||
add_library( | ||
mecanum_drive_controller | ||
SHARED | ||
src/mecanum_drive_controller.cpp | ||
src/odometry.cpp | ||
) | ||
target_include_directories(mecanum_drive_controller PUBLIC | ||
"$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>" | ||
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>") | ||
target_link_libraries(mecanum_drive_controller mecanum_drive_controller_parameters) | ||
ament_target_dependencies(mecanum_drive_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) | ||
target_compile_definitions(mecanum_drive_controller PRIVATE "MECANUM_DRIVE_CONTROLLER_BUILDING_DLL") | ||
|
||
pluginlib_export_plugin_description_file( | ||
controller_interface mecanum_drive_controller.xml) | ||
|
||
install( | ||
TARGETS | ||
mecanum_drive_controller | ||
RUNTIME DESTINATION bin | ||
ARCHIVE DESTINATION lib | ||
LIBRARY DESTINATION lib | ||
) | ||
|
||
install( | ||
DIRECTORY include/ | ||
DESTINATION include | ||
destogl marked this conversation as resolved.
Show resolved
Hide resolved
|
||
) | ||
|
||
if(BUILD_TESTING) | ||
find_package(ament_cmake_gmock REQUIRED) | ||
find_package(controller_manager REQUIRED) | ||
find_package(hardware_interface REQUIRED) | ||
find_package(ros2_control_test_assets REQUIRED) | ||
|
||
ament_add_gmock(test_load_mecanum_drive_controller test/test_load_mecanum_drive_controller.cpp) | ||
target_include_directories(test_load_mecanum_drive_controller PRIVATE include) | ||
ament_target_dependencies( | ||
test_load_mecanum_drive_controller | ||
controller_manager | ||
hardware_interface | ||
ros2_control_test_assets | ||
) | ||
|
||
add_rostest_with_parameters_gmock( | ||
test_mecanum_drive_controller test/test_mecanum_drive_controller.cpp | ||
${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_params.yaml) | ||
target_include_directories(test_mecanum_drive_controller PRIVATE include) | ||
target_link_libraries(test_mecanum_drive_controller mecanum_drive_controller) | ||
ament_target_dependencies( | ||
test_mecanum_drive_controller | ||
controller_interface | ||
hardware_interface | ||
) | ||
|
||
add_rostest_with_parameters_gmock( | ||
test_mecanum_drive_controller_preceeding test/test_mecanum_drive_controller_preceeding.cpp | ||
${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_preceeding_params.yaml) | ||
target_include_directories(test_mecanum_drive_controller_preceeding PRIVATE include) | ||
target_link_libraries(test_mecanum_drive_controller_preceeding mecanum_drive_controller) | ||
ament_target_dependencies( | ||
test_mecanum_drive_controller_preceeding | ||
controller_interface | ||
hardware_interface | ||
) | ||
endif() | ||
|
||
ament_export_include_directories( | ||
include | ||
) | ||
ament_export_dependencies( | ||
${THIS_PACKAGE_INCLUDE_DEPENDS} | ||
) | ||
ament_export_libraries( | ||
mecanum_drive_controller | ||
) | ||
|
||
ament_package() |
137 changes: 137 additions & 0 deletions
137
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,137 @@ | ||
// 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.h" | ||
#include "realtime_tools/realtime_publisher.h" | ||
#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_; | ||
|
||
// used for chained controller | ||
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_ |
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@Robotgir please check that is change the same as ros-controls#510
Just to be fully aligned later.