Add Mecanum Drive Controller #512

destogl · 2023-01-27T23:01:48Z

.pre-commit-config.yaml

codecov-commenter · 2023-01-27T23:10:40Z

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 30.88%. Comparing base (0b43291) to head (f6de20a).
Report is 33 commits behind head on master.

❗ Current head f6de20a differs from pull request most recent head 3da18a8

Please upload reports for the commit 3da18a8 to get more accurate results.

Additional details and impacted files

@@             Coverage Diff             @@
##           master     #512       +/-   ##
===========================================
- Coverage   71.86%   30.88%   -40.98%     
===========================================
  Files          41        7       -34     
  Lines        3650      832     -2818     
  Branches     1794      505     -1289     
===========================================
- Hits         2623      257     -2366     
+ Misses        707      133      -574     
- Partials      320      442      +122

Flag	Coverage Δ
unittests	`30.88% <ø> (-40.98%)`	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

see 43 files with indirect coverage changes

mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp

mecanum_drive_controller/test/test_mecanum_drive_controller.cpp

admittance_controller/include/admittance_controller/admittance_rule.hpp

mecanum_drive_controller/test/mecanum_drive_controller_params.yaml

mecanum_drive_controller/mecanum_drive_controller.xml

mecanum_drive_controller/package.xml

mecanum_drive_controller/src/mecanum_drive_controller.cpp

mecanum_drive_controller/src/mecanum_drive_controller.yaml

mergify · 2023-03-28T18:47:21Z

This pull request is in conflict. Could you fix it @destogl?

mecanum_drive_controller/test/test_mecanum_drive_controller.cpp

mecanum_drive_controller/CMakeLists.txt

mecanum_drive_controller/src/mecanum_drive_controller.cpp

mecanum_drive_controller/test/test_mecanum_drive_controller.cpp

christophfroehlich

LGTM, suggestions only for small changes like typos etc.

mecanum_drive_controller/src/mecanum_drive_controller.yaml

mecanum_drive_controller/src/odometry.cpp

mecanum_drive_controller/doc/userdoc.rst

mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp

mergify · 2023-07-27T11:14:56Z

This pull request is in conflict. Could you fix it @destogl?

luis-camero · 2024-07-15T16:31:19Z

@christophfroehlich, does the WaitSet issue in the test framework need to be resolved before this PR can be merged? Bypassing the subscriber/publisher results in all tests passing.

@bmagyar would you accept a PR to Humble with the same "hack" to support Twist and TwistStamped as in the Diff. Drive Controller in #143.

luis-camero · 2024-07-22T15:34:50Z

@destogl could you take a look at the PR I made to your fork. I think there's an issue with the odometry topic.

Pratham-Pandey · 2024-07-23T01:17:31Z

@luis-camero is odom frame available in your case in rviz? In my case, the odom frame is not even listed in rviz Fixed Frame dropdown menu. Could you share the controller yaml file and the URDF?

luis-camero · 2024-07-23T20:40:13Z

@luis-camero is odom frame available in your case in rviz? In my case, the odom frame is not even listed in rviz Fixed Frame dropdown menu. Could you share the controller yaml file and the URDF?

@Pratham-Pandey, take a look at the PR I made.

HopeCollector · 2024-07-30T02:46:44Z

mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp

+
+TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set)
+{
+  SetUpController();


Setting up the controller at the beginning will cause the test to fail later because the controller parameter is already configured in the function, according to my test, moving it to line 46, the test works fine.

It looks like I opened a review by mistake, feel free to delete the review I submitted if needed!

* Add hardware interface testing dependency * Use orientation w.r.t. odom to set position

bmagyar · 2024-07-31T21:04:58Z

@destogl I think there were a few API breaks since those tests were written ;)

Pratham-Pandey · 2024-08-07T15:36:04Z

Can this controller be used to move to a specified coordinate or do i need to implement a separate controller (for example a PID controller) to do this?

christophfroehlich · 2024-08-07T20:19:58Z

Can this controller be used to move to a specified coordinate or do i need to implement a separate controller (for example a PID controller) to do this?

Usually, there is some navigation task involved. See the nav2 docs, the nav2 controllers publish a stamped twist as far as I understood, which you can then feed into this ros2_controller.

mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp

mecanum_drive_controller/test/test_mecanum_drive_controller.hpp

christophfroehlich

And some more: StoglRobotics-forks#23

@destogl but when_reference_msg_received_expect_updated_commands_and_status_message still fails

* Fix tests * Fix format

Pratham-Pandey · 2024-11-01T13:39:33Z

I am unable to compile it successfully. Others experiencing the same?

Sequence of steps:

git clone https://github.com/StoglRobotics-forks/ros2_controllers.git
cd ros2_controllers/
git checkout add-mecanum-drive-controller
cd ../..
colcon build --symlink-install

Error Log:

Starting >>> forward_command_controller
Starting >>> steering_controllers_library
Starting >>> joint_trajectory_controller
Starting >>> diff_drive_controller
[Processing: diff_drive_controller, forward_command_controller, joint_trajectory_controller, steering_controllers_library]             
[Processing: diff_drive_controller, forward_command_controller, joint_trajectory_controller, steering_controllers_library]                         
--- stderr: joint_trajectory_controller                                                                                                            
/home/user_2/files/general/update_cont/src/ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp: In member function ‘virtual controller_interface::CallbackReturn joint_trajectory_controller::JointTrajectoryController::on_init()’:
/home/user_2/files/general/update_cont/src/ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp:58:30: error: ‘get_robot_description’ was not declared in this scope; did you mean ‘goal_event_descriptions’?
   58 |   const std::string & urdf = get_robot_description();
      |                              ^~~~~~~~~~~~~~~~~~~~~
      |                              goal_event_descriptions
/home/user_2/files/general/update_cont/src/ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp: In member function ‘virtual controller_interface::return_type joint_trajectory_controller::JointTrajectoryController::update(const rclcpp::Time&, const rclcpp::Duration&)’:
/home/user_2/files/general/update_cont/src/ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp:140:7: error: ‘get_lifecycle_state’ was not declared in this scope; did you mean ‘rcl_lifecycle_state_t’?
  140 |   if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
      |       ^~~~~~~~~~~~~~~~~~~
      |       rcl_lifecycle_state_t
/home/user_2/files/general/update_cont/src/ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp: In member function ‘void joint_trajectory_controller::JointTrajectoryController::query_state_service(std::shared_ptr<control_msgs::srv::QueryTrajectoryState_Request_<std::allocator<void> > >, std::shared_ptr<control_msgs::srv::QueryTrajectoryState_Response_<std::allocator<void> > >)’:
/home/user_2/files/general/update_cont/src/ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp:596:7: error: ‘get_lifecycle_state’ was not declared in this scope; did you mean ‘rcl_lifecycle_state_t’?
  596 |   if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
      |       ^~~~~~~~~~~~~~~~~~~
      |       rcl_lifecycle_state_t
/home/user_2/files/general/update_cont/src/ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp: In member function ‘rclcpp_action::GoalResponse joint_trajectory_controller::JointTrajectoryController::goal_received_callback(const GoalUUID&, std::shared_ptr<const control_msgs::action::FollowJointTrajectory_Goal_<std::allocator<void> > >)’:
/home/user_2/files/general/update_cont/src/ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp:1115:7: error: ‘get_lifecycle_state’ was not declared in this scope; did you mean ‘rcl_lifecycle_state_t’?
 1115 |   if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
      |       ^~~~~~~~~~~~~~~~~~~
      |       rcl_lifecycle_state_t
