Skip to content
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

Add Mecanum Drive Controller #512

Open
wants to merge 97 commits into
base: master
Choose a base branch
from

Conversation

destogl
Copy link
Member

@destogl destogl commented Jan 27, 2023

.pre-commit-config.yaml Outdated Show resolved Hide resolved
@codecov-commenter
Copy link

codecov-commenter commented Jan 27, 2023

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

@destogl destogl force-pushed the add-mecanum-drive-controller branch from 9b036ae to 43b183f Compare February 8, 2023 16:36
@destogl destogl force-pushed the add-mecanum-drive-controller branch from 936bb9e to 48415bf Compare March 8, 2023 18:45
@mergify
Copy link
Contributor

mergify bot commented Mar 28, 2023

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

Copy link
Contributor

@christophfroehlich christophfroehlich left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM, suggestions only for small changes like typos etc.

mecanum_drive_controller/src/mecanum_drive_controller.yaml Outdated Show resolved Hide resolved
mecanum_drive_controller/src/mecanum_drive_controller.yaml Outdated Show resolved Hide resolved
mecanum_drive_controller/src/mecanum_drive_controller.yaml Outdated Show resolved Hide resolved
mecanum_drive_controller/src/odometry.cpp Show resolved Hide resolved
mecanum_drive_controller/src/odometry.cpp Outdated Show resolved Hide resolved
mecanum_drive_controller/doc/userdoc.rst Outdated Show resolved Hide resolved
mecanum_drive_controller/doc/userdoc.rst Outdated Show resolved Hide resolved
mecanum_drive_controller/doc/userdoc.rst Outdated Show resolved Hide resolved
@mergify
Copy link
Contributor

mergify bot commented Jul 27, 2023

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

@luis-camero
Copy link

@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
Copy link

@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
Copy link

@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.


TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set)
{
SetUpController();

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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
@destogl destogl added backport-humble This label should be used by maintaines only! Label triggers PR backport to ROS2 humble. backport-iron This label should be used by maintaines only! Label triggers PR backport to ROS2 Iron. labels Jul 31, 2024
@bmagyar
Copy link
Member

bmagyar commented Jul 31, 2024

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

@Pratham-Pandey
Copy link

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
Copy link
Contributor

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.

Copy link
Contributor

@christophfroehlich christophfroehlich left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

And some more: StoglRobotics-forks#23

@destogl but when_reference_msg_received_expect_updated_commands_and_status_message still fails

@Pratham-Pandey
Copy link

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
Copy link
Contributor

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

@Pratham-Pandey
Copy link

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
Copy link
Contributor

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
Copy link

@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
Copy link
Contributor

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

@Pratham-Pandey
Copy link

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

Working 👍

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
backport-humble This label should be used by maintaines only! Label triggers PR backport to ROS2 humble. backport-iron This label should be used by maintaines only! Label triggers PR backport to ROS2 Iron.
Projects
None yet
Development

Successfully merging this pull request may close these issues.