gmake[2]: *** [CMakeFiles/joint_trajectory_controller.dir/build.make:76: CMakeFiles/joint_trajectory_controller.dir/src/joint_trajectory_controller.cpp.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:212: CMakeFiles/joint_trajectory_controller.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< joint_trajectory_controller [1min 7s, exited with code 2]
Aborted  <<< diff_drive_controller [1min 7s]                                                             
Aborted  <<< steering_controllers_library [1min 34s]                                                       
Aborted  <<< forward_command_controller [1min 43s]                                         

Summary: 0 packages finished [1min 44s]
  1 package failed: joint_trajectory_controller
  3 packages aborted: diff_drive_controller forward_command_controller steering_controllers_library
  4 packages had stderr output: diff_drive_controller forward_command_controller joint_trajectory_controller steering_controllers_library
  19 packages not processed

christophfroehlich · 2024-11-01T15:35:24Z

Do you have ros2_control from jazzy or rolling? The API is not backwards compatible.

Pratham-Pandey · 2024-11-01T16:15:06Z

Do you have ros2_control from jazzy or rolling? The API is not backwards compatible.

I am running Ros2 Iron. Does ros2_control has separate version? how do i check it?

christophfroehlich · 2024-11-01T18:15:14Z

I am running Ros2 Iron. Does ros2_control has separate version? how do i check it?

Sure, there are different versions of ros2_control for different distros. You can compile the development version (which is rolling) from source on iron, see the docs.

Pratham-Pandey · 2024-11-02T08:28:25Z

@christophfroehlich after installing ros2_control development version successfully, i re-executed the steps described above in my previous comment but the following error occurs while building. Are you able to build this PR on your machine?:

[0.993s] WARNING:colcon.colcon_ros.prefix_path.catkin:The path '/home/user_2/files/general/update_cont/install/test_bot' in the environment variable CMAKE_PREFIX_PATH doesn't exist
[1.119s] WARNING:colcon.colcon_core.package_selection:Some selected packages are already built in one or more underlay workspaces:
	'joint_trajectory_controller' is in: /home/user_2/files/general/ros2_control_dev/install/joint_trajectory_controller, /opt/ros/iron
	'joint_state_broadcaster' is in: /home/user_2/files/general/ros2_control_dev/install/joint_state_broadcaster, /opt/ros/iron
	'effort_controllers' is in: /home/user_2/files/general/ros2_control_dev/install/effort_controllers, /opt/ros/iron
	'tricycle_controller' is in: /home/user_2/files/general/ros2_control_dev/install/tricycle_controller, /opt/ros/iron
	'imu_sensor_broadcaster' is in: /home/user_2/files/general/ros2_control_dev/install/imu_sensor_broadcaster, /opt/ros/iron
	'velocity_controllers' is in: /home/user_2/files/general/ros2_control_dev/install/velocity_controllers, /opt/ros/iron
	'diff_drive_controller' is in: /home/user_2/files/general/ros2_control_dev/install/diff_drive_controller, /opt/ros/iron
If a package in a merged underlay workspace is overridden and it installs headers, then all packages in the overlay must sort their include directories by workspace order. Failure to do so may result in build failures or undefined behavior at run time.
If the overridden package is used by another package in any underlay, then the overriding package in the overlay must be API and ABI compatible or undefined behavior at run time may occur.

If you understand the risks and want to override a package anyways, add the following to the command line:
	--allow-overriding diff_drive_controller effort_controllers imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller tricycle_controller velocity_controllers

This may be promoted to an error in a future release of colcon-override-check.
Starting >>> forward_command_controller
Starting >>> steering_controllers_library
Starting >>> joint_trajectory_controller
Starting >>> diff_drive_controller
[Processing: diff_drive_controller, forward_command_controller, joint_trajectory_controller, steering_controllers_library]
[Processing: diff_drive_controller, forward_command_controller, joint_trajectory_controller, steering_controllers_library]
[Processing: diff_drive_controller, forward_command_controller, joint_trajectory_controller, steering_controllers_library]
--- stderr: steering_controllers_library    
In file included from /opt/ros/iron/src/gtest_vendor/include/gtest/gtest.h:375,
                 from /opt/ros/iron/src/gmock_vendor/include/gmock/internal/gmock-internal-utils.h:47,
                 from /opt/ros/iron/src/gmock_vendor/include/gmock/gmock-actions.h:51,
                 from /opt/ros/iron/src/gmock_vendor/include/gmock/gmock.h:59,
                 from /home/user_2/files/general/update_cont/src/ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.hpp:18,
                 from /home/user_2/files/general/update_cont/src/ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.cpp:15:
/home/user_2/files/general/update_cont/src/ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.cpp: In member function ‘virtual void SteeringControllersLibraryTest_check_exported_interfaces_Test::TestBody()’:
/home/user_2/files/general/update_cont/src/ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.cpp:73:39: error: ‘__gnu_cxx::__alloc_traits<std::allocator<std::shared_ptr<hardware_interface::CommandInterface> >, std::shared_ptr<hardware_interface::CommandInterface> >::value_type’ {aka ‘class std::shared_ptr<hardware_interface::CommandInterface>’} has no member named ‘get_name’
   73 |     EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name);
      |                                       ^~~~~~~~
/opt/ros/iron/src/gtest_vendor/include/gtest/gtest_pred_impl.h:77:52: note: in definition of macro ‘GTEST_ASSERT_’
   77 |   if (const ::testing::AssertionResult gtest_ar = (expression)) \
      |                                                    ^~~~~~~~~~
/opt/ros/iron/src/gtest_vendor/include/gtest/gtest_pred_impl.h:164:3: note: in expansion of macro ‘GTEST_PRED_FORMAT2_’
  164 |   GTEST_PRED_FORMAT2_(pred_format, v1, v2, GTEST_NONFATAL_FAILURE_)
      |   ^~~~~~~~~~~~~~~~~~~
/opt/ros/iron/src/gtest_vendor/include/gtest/gtest.h:2028:3: note: in expansion of macro ‘EXPECT_PRED_FORMAT2’
 2028 |   EXPECT_PRED_FORMAT2(::testing::internal::EqHelper::Compare, val1, val2)
      |   ^~~~~~~~~~~~~~~~~~~
/home/user_2/files/general/update_cont/src/ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.cpp:73:5: note: in expansion of macro ‘EXPECT_EQ’
   73 |     EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name);
      |     ^~~~~~~~~
/home/user_2/files/general/update_cont/src/ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.cpp:74:39: error: ‘__gnu_cxx::__alloc_traits<std::allocator<std::shared_ptr<hardware_interface::CommandInterface> >, std::shared_ptr<hardware_interface::CommandInterface> >::value_type’ {aka ‘class std::shared_ptr<hardware_interface::CommandInterface>’} has no member named ‘get_prefix_name’
   74 |     EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name());
      |                                       ^~~~~~~~~~~~~~~
/opt/ros/iron/src/gtest_vendor/include/gtest/gtest_pred_impl.h:77:52: note: in definition of macro ‘GTEST_ASSERT_’
   77 |   if (const ::testing::AssertionResult gtest_ar = (expression)) \
      |                                                    ^~~~~~~~~~
/opt/ros/iron/src/gtest_vendor/include/gtest/gtest_pred_impl.h:164:3: note: in expansion of macro ‘GTEST_PRED_FORMAT2_’
  164 |   GTEST_PRED_FORMAT2_(pred_format, v1, v2, GTEST_NONFATAL_FAILURE_)
      |   ^~~~~~~~~~~~~~~~~~~
/opt/ros/iron/src/gtest_vendor/include/gtest/gtest.h:2028:3: note: in expansion of macro ‘EXPECT_PRED_FORMAT2’
 2028 |   EXPECT_PRED_FORMAT2(::testing::internal::EqHelper::Compare, val1, val2)
      |   ^~~~~~~~~~~~~~~~~~~
/home/user_2/files/general/update_cont/src/ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.cpp:74:5: note: in expansion of macro ‘EXPECT_EQ’
   74 |     EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name());
      |     ^~~~~~~~~
/home/user_2/files/general/update_cont/src/ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.cpp:75:39: error: ‘__gnu_cxx::__alloc_traits<std::allocator<std::shared_ptr<hardware_interface::CommandInterface> >, std::shared_ptr<hardware_interface::CommandInterface> >::value_type’ {aka ‘class std::shared_ptr<hardware_interface::CommandInterface>’} has no member named ‘get_interface_name’
   75 |     EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]);
      |                                       ^~~~~~~~~~~~~~~~~~
/opt/ros/iron/src/gtest_vendor/include/gtest/gtest_pred_impl.h:77:52: note: in definition of macro ‘GTEST_ASSERT_’
   77 |   if (const ::testing::AssertionResult gtest_ar = (expression)) \
      |                                                    ^~~~~~~~~~
/opt/ros/iron/src/gtest_vendor/include/gtest/gtest_pred_impl.h:164:3: note: in expansion of macro ‘GTEST_PRED_FORMAT2_’
  164 |   GTEST_PRED_FORMAT2_(pred_format, v1, v2, GTEST_NONFATAL_FAILURE_)
      |   ^~~~~~~~~~~~~~~~~~~
/opt/ros/iron/src/gtest_vendor/include/gtest/gtest.h:2028:3: note: in expansion of macro ‘EXPECT_PRED_FORMAT2’
 2028 |   EXPECT_PRED_FORMAT2(::testing::internal::EqHelper::Compare, val1, val2)
      |   ^~~~~~~~~~~~~~~~~~~
/home/user_2/files/general/update_cont/src/ros2_controllers/steering_controllers_library/test/test_steering_controllers_library.cpp:75:5: note: in expansion of macro ‘EXPECT_EQ’
   75 |     EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]);
      |     ^~~~~~~~~
gmake[2]: *** [CMakeFiles/test_steering_controllers_library.dir/build.make:76: CMakeFiles/test_steering_controllers_library.dir/test/test_steering_controllers_library.cpp.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:234: CMakeFiles/test_steering_controllers_library.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< steering_controllers_library [1min 43s, exited with code 2]
Aborted  <<< diff_drive_controller [2min 16s]          
Aborted  <<< forward_command_controller [2min 39s]                                                          
Aborted  <<< joint_trajectory_controller [3min 32s]                                          

Summary: 0 packages finished [3min 33s]
  1 package failed: steering_controllers_library
  3 packages aborted: diff_drive_controller forward_command_controller joint_trajectory_controller
  1 package had stderr output: steering_controllers_library
  19 packages not processed

christophfroehlich · 2024-11-02T12:56:19Z

you are right, try StoglRobotics-forks#26
👀 @destogl

Pratham-Pandey · 2024-11-02T16:46:42Z

you are right, try StoglRobotics-forks#26 👀 @destogl

Working 👍

destogl force-pushed the add-mecanum-drive-controller branch from a22266c to 002b436 Compare January 27, 2023 23:01

github-actions bot requested review from anfemosa, bmagyar, erickisos, livanov93, mhubii and VX792 January 27, 2023 23:02

destogl commented Jan 27, 2023

View reviewed changes

.pre-commit-config.yaml Outdated Show resolved Hide resolved

destogl mentioned this pull request Jan 28, 2023

Add state message for mechanum controller ros-controls/control_msgs#79

Merged

destogl commented Jan 31, 2023

View reviewed changes

mecanum_drive_controller/test/test_mecanum_drive_controller.cpp Show resolved Hide resolved

mecanum_drive_controller/test/test_mecanum_drive_controller.cpp Outdated Show resolved Hide resolved

mecanum_drive_controller/test/test_mecanum_drive_controller.cpp Outdated Show resolved Hide resolved

destogl force-pushed the add-mecanum-drive-controller branch from 9b036ae to 43b183f Compare February 8, 2023 16:36

destogl commented Feb 8, 2023

View reviewed changes

admittance_controller/include/admittance_controller/admittance_rule.hpp Outdated Show resolved Hide resolved

mecanum_drive_controller/test/mecanum_drive_controller_params.yaml Outdated Show resolved Hide resolved

destogl commented Feb 13, 2023

View reviewed changes

mecanum_drive_controller/mecanum_drive_controller.xml Outdated Show resolved Hide resolved

destogl commented Feb 13, 2023

View reviewed changes

mecanum_drive_controller/package.xml Show resolved Hide resolved

destogl commented Feb 13, 2023

View reviewed changes

mecanum_drive_controller/src/mecanum_drive_controller.cpp Outdated Show resolved Hide resolved

destogl commented Feb 13, 2023

View reviewed changes

mecanum_drive_controller/src/mecanum_drive_controller.yaml Outdated Show resolved Hide resolved

destogl force-pushed the add-mecanum-drive-controller branch from 80df10c to 936bb9e Compare February 22, 2023 16:48

destogl force-pushed the add-mecanum-drive-controller branch from 936bb9e to 48415bf Compare March 8, 2023 18:45

erickisos approved these changes Mar 16, 2023

View reviewed changes