From 001d896e73297a4913999da47449dd835e8c8b9e Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sat, 23 Jul 2022 10:07:17 -0700 Subject: [PATCH 001/126] Parameter loading fixup in diff_drive and gripper controllers (#385) * Parameter loading fixup * Lint --- diff_drive_controller/src/diff_drive_controller.cpp | 7 ++++--- .../gripper_controllers/gripper_action_controller_impl.hpp | 2 +- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index bc2e7eed22..aec603ad22 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -77,9 +77,10 @@ controller_interface::CallbackReturn DiffDriveController::on_init() auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); - auto_declare("publish_limited_velocity", publish_limited_velocity_); + publish_limited_velocity_ = auto_declare("publish_limited_velocity", + publish_limited_velocity_); auto_declare("velocity_rolling_window_size", 10); - auto_declare("use_stamped_vel", use_stamped_vel_); + use_stamped_vel_ = auto_declare("use_stamped_vel", use_stamped_vel_); auto_declare("linear.x.has_velocity_limits", false); auto_declare("linear.x.has_acceleration_limits", false); @@ -100,7 +101,7 @@ controller_interface::CallbackReturn DiffDriveController::on_init() auto_declare("angular.z.min_acceleration", NAN); auto_declare("angular.z.max_jerk", NAN); auto_declare("angular.z.min_jerk", NAN); - auto_declare("publish_rate", publish_rate_); + publish_rate_ = auto_declare("publish_rate", publish_rate_); } catch (const std::exception & e) { diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index a92fc9d2a7..f1d270b52f 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -43,7 +43,7 @@ controller_interface::CallbackReturn GripperActionController: { // with the lifecycle node being initialized, we can declare parameters auto_declare("action_monitor_rate", 20.0); - auto_declare("joint", joint_name_); + joint_name_ = auto_declare("joint", joint_name_); auto_declare("goal_tolerance", 0.01); auto_declare("max_effort", 0.0); auto_declare("allow_stalling", false); From d8d661607f770e704b774249a31534fa0eb0d46b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 24 Jul 2022 08:43:40 +0200 Subject: [PATCH 002/126] Use system time in all tests to avoid error with different time sources. (#334) --- .../test/test_trajectory_controller.cpp | 22 ++++++++++--------- .../test/test_trajectory_controller_utils.hpp | 2 +- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 57b0a36251..e4be504717 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -78,7 +78,7 @@ TEST_P(TrajectoryControllerTestParameterized, configure) time_from_start.sec = 1; time_from_start.nanosec = 0; std::vector> points{{{3.3, 4.4, 5.5}}}; - publish(time_from_start, points); + publish(time_from_start, points, rclcpp::Time()); std::this_thread::sleep_for(std::chrono::milliseconds(10)); traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); @@ -141,7 +141,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) // time_from_start.sec = 1; // time_from_start.nanosec = 0; // std::vector> points {{{3.3, 4.4, 5.5}}}; -// publish(time_from_start, points); +// publish(time_from_start, points, rclcpp::Time()); // // wait for msg is be published to the system // std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // executor.spin_once(); @@ -189,7 +189,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) // {{10.10, 11.11, 12.12}} // }; // // *INDENT-ON* -// publish(time_from_start, points); +// publish(time_from_start, points, rclcpp::Time()); // // wait for msg is be published to the system // std::this_thread::sleep_for(std::chrono::milliseconds(500)); // executor.spin_once(); @@ -241,7 +241,7 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) time_from_start.sec = 1; time_from_start.nanosec = 0; std::vector> points{{{3.3, 4.4, 5.5}}}; - publish(time_from_start, points); + publish(time_from_start, points, rclcpp::Time()); traj_controller_->wait_for_trajectory(executor); traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); @@ -847,9 +847,10 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; std::vector> points_new{{{-1., -2., -3.}}}; + RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); const auto delay = std::chrono::milliseconds(500); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; - publish(time_from_start, points_old); + publish(time_from_start, points_old, rclcpp::Time()); trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired = expected_actual; @@ -861,6 +862,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; expected_desired = expected_actual; + std::cout << "Sending old trajectory" << std::endl; publish(time_from_start, points_new, new_traj_start); waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); } @@ -876,7 +878,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory const auto delay = std::chrono::milliseconds(500); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; - publish(time_from_start, points_old); + publish(time_from_start, points_old, rclcpp::Time()); trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired = expected_actual; @@ -1014,7 +1016,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // Move joint back to the first goal points = {{first_goal}}; - publish(time_from_start, points); + publish(time_from_start, points, rclcpp::Time()); traj_controller_->wait_for_trajectory(executor); // One the first update(s) there should be a "jump" in the goal direction from command @@ -1057,7 +1059,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e time_from_start.sec = 1; time_from_start.nanosec = 0; std::vector> points{{first_goal}}; - publish(time_from_start, points); + publish(time_from_start, points, rclcpp::Time()); traj_controller_->wait_for_trajectory(executor); updateController(rclcpp::Duration::from_seconds(1.1)); @@ -1074,7 +1076,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // Move joint further in the same direction as before (to the second goal) points = {{second_goal}}; - publish(time_from_start, points); + publish(time_from_start, points, rclcpp::Time()); traj_controller_->wait_for_trajectory(executor); // One the first update(s) there **should not** be a "jump" in opposite direction from command @@ -1101,7 +1103,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // Move joint back to the first goal points = {{first_goal}}; - publish(time_from_start, points); + publish(time_from_start, points, rclcpp::Time()); traj_controller_->wait_for_trajectory(executor); // One the first update(s) there **should not** be a "jump" in the goal direction from command diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index c4becd25d6..5c55d87972 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -279,7 +279,7 @@ class TrajectoryControllerTest : public ::testing::Test */ void publish( const builtin_interfaces::msg::Duration & delay_btwn_points, - const std::vector> & points, rclcpp::Time start_time = rclcpp::Time(), + const std::vector> & points, rclcpp::Time start_time, const std::vector & joint_names = {}, const std::vector> & points_velocities = {}) { From 4504e8d36157770b024997208975cbc40045f050 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Noel=20Jim=C3=A9nez=20Garc=C3=ADa?= <48183611+Noel215@users.noreply.github.com> Date: Fri, 29 Jul 2022 09:19:59 +0200 Subject: [PATCH 003/126] port rqt_joint_trajectory_controller to ros2 (#356) --- rqt_joint_trajectory_controller/package.xml | 35 ++ rqt_joint_trajectory_controller/plugin.xml | 21 + .../resource/double_editor.ui | 95 ++++ .../resource/joint_trajectory_controller.ui | 202 +++++++ .../resource/off.svg | 267 ++++++++++ .../resource/on.svg | 231 ++++++++ .../resource/rqt_joint_trajectory_controller | 0 .../resource/scope.png | Bin 0 -> 3205 bytes .../resource/scope.svg | 269 ++++++++++ .../__init__.py | 0 .../double_editor.py | 110 ++++ .../joint_limits_urdf.py | 100 ++++ .../joint_trajectory_controller.py | 500 ++++++++++++++++++ .../rqt_joint_trajectory_controller.py | 8 + .../update_combo.py | 57 ++ .../rqt_joint_trajectory_controller/utils.py | 416 +++++++++++++++ rqt_joint_trajectory_controller/setup.cfg | 4 + rqt_joint_trajectory_controller/setup.py | 33 ++ 18 files changed, 2348 insertions(+) create mode 100644 rqt_joint_trajectory_controller/package.xml create mode 100644 rqt_joint_trajectory_controller/plugin.xml create mode 100644 rqt_joint_trajectory_controller/resource/double_editor.ui create mode 100644 rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui create mode 100644 rqt_joint_trajectory_controller/resource/off.svg create mode 100644 rqt_joint_trajectory_controller/resource/on.svg create mode 100644 rqt_joint_trajectory_controller/resource/rqt_joint_trajectory_controller create mode 100644 rqt_joint_trajectory_controller/resource/scope.png create mode 100644 rqt_joint_trajectory_controller/resource/scope.svg create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/__init__.py create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py create mode 100755 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py create mode 100644 rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py create mode 100644 rqt_joint_trajectory_controller/setup.cfg create mode 100644 rqt_joint_trajectory_controller/setup.py diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml new file mode 100644 index 0000000000..343f733339 --- /dev/null +++ b/rqt_joint_trajectory_controller/package.xml @@ -0,0 +1,35 @@ + + + + rqt_joint_trajectory_controller + 2.5.0 + Graphical frontend for interacting with joint_trajectory_controller instances. + + Bence Magyar + Mathias Lüdtke + Enrique Fernandez + + Apache-2.0 + + http://wiki.ros.org/rqt_joint_trajectory_controller + + Adolfo Rodriguez Tsouroukdissian + Noel Jimenez Garcia + + control_msgs + controller_manager_msgs + python_qt_binding + python3-rospkg + trajectory_msgs + rclpy + rqt_gui + rqt_gui_py + qt_gui + + + ament_python + + + diff --git a/rqt_joint_trajectory_controller/plugin.xml b/rqt_joint_trajectory_controller/plugin.xml new file mode 100644 index 0000000000..4feade126e --- /dev/null +++ b/rqt_joint_trajectory_controller/plugin.xml @@ -0,0 +1,21 @@ + + + + Graphical frontend for interacting with joint_trajectory_controller instances. + + + + + folder + Plugins related to robot tools. + + + resource/scope.png + + Monitor and send commands to running joint trajectory controllers. + + + + diff --git a/rqt_joint_trajectory_controller/resource/double_editor.ui b/rqt_joint_trajectory_controller/resource/double_editor.ui new file mode 100644 index 0000000000..547a90220f --- /dev/null +++ b/rqt_joint_trajectory_controller/resource/double_editor.ui @@ -0,0 +1,95 @@ + + + double_editor + + + true + + + + 0 + 0 + 249 + 121 + + + + + 0 + 0 + + + + Form + + + + + + true + + + + 0 + 0 + + + + + 0 + 0 + + + + 100 + + + Qt::Horizontal + + + false + + + false + + + + + + + true + + + + 0 + 0 + + + + + 60 + 0 + + + + + 100 + 0 + + + + QAbstractSpinBox::UpDownArrows + + + false + + + 6 + + + + + + + + diff --git a/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui new file mode 100644 index 0000000000..3f3b6c1186 --- /dev/null +++ b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui @@ -0,0 +1,202 @@ + + + joint_trajectory_controller + + + + 0 + 0 + 336 + 317 + + + + Joint trajectory controller + + + + + + + + + + + + + + + + + + + + <html><head/><body><p><span style=" font-weight:600;">controller manager</span> namespace. It is assumed that the <span style=" font-weight:600;">robot_description</span> parameter also lives in the same namesapce.</p></body></html> + + + controller manager ns + + + cm_combo + + + + + + + controller + + + jtc_combo + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + <html><head/><body><p>Enable/disable sending commands to the controller.</p></body></html> + + + + + + + off.svg + on.svg + on.svgoff.svg + + + + 48 + 48 + + + + true + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + joints + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + false + + + false + + + + + + + 0 + 0 + + + + QFrame::NoFrame + + + QFrame::Plain + + + Qt::ScrollBarAsNeeded + + + Qt::ScrollBarAlwaysOff + + + true + + + + + 0 + 0 + 314 + 109 + + + + + + + + + + + + speed scaling + + + + + + + + cm_combo + jtc_combo + enable_button + + + + diff --git a/rqt_joint_trajectory_controller/resource/off.svg b/rqt_joint_trajectory_controller/resource/off.svg new file mode 100644 index 0000000000..4c91a11260 --- /dev/null +++ b/rqt_joint_trajectory_controller/resource/off.svg @@ -0,0 +1,267 @@ + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + red power button + 08 12 2006 + + + molumen + + + red power button + + + + icon + button + design + UI + interface + power + switch + on + off + red + glossy + toggle + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_joint_trajectory_controller/resource/on.svg b/rqt_joint_trajectory_controller/resource/on.svg new file mode 100644 index 0000000000..233d356054 --- /dev/null +++ b/rqt_joint_trajectory_controller/resource/on.svg @@ -0,0 +1,231 @@ + + + + + + + + + + + + + + + + + + + + image/svg+xml + + green power button + 08 12 2006 + + + molumen + + + green power button + + + + icon + button + design + UI + interface + power + switch + on + off + green + glossy + toggle + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_joint_trajectory_controller/resource/rqt_joint_trajectory_controller b/rqt_joint_trajectory_controller/resource/rqt_joint_trajectory_controller new file mode 100644 index 0000000000..e69de29bb2 diff --git a/rqt_joint_trajectory_controller/resource/scope.png b/rqt_joint_trajectory_controller/resource/scope.png new file mode 100644 index 0000000000000000000000000000000000000000..24c089b70a3d2f984edd161dc746b54a250c6af4 GIT binary patch literal 3205 zcmV;040`j4P)T-R~Oe|64%^_$%@!y!eAvK5PBAik=o^$?H^{-QPrp0^D*I9zcDc^YG4gJCkFN9y!X12*&Z@rcK z$5>g;>)w3x&7C-oZ+q`w6_G#m-v3?!h{zQ9Z|B@UD5d`9_U+qdO}`4@&Ye52dGG%Y zxbhY6^GW4rBJvlvZ{NPR7+3yv{cMD z!yUd`yN)0L?<%EUy?y((gB1>xW%<`4(o22G-;RFD$Yoz{<=$OqvC)cw*DC4 zW#`;q0zU#!%k{kfQ|BB%Nq^1QXNd6S9`)OOKCvf!SR62qm_J+WJAjI~-uoK>pV?E8 z%6y&ukf^rAxF%t&D08tGawz0myxca}E(n)&KxFS553!`6%;eo1D`rQG`06nKWp$BkECt z2d4RqVKyNQ9D+If=KMO_9lfUWQh9SVa|QremZ5cJu|*8SJpL{Ku=bt^q0+RH8vBhN zd#yI#{a(go67kVTm-yx9Clv9Rkt$HiasKRCo$GPyfL|JV4FIs5RAj|Sb26S6&)h0= zol-&^hIAWkZa%fcw_i9#2ae88#{N?Q<-hLn(0h)8l)~5z04Scw(;hYeEVt*J!#Rh? zuZM?hJ`0c{)#d}GX~YS8%}adinF6If4?lR3ga3PxAT+%6ZAUL@lLQf332Odmr3kbl z5QWwXrDZb%t!6aw+e2sp06@ZXA++j&6~irDt3-%`fM#50|5B4?_lV)cT|PUUqD0ZS z+M>5VqjUEX^(bbV&%pVLGb*Tf6NjXcP}mad9Bw65$*KpuUyyhcz!H`CWg3m@Lhsu4 zQmC~Xfbs-7pb^%&)@t+oEsqk<@u0yZ?xRF8nKfwb47vVv%)_*Wn&y<&5C;+Ms7WiT z)9i-y8Y71Jgqh7S)>M(OG@_py?*b02k(e_Nl&PctaC>?K7(6NoltXF>)&(?bDP>;c z*B?*0KRiN-}}3ak1Vrs%B(E0z9M_? zYINoW%Laco)94C0SBbhRB4T!)m zoFE8S+k3qJYEHeGf>-?D`@-GGA(LB7LaD39hImNYWhIXw<97brR5hd&Eaz^AQ3!#-$ za5YUU4T7MxA_@a)QB0>>;;g1WapbPRc!L(j%w!y$z_)-adxFpRsn;E?okyHJ+~vXO zm`)JW=(o7EJE5JlX+#Ora)$M?#>ponaTP`YQKINwjW{nlfTSK!s|nqt#^p;*+TAI6 zR!282wH;5psS$)IvCOo^lnK3S$0Xb!+4~R>K0i*F#C?J&VK{Ek-WhW9xtLkp!B29g zMT!Letdw}%NxaCfN-MOgzHlXs4$Ecw_uk|21d*cN1Tz+D4Udn6C{8)J-{bUj#_^*W zp%En%iNbl0bq3=s&U>^LlvEigiDLG4DmM>?0ht|QFnDoz zpl~_;e8>k68l0YVxw>ySIu7{o?v#VcG2>!Jtq^n>(?4$0yF6sCxlccBQCdTyJ+;tP z>ngn+KoA6CttAXYy0s2c4+vF2rxnx8b4q7O%Zy2pl9m~w90-I#Mc{6ya_ey)a#vuz zT~>~zGmP>HkEX|bc3%^H5>h(DB%d-ZQjD{tE`nmhz56Y8uXvul0n=iiFbD{m4n3Pu zIzzoo&{|`yUC`180AU!4bB-iQci_nR}_g6cau>O1U+zaplGlWqyOb zVGZ4I^n)3*G9wgEbQF>2xvY7>S}P)g)|&5przDIvo0yx=EB@=}d%SZUVWYoy1&u<}NSX%6K&B;_{rC&8VrE zRy`z)OO6lQ zA3$i#W;I%trMK2Holg1q@EAW<1WL2#8mKa0uihg$z0A)318!Wsf*H4%ltNP)T)nCh zpl}g!*HI6It_`t%y>)E$0=(Z$$=*Y=t5FI9Wyti1LorMa6CxE6go^$_Ms5pQhl*#j z7G+ub#mq|pixX~`jVN#+LNT6F^o4PwpvF_|>_ew+kfVq))fiFKg6iJr^M{7R!wFeE zoj&hVJl%xb|j8*|!6qoj529s(&pVrB;aN&V7007onLnG}VoyXz4P`WC6 z2@sW*Sy=%uOn|j^drzzuhnMmd2>~Y_;|;khn3;_6s79wdp_%NHnUaR8ku(NmSwc21 zADsBj3f8k$&umAqc-1d^0RW4`GT#qwkwTZUS&gzZtP{!o~i zjB*aZ%SvDU1^xAJq+2ocLN=;V<{=h zdqX0Xp^{@J!zPc81{@7X4D%@s7ce^6ycpI2YvT(H6?r|Q)9o3mvm1k|-u7}qN0w#T zS!^x(fw|kfRL4)|3WmjqU)@*W1$FtSpe%PE1q_N2nagmv?cuW}OwRZr^Adi0Bem94 z@g&zOoNFiN-IwttqjJorgCp9X?(y_q%7cTLUpzWtP>d+O!7tj;ZGH6lE1HD`=f+y< z&V|?c^!tT3bK5Ld)iaK%NjaDu^TCnj{kw|XoH8mVOw1HJ2XJA!yl7m_?_*cjJf~mD zm5*;|st^3q8tc}q3bKira#Fx-T4Lu1j&;>N&bqs|?0?I+QUZQk?Xwj8xw~eC zEeA~D9W$3QD)aUCSHIbI(|pktjeg8G=k{~Hp+&L*jwD_+)(2N3B0!wv6c94Qbrhi*)}_OKgkK zdF7Q?x+3xc@RZjuxK)x}wz%;3`%?rxQ#3x;v_7t)T}1x*op;{(^M!uxAKfp%{POoi z#x7wwATKWbM93Uc?oEI)w?_=CnEBn-ur)i r_uY4YbpCe}8vwp;^0@5#wUGY@C17*|Z}9e100000NkvXXu0mjf>V71? literal 0 HcmV?d00001 diff --git a/rqt_joint_trajectory_controller/resource/scope.svg b/rqt_joint_trajectory_controller/resource/scope.svg new file mode 100644 index 0000000000..ee4d4da371 --- /dev/null +++ b/rqt_joint_trajectory_controller/resource/scope.svg @@ -0,0 +1,269 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/__init__.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py new file mode 100644 index 0000000000..23a1f2c806 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python + +# Copyright 2022 PAL Robotics S.L. +# +# 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. + +import os +from ament_index_python.packages import get_package_share_directory + +from python_qt_binding import loadUi +from python_qt_binding.QtCore import Signal +from python_qt_binding.QtWidgets import QWidget + + +class DoubleEditor(QWidget): + # TODO: + # - Actually make bounds optional + # + # - Support wrapping mode + # + # - Support unspecified (+-Inf) lower and upper bounds (both, or one) + # + # - Allow to specify the step and page increment sizes + # (right-click context menu?) + # + # - Use alternative widget to slider for values that wrap, or are + # unbounded. + # QwtWheel could be a good choice, dials are not so good because they + # use lots of vertical (premium) screen space, and are fine for wrapping + # values, but not so much for unbounded ones + # + # - Merge with existing similar code such as rqt_reconfigure's + # DoubleEditor? + """ + Widget that allows to edit the value of a floating-point value. + Optionally subject to lower and upper bounds. + """ + + valueChanged = Signal(float) + + def __init__(self, min_val, max_val): + super(DoubleEditor, self).__init__() + + # Preconditions + assert min_val < max_val + + # Cache values + self._min_val = min_val + self._max_val = max_val + + # Load editor UI + ui_file = os.path.join(get_package_share_directory('rqt_joint_trajectory_controller'), + 'resource', 'double_editor.ui') + loadUi(ui_file, self) + + # Setup widget ranges and slider scale factor + self.slider.setRange(0, 100) + self.slider.setSingleStep(1) + self._scale = (max_val - min_val) / \ + (self.slider.maximum() - self.slider.minimum()) + self.spin_box.setRange(min_val, max_val) + self.spin_box.setSingleStep(self._scale) + + # Couple slider and spin box together + self.slider.valueChanged.connect(self._on_slider_changed) + self.spin_box.valueChanged.connect(self._on_spinbox_changed) + + # Ensure initial sync of slider and spin box + self._on_spinbox_changed() + + def _slider_to_val(self, sval): + return self._min_val + self._scale * (sval - self.slider.minimum()) + + def _val_to_slider(self, val): + return round(self.slider.minimum() + (val - self._min_val) / + self._scale) + + def _on_slider_changed(self): + val = self._slider_to_val(self.slider.value()) + self.spin_box.blockSignals(True) # Prevents updating the command twice + self.spin_box.setValue(val) + self.spin_box.blockSignals(False) + self.valueChanged.emit(val) + + def _on_spinbox_changed(self): + val = self.spin_box.value() + self.slider.blockSignals(True) # Prevents updating the command twice + self.slider.setValue(self._val_to_slider(val)) + self.slider.blockSignals(False) + self.valueChanged.emit(val) + + def setValue(self, val): + if val != self.spin_box.value(): + self.spin_box.blockSignals(True) + self.spin_box.setValue(val) # Update spin box first + self._on_spinbox_changed() # Sync slider with spin box + self.spin_box.blockSignals(False) + + def value(self): + return self.spin_box.value() diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py new file mode 100644 index 0000000000..6f339ba387 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python + +# TODO: Use urdf_parser_py.urdf instead. I gave it a try, but got +# Exception: Required attribute not set in XML: upper +# upper is an optional attribute, so I don't understand what's going on +# See comments in https://github.com/ros/urdfdom/issues/36 + +import xml.dom.minidom +from math import pi + +import rclpy +from std_msgs.msg import String + +description = "" + + +def callback(msg): + global description + description = msg.data + + +def get_joint_limits(node, key='robot_description', use_smallest_joint_limits=True): + global description + use_small = use_smallest_joint_limits + use_mimic = True + + # Code inspired on the joint_state_publisher package by David Lu!!! + # https://github.com/ros/robot_model/blob/indigo-devel/ + # joint_state_publisher/joint_state_publisher/joint_state_publisher + + qos_profile = rclpy.qos.QoSProfile(depth=1) + qos_profile.durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL + qos_profile.reliability = rclpy.qos.ReliabilityPolicy.RELIABLE + + node.create_subscription(String, "/robot_description", callback, qos_profile) + rclpy.spin_once(node) + + free_joints = {} + dependent_joints = {} + + if description != "": + robot = xml.dom.minidom.parseString(description)\ + .getElementsByTagName('robot')[0] + + # Find all non-fixed joints + for child in robot.childNodes: + if child.nodeType is child.TEXT_NODE: + continue + if child.localName == 'joint': + jtype = child.getAttribute('type') + if jtype == 'fixed': + continue + name = child.getAttribute('name') + try: + limit = child.getElementsByTagName('limit')[0] + except: + continue + if jtype == 'continuous': + minval = -pi + maxval = pi + else: + try: + minval = float(limit.getAttribute('lower')) + maxval = float(limit.getAttribute('upper')) + except: + continue + try: + maxvel = float(limit.getAttribute('velocity')) + except: + continue + safety_tags = child.getElementsByTagName('safety_controller') + if use_small and len(safety_tags) == 1: + tag = safety_tags[0] + if tag.hasAttribute('soft_lower_limit'): + minval = max(minval, + float(tag.getAttribute('soft_lower_limit'))) + if tag.hasAttribute('soft_upper_limit'): + maxval = min(maxval, + float(tag.getAttribute('soft_upper_limit'))) + + mimic_tags = child.getElementsByTagName('mimic') + if use_mimic and len(mimic_tags) == 1: + tag = mimic_tags[0] + entry = {'parent': tag.getAttribute('joint')} + if tag.hasAttribute('multiplier'): + entry['factor'] = float(tag.getAttribute('multiplier')) + if tag.hasAttribute('offset'): + entry['offset'] = float(tag.getAttribute('offset')) + + dependent_joints[name] = entry + continue + + if name in dependent_joints: + continue + + joint = {'min_position': minval, 'max_position': maxval} + joint["has_position_limits"] = jtype != 'continuous' + joint['max_velocity'] = maxvel + free_joints[name] = joint + return free_joints diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py new file mode 100644 index 0000000000..f3c98f9fe0 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -0,0 +1,500 @@ +#!/usr/bin/env python + +# Copyright 2022 PAL Robotics S.L. +# +# 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. + +from __future__ import print_function +import os +import rclpy +import threading +from ament_index_python.packages import get_package_share_directory + +from qt_gui.plugin import Plugin +from python_qt_binding import loadUi +from python_qt_binding.QtCore import QTimer, Signal +from python_qt_binding.QtWidgets import QWidget, QFormLayout + +from control_msgs.msg import JointTrajectoryControllerState +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + +from .utils import ControllerLister, ControllerManagerLister +from .double_editor import DoubleEditor +from .joint_limits_urdf import get_joint_limits +from .update_combo import update_combo + +# TODO: +# - Better UI suppor for continuous joints (see DoubleEditor TODO) +# - Can we load controller joints faster?, it's currently pretty slow +# - If URDF is reloaded, allow to reset the whole plugin? +# - Allow to configure: +# - URDF location +# - Command publishing and state update frequency +# - Controller manager and jtc monitor frequency +# - Min trajectory duration +# - Fail gracefully when the URDF or some other requisite is not set +# - Could users confuse the enable/disable button with controller start/stop +# (in a controller manager sense)? +# - Better decoupling between model and view +# - Tab order is wrong. For the record, this did not work: +# QWidget.setTabOrder(self._widget.controller_group, +# self._widget.joint_group) +# QWidget.setTabOrder(self._widget.joint_group, +# self._widget.speed_scaling_group) + +# NOTE: +# Controller enable/disable icons are in the public domain, and available here: +# freestockphotos.biz/photos.php?c=all&o=popular&s=0&lic=all&a=all&set=powocab_u2 + + +class JointTrajectoryController(Plugin): + """ + Graphical frontend for a C{JointTrajectoryController}. + + There are two modes for interacting with a controller: + 1. B{Monitor mode} Joint displays are updated with the state reported + by the controller. This is a read-only mode and does I{not} send + control commands. Every time a new controller is selected, it starts + in monitor mode for safety reasons. + + 2. B{Control mode} Joint displays update the control command that is + sent to the controller. Commands are sent periodically evan if the + displays are not being updated by the user. + + To control the aggressiveness of the motions, the maximum speed of the + sent commands can be scaled down using the C{Speed scaling control} + + This plugin is able to detect and keep track of all active controller + managers, as well as the JointTrajectoryControllers that are I{running} + in each controller manager. + For a controller to be compatible with this plugin, it must comply with + the following requisites: + - The controller type contains the C{JointTrajectoryController} + substring, e.g., C{position_controllers/JointTrajectoryController} + - The controller exposes the C{command} and C{state} topics in its + ROS interface. + + Additionally, there must be a URDF loaded with a valid joint limit + specification, namely position (when applicable) and velocity limits. + + A reference implementation of the C{JointTrajectoryController} is + available in the C{joint_trajectory_controller} ROS package. + """ + + _cmd_pub_freq = 10.0 # Hz + _widget_update_freq = 30.0 # Hz + _ctrlrs_update_freq = 1 # Hz + _min_traj_dur = 5.0 / _cmd_pub_freq # Minimum trajectory duration + + jointStateChanged = Signal([dict]) + + def __init__(self, context): + super(JointTrajectoryController, self).__init__(context) + self.setObjectName('JointTrajectoryController') + self._node = rclpy.node.Node("rqt_joint_trajectory_controller") + self._executor = None + self._executor_thread = None + + # Create QWidget and extend it with all the attributes and children + # from the UI file + self._widget = QWidget() + ui_file = os.path.join(get_package_share_directory('rqt_joint_trajectory_controller'), + 'resource', + 'joint_trajectory_controller.ui') + loadUi(ui_file, self._widget) + self._widget.setObjectName('JointTrajectoryControllerUi') + ns = self._node.get_namespace()[1:-1] + self._widget.controller_group.setTitle('ns: ' + ns) + + # Setup speed scaler + speed_scaling = DoubleEditor(1.0, 100.0) + speed_scaling.spin_box.setSuffix('%') + speed_scaling.spin_box.setValue(50.0) + speed_scaling.spin_box.setDecimals(0) + speed_scaling.setEnabled(False) + self._widget.speed_scaling_layout.addWidget(speed_scaling) + self._speed_scaling_widget = speed_scaling + speed_scaling.valueChanged.connect(self._on_speed_scaling_change) + self._on_speed_scaling_change(speed_scaling.value()) + + # Show _widget.windowTitle on left-top of each plugin (when + # it's set in _widget). This is useful when you open multiple + # plugins at once. Also if you open multiple instances of your + # plugin at once, these lines add number to make it easy to + # tell from pane to pane. + if context.serial_number() > 1: + self._widget.setWindowTitle(self._widget.windowTitle() + + (' (%d)' % context.serial_number())) + # Add widget to the user interface + context.add_widget(self._widget) + + # Initialize members + self._jtc_name = [] # Name of selected joint trajectory controller + self._cm_ns = [] # Namespace of the selected controller manager + self._joint_pos = {} # name->pos map for joints of selected controller + self._joint_names = [] # Ordered list of selected controller joints + self._robot_joint_limits = {} # Lazily evaluated on first use + + # Timer for sending commands to active controller + self._update_cmd_timer = QTimer(self) + self._update_cmd_timer.setInterval(int(1000.0 / self._cmd_pub_freq)) + self._update_cmd_timer.timeout.connect(self._update_cmd_cb) + + # Timer for updating the joint widgets from the controller state + self._update_act_pos_timer = QTimer(self) + self._update_act_pos_timer.setInterval(int(1000.0 / + self._widget_update_freq)) + self._update_act_pos_timer.timeout.connect(self._update_joint_widgets) + + # Timer for controller manager updates + self._list_cm = ControllerManagerLister() + self._update_cm_list_timer = QTimer(self) + self._update_cm_list_timer.setInterval(int(1000.0 / + self._ctrlrs_update_freq)) + self._update_cm_list_timer.timeout.connect(self._update_cm_list) + self._update_cm_list_timer.start() + + # Timer for running controller updates + self._update_jtc_list_timer = QTimer(self) + self._update_jtc_list_timer.setInterval(int(1000.0 / + self._ctrlrs_update_freq)) + self._update_jtc_list_timer.timeout.connect(self._update_jtc_list) + self._update_jtc_list_timer.start() + + # Signal connections + w = self._widget + w.enable_button.toggled.connect(self._on_jtc_enabled) + w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change) + w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change) + + self._cmd_pub = None # Controller command publisher + self._state_sub = None # Controller state subscriber + + self._list_controllers = None + + def shutdown_plugin(self): + self._update_cmd_timer.stop() + self._update_act_pos_timer.stop() + self._update_cm_list_timer.stop() + self._update_jtc_list_timer.stop() + self._unregister_state_sub() + self._unregister_cmd_pub() + self._unregister_executor() + + def save_settings(self, plugin_settings, instance_settings): + instance_settings.set_value('cm_ns', self._cm_ns) + instance_settings.set_value('jtc_name', self._jtc_name) + + def restore_settings(self, plugin_settings, instance_settings): + # Restore last session's controller_manager, if present + self._update_cm_list() + cm_ns = instance_settings.value('cm_ns') + cm_combo = self._widget.cm_combo + cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())] + try: + idx = cm_list.index(cm_ns) + cm_combo.setCurrentIndex(idx) + # Resore last session's controller, if running + self._update_jtc_list() + jtc_name = instance_settings.value('jtc_name') + jtc_combo = self._widget.jtc_combo + jtc_list = [jtc_combo.itemText(i) for i in range(jtc_combo.count())] + try: + idx = jtc_list.index(jtc_name) + jtc_combo.setCurrentIndex(idx) + except (ValueError): + pass + except (ValueError): + pass + + # def trigger_configuration(self): + # Comment in to signal that the plugin has a way to configure + # This will enable a setting button (gear icon) in each dock widget + # title bar + # Usually used to open a modal configuration dialog + + def _update_cm_list(self): + update_combo(self._widget.cm_combo, self._list_cm()) + + def _update_jtc_list(self): + # Clear controller list if no controller information is available + if not self._list_controllers: + self._widget.jtc_combo.clear() + return + + # List of running controllers with a valid joint limits specification + # for _all_ their joints + running_jtc = self._running_jtc_info() + if running_jtc and not self._robot_joint_limits: + self._robot_joint_limits = get_joint_limits(self._node) # Lazy evaluation + valid_jtc = [] + if (self._robot_joint_limits): + for jtc_info in running_jtc: + has_limits = all(name in self._robot_joint_limits + for name in _jtc_joint_names(jtc_info)) + if has_limits: + valid_jtc.append(jtc_info) + valid_jtc_names = [data.name for data in valid_jtc] + + # Update widget + update_combo(self._widget.jtc_combo, sorted(valid_jtc_names)) + + def _on_speed_scaling_change(self, val): + self._speed_scale = val / self._speed_scaling_widget.slider.maximum() + + def _on_joint_state_change(self, actual_pos): + assert(len(actual_pos) == len(self._joint_pos)) + for name in actual_pos.keys(): + self._joint_pos[name]['position'] = actual_pos[name] + + def _on_cm_change(self, cm_ns): + self._cm_ns = cm_ns + if cm_ns: + self._list_controllers = ControllerLister(cm_ns) + # NOTE: Clear below is important, as different controller managers + # might have controllers with the same name but different + # configurations. Clearing forces controller re-discovery + self._widget.jtc_combo.clear() + self._update_jtc_list() + else: + self._list_controllers = None + + def _on_jtc_change(self, jtc_name): + self._unload_jtc() + self._jtc_name = jtc_name + if self._jtc_name: + self._load_jtc() + + def _on_jtc_enabled(self, val): + # Don't allow enabling if there are no controllers selected + if not self._jtc_name: + self._widget.enable_button.setChecked(False) + return + + # Enable/disable joint displays + for joint_widget in self._joint_widgets(): + joint_widget.setEnabled(val) + + # Enable/disable speed scaling + self._speed_scaling_widget.setEnabled(val) + + if val: + # Widgets send desired position commands to controller + self._update_act_pos_timer.stop() + self._update_cmd_timer.start() + else: + # Controller updates widgets with actual position + self._update_cmd_timer.stop() + self._update_act_pos_timer.start() + + def _load_jtc(self): + # Initialize joint data corresponding to selected controller + running_jtc = self._running_jtc_info() + self._joint_names = next(_jtc_joint_names(x) for x in running_jtc + if x.name == self._jtc_name) + for name in self._joint_names: + self._joint_pos[name] = {} + + # Update joint display + try: + layout = self._widget.joint_group.layout() + for name in self._joint_names: + limits = self._robot_joint_limits[name] + joint_widget = DoubleEditor(limits['min_position'], + limits['max_position']) + layout.addRow(name, joint_widget) + # NOTE: Using partial instead of a lambda because lambdas + # "will not evaluate/look up the argument values before it is + # effectively called, breaking situations like using a loop + # variable inside it" + from functools import partial + par = partial(self._update_single_cmd_cb, name=name) + joint_widget.valueChanged.connect(par) + except: + # TODO: Can we do better than swallow the exception? + from sys import exc_info + print('Unexpected error:', exc_info()[0]) + + # Enter monitor mode (sending commands disabled) + self._on_jtc_enabled(False) + + # Setup ROS interfaces + jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name) + state_topic = jtc_ns + '/state' + cmd_topic = jtc_ns + '/joint_trajectory' + self._state_sub = self._node.create_subscription(JointTrajectoryControllerState, + state_topic, + self._state_cb, + 1) + self._cmd_pub = self._node.create_publisher(JointTrajectory, + cmd_topic, + 1) + + self.jointStateChanged.connect(self._on_joint_state_change) + + self._executor = rclpy.executors.SingleThreadedExecutor() + self._executor.add_node(self._node) + self._executor_thread = threading.Thread(target=self._executor.spin, daemon=True) + self._executor_thread.start() + + def _unload_jtc(self): + # Stop updating the joint positions + try: + self.jointStateChanged.disconnect(self._on_joint_state_change) + except: + pass + + # Reset ROS interfaces + self._unregister_state_sub() + self._unregister_cmd_pub() + self._unregister_executor() + + # Clear joint widgets + # NOTE: Implementation is a workaround for: + # https://bugreports.qt-project.org/browse/QTBUG-15990 :( + + layout = self._widget.joint_group.layout() + if layout is not None: + while layout.count(): + layout.takeAt(0).widget().deleteLater() + # Delete existing layout by reparenting to temporary + QWidget().setLayout(layout) + self._widget.joint_group.setLayout(QFormLayout()) + + # Reset joint data + self._joint_names = [] + self._joint_pos = {} + + # Enforce monitor mode (sending commands disabled) + self._widget.enable_button.setChecked(False) + + def _running_jtc_info(self): + from .utils import filter_by_type, filter_by_state + + controller_list = self._list_controllers() + jtc_list = filter_by_type(controller_list, + 'JointTrajectoryController', + match_substring=True) + running_jtc_list = filter_by_state(jtc_list, 'active') + return running_jtc_list + + def _unregister_cmd_pub(self): + if self._cmd_pub is not None: + self._node.destroy_publisher(self._cmd_pub) + self._cmd_pub = None + + def _unregister_state_sub(self): + if self._state_sub is not None: + self._node.destroy_subscription(self._state_sub) + self._state_sub = None + + def _unregister_executor(self): + if self._executor is not None: + self._executor.shutdown() + self._executor_thread.join() + self._executor = None + + def _state_cb(self, msg): + actual_pos = {} + for i in range(len(msg.joint_names)): + joint_name = msg.joint_names[i] + joint_pos = msg.actual.positions[i] + actual_pos[joint_name] = joint_pos + self.jointStateChanged.emit(actual_pos) + + def _update_single_cmd_cb(self, val, name): + self._joint_pos[name]['command'] = val + + def _update_cmd_cb(self): + dur = [] + traj = JointTrajectory() + traj.joint_names = self._joint_names + point = JointTrajectoryPoint() + for name in traj.joint_names: + pos = self._joint_pos[name]['position'] + cmd = pos + try: + cmd = self._joint_pos[name]['command'] + except (KeyError): + pass + max_vel = self._robot_joint_limits[name]['max_velocity'] + dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur)) + point.positions.append(cmd) + duration = rclpy.duration.Duration(seconds=(max(dur) / self._speed_scale)) + point.time_from_start = duration.to_msg() + traj.points.append(point) + + self._cmd_pub.publish(traj) + + def _update_joint_widgets(self): + joint_widgets = self._joint_widgets() + for id in range(len(joint_widgets)): + joint_name = self._joint_names[id] + try: + joint_pos = self._joint_pos[joint_name]['position'] + joint_widgets[id].setValue(joint_pos) + except (KeyError): + pass # Can happen when first connected to controller + + def _joint_widgets(self): # TODO: Cache instead of compute every time? + widgets = [] + layout = self._widget.joint_group.layout() + for row_id in range(layout.rowCount()): + widgets.append(layout.itemAt(row_id, + QFormLayout.FieldRole).widget()) + return widgets + + +def _jtc_joint_names(jtc_info): + # NOTE: We assume that there is at least one hardware interface that + # claims resources (there should be), and the resource list is fetched + # from the first available interface + + joint_names = [] + for interface in jtc_info.claimed_interfaces: + name = interface.split("/")[0] + joint_names.append(name) + + return joint_names + + +def _resolve_controller_ns(cm_ns, controller_name): + """ + Resolve a controller's namespace from that of the controller manager. + Controllers are assumed to live one level above the controller + manager, e.g. + + >>> _resolve_controller_ns('/path/to/controller_manager', 'foo') + '/path/to/foo' + + In the particular case in which the controller manager is not + namespaced, the controller is assumed to live in the root namespace + + >>> _resolve_controller_ns('/', 'foo') + '/foo' + >>> _resolve_controller_ns('', 'foo') + '/foo' + + @param cm_ns Controller manager namespace (can be an empty string) + @type cm_ns str + @param controller_name Controller name (non-empty string) + @type controller_name str + @return Controller namespace + @rtype str + """ + assert(controller_name) + ns = cm_ns.rsplit('/', 1)[0] + if ns != '/': + ns += '/' + ns += controller_name + return ns diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py new file mode 100755 index 0000000000..d0eb860fc5 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py @@ -0,0 +1,8 @@ +#!/usr/bin/env python3 + +import sys + +from rqt_gui.main import Main + +main = Main() +sys.exit(main.main(sys.argv, standalone='rqt_joint_trajectory_controller')) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py new file mode 100644 index 0000000000..64e9565cd6 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python + +# Copyright 2022 PAL Robotics S.L. +# +# 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. + + +def update_combo(combo, new_vals): + """ + Update the contents of a combo box with a set of new values. + + If the previously selected element is still present in the new values, it + will remain as active selection, even if its index has changed. This will + not trigger any signal. + + If the previously selected element is no longer present in the new values, + the combo will unset its selection. This will trigger signals for changed + element. + """ + selected_val = combo.currentText() + old_vals = [combo.itemText(i) for i in range(combo.count())] + + # Check if combo items changed + if not _is_permutation(old_vals, new_vals): + # Determine if selected value is in the new list + selected_id = -1 + try: + selected_id = new_vals.index(selected_val) + except (ValueError): + combo.setCurrentIndex(-1) + + # Re-populate items + combo.blockSignals(True) # No need to notify these changes + combo.clear() + combo.insertItems(0, new_vals) + combo.setCurrentIndex(selected_id) # Restore selection + combo.blockSignals(False) + + +def _is_permutation(a, b): + """ + @type a [] + @type b [] + @return True if C{a} is a permutation of C{b}, false otherwise + @rtype bool + """ + return len(a) == len(b) and sorted(a) == sorted(b) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py new file mode 100644 index 0000000000..e2b6ffa438 --- /dev/null +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py @@ -0,0 +1,416 @@ +#!/usr/bin/env python + +# Copyright 2022 PAL Robotics S.L. +# +# 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. + +# NOTE: The Python API contained in this file is considered UNSTABLE and +# subject to change. +# No backwards compatibility guarrantees are provided at this moment. + + +import rclpy +from controller_manager_msgs.srv import ListControllers + +# Names of controller manager services, and their respective types +_LIST_CONTROLLERS_STR = 'list_controllers' +_LIST_CONTROLLERS_TYPE = 'controller_manager_msgs/srv/ListControllers' +_LIST_CONTROLLER_TYPES_STR = 'list_controller_types' +_LIST_CONTROLLER_TYPES_TYPE = 'controller_manager_msgs/srv/ListControllerTypes' +_LOAD_CONTROLLER_STR = 'load_controller' +_LOAD_CONTROLLER_TYPE = 'controller_manager_msgs/srv/LoadController' +_UNLOAD_CONTROLLER_STR = 'unload_controller' +_UNLOAD_CONTROLLER_TYPE = 'controller_manager_msgs/srv/UnloadController' +_SWITCH_CONTROLLER_STR = 'switch_controller' +_SWITCH_CONTROLLER_TYPE = 'controller_manager_msgs/srv/SwitchController' +_RELOAD_CONTROLLER_LIBS_STR = 'reload_controller_libraries' +_RELOAD_CONTROLLER_LIBS_TYPE = ('controller_manager_msgs/srv/' + 'ReloadControllerLibraries') + +# Map from service names to their respective type +cm_services = { + _LIST_CONTROLLERS_STR: _LIST_CONTROLLERS_TYPE, + _LIST_CONTROLLER_TYPES_STR: _LIST_CONTROLLER_TYPES_TYPE, + _LOAD_CONTROLLER_STR: _LOAD_CONTROLLER_TYPE, + _UNLOAD_CONTROLLER_STR: _UNLOAD_CONTROLLER_TYPE, + _SWITCH_CONTROLLER_STR: _SWITCH_CONTROLLER_TYPE, + _RELOAD_CONTROLLER_LIBS_STR: _RELOAD_CONTROLLER_LIBS_TYPE +} + + +def get_controller_managers(namespace='/', initial_guess=None): + """ + Get list of active controller manager namespaces. + + @param namespace: Namespace where to look for controller managers. + @type namespace: str + @param initial_guess: Initial guess of the active controller managers. + Typically c{initial_guess} is the output of a previous call to this method, + and is useful when periodically checking for changes in the list of + active controller managers. + Elements in this list will go through a lazy validity check (as opposed to + a full name+type API verification), so providing a good estimate can + significantly reduce the number of ROS master queries incurred by this + method. + @type initial_guess: [str] + @return: Sorted list of active controller manager namespaces. + @rtype: [str] + """ + ns_list = [] + if initial_guess is not None: + ns_list = initial_guess[:] # force copy + + # Get list of (potential) currently running controller managers + node = rclpy.node.Node("get_controller_managers_node") + ns_list_curr = _sloppy_get_controller_managers(node, namespace) + + # Update initial guess: + # 1. Remove entries not found in current list + # 2. Add new untracked controller managers + ns_list[:] = [ns for ns in ns_list if ns in ns_list_curr] + ns_list += [ns for ns in ns_list_curr + if ns not in ns_list and is_controller_manager(node, ns)] + + return sorted(ns_list) + + +def is_controller_manager(node, namespace): + """ + Check if the input namespace exposes the controller_manager ROS interface. + + This method has the overhead of several ROS master queries + (one per ROS API member). + + @param namespace: Namespace to check + @type namespace: str + @return: True if namespace exposes the controller_manager ROS interface + @rtype: bool + """ + cm_ns = namespace + if not cm_ns or cm_ns[-1] != '/': + cm_ns += '/' + for srv_name in cm_services.keys(): + if not _srv_exists(node, cm_ns + srv_name, cm_services[srv_name]): + return False + return True + + +def _sloppy_get_controller_managers(node, namespace): + """ + Get list of I{potential} active controller manager namespaces. + + The method name is prepended with I{sloppy}, and the returned list contains + I{potential} active controller managers because it does not perform a + thorough check of the expected ROS API. + It rather tries to minimize the number of ROS master queries. + + This method has the overhead of one ROS master query. + + @param namespace: Namespace where to look for controller managers. + @type namespace: str + @return: List of I{potential} active controller manager namespaces. + @rtype: [str] + """ + # refresh the list of controller managers we can find + srv_list = node.get_service_names_and_types() + + ns_list = [] + for srv_info in srv_list: + match_str = '/' + _LIST_CONTROLLERS_STR + # First element of srv_name is the service name + if match_str in srv_info[0]: + ns = srv_info[0].split(match_str)[0] + if ns == '': + # controller manager services live in root namespace + # unlikely, but still possible + ns = '/' + ns_list.append(ns) + return ns_list + + +def _srv_exists(node, srv_name, srv_type): + """ + Check if a ROS service of specific name and type exists. + + This method has the overhead of one ROS master query. + + @param srv_name: Fully qualified service name + @type srv_name: str + @param srv_type: Service type + @type srv_type: str + """ + if not srv_name or not srv_type: + return False + + srv_list = node.get_service_names_and_types() + srv_info = [item for item in srv_list if item[0] == srv_name] + srv_obtained_type = srv_info[0][1][0] + return srv_type == srv_obtained_type + + +############################################################################### +# +# Convenience classes for querying controller managers and controllers +# +############################################################################### + +class ControllerManagerLister: + """ + Convenience functor for querying the list of active controller managers. + + Useful when frequently updating the list, as it internally performs + some optimizations that reduce the number of interactions with the + ROS master. + + Example usage: + >>> list_cm = ControllerManagerLister() + >>> print(list_cm()) + """ + + def __init__(self, namespace='/'): + self._ns = namespace + self._cm_list = [] + + def __call__(self): + """Get list of running controller managers.""" + self._cm_list = get_controller_managers(self._ns, self._cm_list) + return self._cm_list + + +class ControllerLister: + """ + Convenience functor for querying loaded controller data. + + The output of calling this functor can be used as input to the different + controller filtering functions available in this module. + + Example usage. Get I{running} controllers of type C{bar_base/bar}: + >>> list_controllers = ControllerLister('foo_robot/controller_manager') + >>> all_ctrl = list_controllers() + >>> running_ctrl = filter_by_state(all_ctrl, 'running') + >>> running_bar_ctrl = filter_by_type(running_ctrl, 'bar_base/bar') + """ + + def __init__(self, namespace='/controller_manager'): + """ + @param namespace Namespace of controller manager to monitor. + + @type namespace str + """ + self._node = rclpy.node.Node("controller_lister") + self._srv_name = namespace + '/' + _LIST_CONTROLLERS_STR + self._srv_client = self._create_client() + + """ + @return: Controller list. + @rtype: [controller_manager_msgs/ControllerState] + """ + + def __call__(self): + controller_list = self._srv_client.call_async(ListControllers.Request()) + rclpy.spin_until_future_complete(self._node, controller_list) + return controller_list.result().controller + + def _create_client(self): + return self._node.create_client(ListControllers, self._srv_name) + +############################################################################### +# +# Convenience methods for filtering controller state information +# +############################################################################### + + +def filter_by_name(ctrl_list, ctrl_name, match_substring=False): + """ + Filter controller state list by controller name. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param ctrl_name: Controller name + @type ctrl_name: str + @param match_substring: Set to True to allow substring matching + @type match_substring: bool + @return: Controllers matching the specified name + @rtype: [controller_manager_msgs/ControllerState] + """ + return _filter_by_attr(ctrl_list, 'name', ctrl_name, match_substring) + + +def filter_by_type(ctrl_list, ctrl_type, match_substring=False): + """ + Filter controller state list by controller type. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param ctrl_type: Controller type + @type ctrl_type: str + @param match_substring: Set to True to allow substring matching + @type match_substring: bool + @return: Controllers matching the specified type + @rtype: [controller_manager_msgs/ControllerState] + """ + return _filter_by_attr(ctrl_list, 'type', ctrl_type, match_substring) + + +def filter_by_state(ctrl_list, ctrl_state, match_substring=False): + """ + Filter controller state list by controller state. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param ctrl_state: Controller state + @type ctrl_state: str + @param match_substring: Set to True to allow substring matching + @type match_substring: bool + @return: Controllers matching the specified state + @rtype: [controller_manager_msgs/ControllerState] + """ + return _filter_by_attr(ctrl_list, 'state', ctrl_state, match_substring) + + +def filter_by_hardware_interface(ctrl_list, + hardware_interface, + match_substring=False): + """ + Filter controller state list by controller hardware interface. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param hardware_interface: Controller hardware interface + @type hardware_interface: str + @param match_substring: Set to True to allow substring matching + @type match_substring: bool + @return: Controllers matching the specified hardware interface + @rtype: [controller_manager_msgs/ControllerState] + """ + list_out = [] + for ctrl in ctrl_list: + for resource_set in ctrl.claimed_resources: + if match_substring: + if hardware_interface in resource_set.hardware_interface: + list_out.append(ctrl) + break + else: + if resource_set.hardware_interface == hardware_interface: + list_out.append(ctrl) + break + return list_out + + +def filter_by_resources(ctrl_list, + resources, + hardware_interface=None, + match_any=False): + """ + Filter controller state list by claimed resources. + + @param ctrl_list: Controller state list + @type ctrl_list: [controller_manager_msgs/ControllerState] + @param resources: Controller resources + @type resources: [str] + @param hardware_interface Controller hardware interface where to look for + resources. If specified, the requested resources will only be searched for + in this interface. If unspecified, all controller hardware interfaces will + be searched for; i.e., if a controller claims resources from multiple + interfaces, the method will succeed if _any_ interface contains the + requested resources (any or all, depending on the value of C{match_any}). + Specifying this parameter allows finer control over determining which + interfaces claim specific resources. + @param match_any: If set to False, all elements in C{resources} must + be claimed by the interface specified in C{hardware_interface} (or _any_ + interface, if C{hardware_interface} is unspecified) for a positive match. + Note that a controller's resources can contain additional entries than + those in C{resources}). + If set to True, at least one element in C{resources} must be claimed by + the interface specified in C{hardware_interface} (or _any_ interface, if + C{hardware_interface} is unspecified) for a positive match. + @type match_any: bool + @return: Controllers matching the specified hardware interface + @rtype: [controller_manager_msgs/ControllerState] + """ + list_out = [] + for ctrl in ctrl_list: + for resource_set in ctrl.claimed_resources: + if (hardware_interface is None or + hardware_interface == resource_set.hardware_interface): + for res in resources: + add_ctrl = not match_any # Initial flag value + if res in resource_set.resources: + if match_any: # One hit: enough to accept controller + add_ctrl = True + break + else: + if not match_any: # One miss: enough to discard controller + add_ctrl = False + break + if add_ctrl: + list_out.append(ctrl) + break + return list_out + + +def _filter_by_attr(list_in, attr_name, attr_val, match_substring=False): + """Filter input list by the value of its elements' attributes.""" + list_out = [] + for val in list_in: + if match_substring: + if attr_val in getattr(val, attr_name): + list_out.append(val) + else: + if getattr(val, attr_name) == attr_val: + list_out.append(val) + return list_out + +############################################################################### +# +# Convenience methods for finding potential controller configurations +# +############################################################################### + +# def get_rosparam_controller_names(namespace='/'): +# """ +# Get list of ROS parameter names that potentially represent a controller +# configuration. + +# Example usage: +# - Assume the following parameters exist in the ROS parameter: +# server: +# - C{/foo/type} +# - C{/xxx/type/xxx} +# - C{/ns/bar/type} +# - C{/ns/yyy/type/yyy} +# - The potential controllers found by this method are: + +# >>> names = get_rosparam_controller_names() # returns ['foo'] +# >>> names_ns = get_rosparam_controller_names('/ns') # returns ['bar'] + +# @param namespace: Namespace where to look for controllers. +# @type namespace: str +# @return: Sorted list of ROS parameter names. +# @rtype: [str] +# """ +# import rosparam +# list_out = [] +# all_params = rosparam.list_params(namespace) +# for param in all_params: +# # Remove namespace from parameter string +# if not namespace or namespace[-1] != '/': +# namespace += '/' +# param_no_ns = param.split(namespace, 1)[1] + +# # Check if parameter corresponds to a controller configuration +# param_split = param_no_ns.split('/') +# if (len(param_split) == 2 and param_split[1] == 'type'): +# list_out.append(param_split[0]) # It does! +# return sorted(list_out) diff --git a/rqt_joint_trajectory_controller/setup.cfg b/rqt_joint_trajectory_controller/setup.cfg new file mode 100644 index 0000000000..4c0b982e25 --- /dev/null +++ b/rqt_joint_trajectory_controller/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/rqt_joint_trajectory_controller +[install] +install-scripts=$base/lib/rqt_joint_trajectory_controller diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py new file mode 100644 index 0000000000..584655963d --- /dev/null +++ b/rqt_joint_trajectory_controller/setup.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python + +from setuptools import setup +from glob import glob + +package_name = "rqt_joint_trajectory_controller" + +setup( + name=package_name, + version="0.0.1", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/resource", glob("resource/*.*")), + ("share/" + package_name, ["plugin.xml"]), + + ], + install_requires=["setuptools"], + zip_safe=True, + author="Noel Jimenez Garcia", + author_email="noel.jimenez@pal-robotics.com", + maintainer="Bence Magyar", + maintainer_email="bence.magyar.robotics@gmail.com", + license="Apache License, Version 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "rqt_joint_trajectory_controller = \ + rqt_joint_trajectory_controller.rqt_joint_trajectory_controller:main", + ], + }, +) From abdbae9ab439f92d185ce89ecc1bdbc6b73ea1b2 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sat, 30 Jul 2022 01:20:09 -0600 Subject: [PATCH 004/126] Formatting changes from pre-commit (#400) Signed-off-by: Tyler Weaver --- diff_drive_controller/src/diff_drive_controller.cpp | 4 ++-- .../gripper_controllers/gripper_action_controller.hpp | 2 +- .../gripper_controllers/gripper_action_controller_impl.hpp | 2 +- .../rqt_joint_trajectory_controller/double_editor.py | 2 +- .../joint_trajectory_controller.py | 7 +++---- .../rqt_joint_trajectory_controller/utils.py | 2 +- 6 files changed, 9 insertions(+), 10 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index aec603ad22..48ebdc9fb7 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -77,8 +77,8 @@ controller_interface::CallbackReturn DiffDriveController::on_init() auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); - publish_limited_velocity_ = auto_declare("publish_limited_velocity", - publish_limited_velocity_); + publish_limited_velocity_ = + auto_declare("publish_limited_velocity", publish_limited_velocity_); auto_declare("velocity_rolling_window_size", 10); use_stamped_vel_ = auto_declare("use_stamped_vel", use_stamped_vel_); diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 98ccbbf5d1..d5999002f4 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -162,7 +162,7 @@ class GripperActionController : public controller_interface::ControllerInterface stall_velocity_threshold_; ///< Stall related parameters double default_max_effort_; ///< Max allowed effort double goal_tolerance_; - bool allow_stalling_; ///< If gripper stalls when moving to goal is considered successful + bool allow_stalling_; ///< If gripper stalls when moving to goal is considered successful /** * \brief Check for success and publish appropriate result and feedback. diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index f1d270b52f..0b5f791e78 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -181,7 +181,7 @@ void GripperActionController::check_for_success( pre_alloc_result_->position = current_position; pre_alloc_result_->reached_goal = false; pre_alloc_result_->stalled = true; - if(allow_stalling_) + if (allow_stalling_) { RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Returning success."); rt_active_goal_->setSucceeded(pre_alloc_result_); diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py index 23a1f2c806..3fdde9daf9 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py @@ -49,7 +49,7 @@ class DoubleEditor(QWidget): valueChanged = Signal(float) def __init__(self, min_val, max_val): - super(DoubleEditor, self).__init__() + super().__init__() # Preconditions assert min_val < max_val diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index f3c98f9fe0..7cb9ea5346 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -14,7 +14,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from __future__ import print_function import os import rclpy import threading @@ -34,7 +33,7 @@ from .update_combo import update_combo # TODO: -# - Better UI suppor for continuous joints (see DoubleEditor TODO) +# - Better UI support for continuous joints (see DoubleEditor TODO) # - Can we load controller joints faster?, it's currently pretty slow # - If URDF is reloaded, allow to reset the whole plugin? # - Allow to configure: @@ -99,7 +98,7 @@ class JointTrajectoryController(Plugin): jointStateChanged = Signal([dict]) def __init__(self, context): - super(JointTrajectoryController, self).__init__(context) + super().__init__(context) self.setObjectName('JointTrajectoryController') self._node = rclpy.node.Node("rqt_joint_trajectory_controller") self._executor = None @@ -204,7 +203,7 @@ def restore_settings(self, plugin_settings, instance_settings): try: idx = cm_list.index(cm_ns) cm_combo.setCurrentIndex(idx) - # Resore last session's controller, if running + # Restore last session's controller, if running self._update_jtc_list() jtc_name = instance_settings.value('jtc_name') jtc_combo = self._widget.jtc_combo diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py index e2b6ffa438..a84eb2a5f8 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py @@ -16,7 +16,7 @@ # NOTE: The Python API contained in this file is considered UNSTABLE and # subject to change. -# No backwards compatibility guarrantees are provided at this moment. +# No backwards compatibility guarantees are provided at this moment. import rclpy From a6937c573ab71c082340bfeccac9320c8014ba9c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 30 Jul 2022 11:08:20 +0100 Subject: [PATCH 005/126] Make JTC callbacks methods with clear names (#397) #abi-breaking MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../joint_trajectory_controller.hpp | 10 ++-- .../src/joint_trajectory_controller.cpp | 52 ++++++++----------- 2 files changed, 30 insertions(+), 32 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 13e5e3ff5f..4c2f612112 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -205,15 +205,19 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::TimerBase::SharedPtr goal_handle_timer_; rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); + // callback for topic interface + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void topic_callback(const std::shared_ptr msg); + // callbacks for action_server_ JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_action::GoalResponse goal_callback( + rclcpp_action::GoalResponse goal_received_callback( const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); JOINT_TRAJECTORY_CONTROLLER_PUBLIC - rclcpp_action::CancelResponse cancel_callback( + rclcpp_action::CancelResponse goal_cancelled_callback( const std::shared_ptr> goal_handle); JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void feedback_setup_callback( + void goal_accepted_callback( std::shared_ptr> goal_handle); // fill trajectory_msg so it matches joints controlled by this controller diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 235681b069..e04f66754c 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -697,30 +697,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( allow_integration_in_goal_trajectories_ = get_node()->get_parameter("allow_integration_in_goal_trajectories").get_value(); - // subscriber callback - // non realtime - // TODO(karsten): check if traj msg and point time are valid - auto callback = [this](const std::shared_ptr msg) -> void { - if (!validate_trajectory_msg(*msg)) - { - return; - } - - // http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement - // always replace old msg with new one for now - if (subscriber_is_active_) - { - add_new_trajectory_msg(msg); - } - }; - - // TODO(karsten1987): create subscriber with subscription deactivated joint_command_subscriber_ = get_node()->create_subscription( - "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback); - - // TODO(karsten1987): no lifecycle for subscriber yet - // joint_command_subscriber_->on_activate(); + "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), + std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1)); // State publisher RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate_); @@ -774,9 +754,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_node()->get_node_base_interface(), get_node()->get_node_clock_interface(), get_node()->get_node_logging_interface(), get_node()->get_node_waitables_interface(), std::string(get_node()->get_name()) + "/follow_joint_trajectory", - std::bind(&JointTrajectoryController::goal_callback, this, _1, _2), - std::bind(&JointTrajectoryController::cancel_callback, this, _1), - std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1)); + std::bind(&JointTrajectoryController::goal_received_callback, this, _1, _2), + std::bind(&JointTrajectoryController::goal_cancelled_callback, this, _1), + std::bind(&JointTrajectoryController::goal_accepted_callback, this, _1)); resize_joint_trajectory_point(state_current_, dof_); resize_joint_trajectory_point(state_desired_, dof_); @@ -857,7 +837,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( last_commanded_state_ = state; } - // TODO(karsten1987): activate subscriptions of subscriber return CallbackReturn::SUCCESS; } @@ -988,7 +967,22 @@ void JointTrajectoryController::publish_state( } } -rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( +void JointTrajectoryController::topic_callback( + const std::shared_ptr msg) +{ + if (!validate_trajectory_msg(*msg)) + { + return; + } + // http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement + // always replace old msg with new one for now + if (subscriber_is_active_) + { + add_new_trajectory_msg(msg); + } +}; + +rclcpp_action::GoalResponse JointTrajectoryController::goal_received_callback( const rclcpp_action::GoalUUID &, std::shared_ptr goal) { RCLCPP_INFO(get_node()->get_logger(), "Received new action goal"); @@ -1010,7 +1004,7 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_callback( return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } -rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( +rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback( const std::shared_ptr> goal_handle) { RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); @@ -1034,7 +1028,7 @@ rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback( return rclcpp_action::CancelResponse::ACCEPT; } -void JointTrajectoryController::feedback_setup_callback( +void JointTrajectoryController::goal_accepted_callback( std::shared_ptr> goal_handle) { // Update new trajectory From 2ee11f5dd65bb0b8f018223c4f3e6700145e5a33 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 1 Aug 2022 07:44:55 +0100 Subject: [PATCH 006/126] Bring version up to date --- rqt_joint_trajectory_controller/package.xml | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 343f733339..a55bcf6a28 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,17 +4,13 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.5.0 + 2.9.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar - Mathias Lüdtke - Enrique Fernandez Apache-2.0 - http://wiki.ros.org/rqt_joint_trajectory_controller - Adolfo Rodriguez Tsouroukdissian Noel Jimenez Garcia From d1fafffecd2d4a9f88e8638def8431d3d2a343b2 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 1 Aug 2022 07:46:46 +0100 Subject: [PATCH 007/126] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 6 ++ effort_controllers/CHANGELOG.rst | 3 + force_torque_sensor_broadcaster/CHANGELOG.rst | 3 + forward_command_controller/CHANGELOG.rst | 3 + gripper_controllers/CHANGELOG.rst | 6 ++ imu_sensor_broadcaster/CHANGELOG.rst | 3 + joint_state_broadcaster/CHANGELOG.rst | 3 + joint_trajectory_controller/CHANGELOG.rst | 6 ++ position_controllers/CHANGELOG.rst | 3 + ros2_controllers/CHANGELOG.rst | 3 + ros2_controllers_test_nodes/CHANGELOG.rst | 3 + rqt_joint_trajectory_controller/CHANGELOG.rst | 84 +++++++++++++++++++ velocity_controllers/CHANGELOG.rst | 3 + 13 files changed, 129 insertions(+) create mode 100644 rqt_joint_trajectory_controller/CHANGELOG.rst diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 54d3e19561..15853cbebc 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Formatting changes from pre-commit (`#400 `_) +* Parameter loading fixup in diff_drive and gripper controllers (`#385 `_) +* Contributors: Andy Zelenak, Tyler Weaver + 2.9.0 (2022-07-14) ------------------ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index d642cba146..b75d868049 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 4c6cb3cf70..1bb0bc517f 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index ee4131d576..4d79367ca8 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 5c9c2bcc8e..6d100fcd90 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Formatting changes from pre-commit (`#400 `_) +* Parameter loading fixup in diff_drive and gripper controllers (`#385 `_) +* Contributors: Andy Zelenak, Tyler Weaver + 2.9.0 (2022-07-14) ------------------ * Allow gripper stalling when moving to goal (`#355 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 13014b5a62..bf35f55674 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 862c663a5d..5efdc7f325 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index ca856cdc28..9b5d12042e 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Make JTC callbacks methods with clear names (`#397 `_) #abi-breaking +* Use system time in all tests to avoid error with different time sources. (`#334 `_) +* Contributors: Bence Magyar, Denis Štogl + 2.9.0 (2022-07-14) ------------------ * Add option to skip interpolation in the joint trajectory controller (`#374 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index ec225f9c9f..444440d3f2 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 0aa1a78d34..1f1d8ffc17 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index b30e88623f..677f03c8b6 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst new file mode 100644 index 0000000000..8c462f5e51 --- /dev/null +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -0,0 +1,84 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rqt_joint_trajectory_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Formatting changes from pre-commit (`#400 `_) +* port rqt_joint_trajectory_controller to ros2 (`#356 `_) +* Contributors: Bence Magyar, Noel Jiménez García, Tyler Weaver + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 24e4c10e5a..40e6d0619f 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.9.0 (2022-07-14) ------------------ From a37fffec068d6dc3600af5f0bbd896ad590822b0 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 1 Aug 2022 07:47:11 +0100 Subject: [PATCH 008/126] 2.10.0 --- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 28 files changed, 41 insertions(+), 41 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 15853cbebc..f582845fe7 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- * Formatting changes from pre-commit (`#400 `_) * Parameter loading fixup in diff_drive and gripper controllers (`#385 `_) * Contributors: Andy Zelenak, Tyler Weaver diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index df90159f64..ae2696d998 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.9.0 + 2.10.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index b75d868049..30863470a1 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index d766cdbef3..0e415ba867 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.9.0 + 2.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 1bb0bc517f..790834ea9e 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index d5b3e9a3ea..c857ab92ab 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.9.0 + 2.10.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 4d79367ca8..16fac9b49f 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index d9d60c95cd..3c81a32680 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.9.0 + 2.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 6d100fcd90..98df8e9bd7 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- * Formatting changes from pre-commit (`#400 `_) * Parameter loading fixup in diff_drive and gripper controllers (`#385 `_) * Contributors: Andy Zelenak, Tyler Weaver diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 27586a3e97..42ba2dc7f6 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.9.0 + 2.10.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index bf35f55674..acd580d511 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 6a4c01c3c5..52ba10b381 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.9.0 + 2.10.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 5efdc7f325..58295ee22b 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9b546b81a3..0048dc4c2b 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.9.0 + 2.10.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 9b5d12042e..f96bd2c9b4 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- * Make JTC callbacks methods with clear names (`#397 `_) #abi-breaking * Use system time in all tests to avoid error with different time sources. (`#334 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 047e778160..02b27cdf83 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.9.0 + 2.10.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 444440d3f2..66cb16f92a 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index aa29cc5ea5..8a2fee3598 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.9.0 + 2.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 1f1d8ffc17..adf3ff3538 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 0f6f2b31d8..8d1cca6666 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.9.0 + 2.10.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 677f03c8b6..c7b22b0183 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index c6c5a6b457..924e7b0bca 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.9.0 + 2.10.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index e10da150bb..4c8fba2fab 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.9.0", + version="2.10.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 8c462f5e51..3607695579 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- * Formatting changes from pre-commit (`#400 `_) * port rqt_joint_trajectory_controller to ros2 (`#356 `_) * Contributors: Bence Magyar, Noel Jiménez García, Tyler Weaver diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index a55bcf6a28..62e703bc3b 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.9.0 + 2.10.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 584655963d..ab2db18405 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="0.0.1", + version="2.10.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 40e6d0619f..6bc28fc062 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.10.0 (2022-08-01) +------------------- 2.9.0 (2022-07-14) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 58d2f7f81b..fac41cadbf 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.9.0 + 2.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 4138b734c3c1389fd715408ead3d24485d3e4c25 Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Mon, 1 Aug 2022 23:36:27 -0700 Subject: [PATCH 009/126] Use explicit type in joint_state_broadcaster test (#403) This use of `auto` is causing a static assert on RHEL. Explicitly specifying the type seems to resolve the failure and allow the test to be compiled. --- joint_state_broadcaster/test/test_joint_state_broadcaster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index ecccaa4b22..7f95e232e2 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -683,7 +683,7 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( ASSERT_TRUE(subscription->take(dynamic_joint_state_msg, msg_info)); const size_t NUM_JOINTS = 3; - const auto INTERFACE_NAMES = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; + const std::vector INTERFACE_NAMES = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); // the order in the message may be different // we only check that all values in this test are present in the message From 279ba38aa56168d4e6eaf698b177f1848944cf00 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 3 Aug 2022 19:00:11 +0200 Subject: [PATCH 010/126] Tricycle controller (#345) --- ros2_controllers/package.xml | 1 + tricycle_controller/CMakeLists.txt | 118 ++++ .../config/tricycle_drive_controller.yaml | 43 ++ tricycle_controller/doc/userdoc.rst | 24 + .../include/tricycle_controller/odometry.hpp | 75 ++ .../tricycle_controller/steering_limiter.hpp | 98 +++ .../tricycle_controller/traction_limiter.hpp | 104 +++ .../tricycle_controller.hpp | 185 +++++ .../tricycle_controller/visibility_control.h | 53 ++ tricycle_controller/package.xml | 35 + tricycle_controller/src/odometry.cpp | 121 ++++ tricycle_controller/src/steering_limiter.cpp | 116 +++ tricycle_controller/src/traction_limiter.cpp | 142 ++++ .../src/tricycle_controller.cpp | 668 ++++++++++++++++++ .../test/config/test_tricycle_controller.yaml | 20 + .../test/test_load_tricycle_controller.cpp | 43 ++ .../test/test_tricycle_controller.cpp | 350 +++++++++ tricycle_controller/tricycle_controller.xml | 7 + 18 files changed, 2203 insertions(+) create mode 100644 tricycle_controller/CMakeLists.txt create mode 100644 tricycle_controller/config/tricycle_drive_controller.yaml create mode 100644 tricycle_controller/doc/userdoc.rst create mode 100644 tricycle_controller/include/tricycle_controller/odometry.hpp create mode 100644 tricycle_controller/include/tricycle_controller/steering_limiter.hpp create mode 100644 tricycle_controller/include/tricycle_controller/traction_limiter.hpp create mode 100644 tricycle_controller/include/tricycle_controller/tricycle_controller.hpp create mode 100644 tricycle_controller/include/tricycle_controller/visibility_control.h create mode 100644 tricycle_controller/package.xml create mode 100644 tricycle_controller/src/odometry.cpp create mode 100644 tricycle_controller/src/steering_limiter.cpp create mode 100644 tricycle_controller/src/traction_limiter.cpp create mode 100644 tricycle_controller/src/tricycle_controller.cpp create mode 100644 tricycle_controller/test/config/test_tricycle_controller.yaml create mode 100644 tricycle_controller/test/test_load_tricycle_controller.cpp create mode 100644 tricycle_controller/test/test_tricycle_controller.cpp create mode 100644 tricycle_controller/tricycle_controller.xml diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 8d1cca6666..aebcf25ba6 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -19,6 +19,7 @@ joint_state_broadcaster joint_trajectory_controller position_controllers + tricycle_controller velocity_controllers diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt new file mode 100644 index 0000000000..c499bca983 --- /dev/null +++ b/tricycle_controller/CMakeLists.txt @@ -0,0 +1,118 @@ +cmake_minimum_required(VERSION 3.8) +project(tricycle_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcpputils REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(ackermann_msgs REQUIRED) + +add_library( + ${PROJECT_NAME} + SHARED + src/tricycle_controller.cpp + src/odometry.cpp + src/traction_limiter.cpp + src/steering_limiter.cpp +) +target_include_directories( + ${PROJECT_NAME} + PRIVATE + include +) + +ament_target_dependencies( + ${PROJECT_NAME} + ackermann_msgs + builtin_interfaces + controller_interface + geometry_msgs + hardware_interface + nav_msgs + std_srvs + pluginlib + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs +) + +pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) +install( + DIRECTORY + include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_tricycle_controller + test/test_tricycle_controller.cpp + ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml) + target_include_directories(test_tricycle_controller PRIVATE include) + target_link_libraries(test_tricycle_controller + ${PROJECT_NAME} + ) + + ament_target_dependencies(test_tricycle_controller + geometry_msgs + hardware_interface + nav_msgs + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs + ) + + ament_add_gmock( + test_load_tricycle_controller + test/test_load_tricycle_controller.cpp + ) + target_include_directories(test_load_tricycle_controller PRIVATE include) + ament_target_dependencies(test_load_tricycle_controller + controller_manager + ros2_control_test_assets + ) +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_dependencies( + controller_interface + geometry_msgs + hardware_interface + rclcpp + rclcpp_lifecycle +) + +ament_package() diff --git a/tricycle_controller/config/tricycle_drive_controller.yaml b/tricycle_controller/config/tricycle_drive_controller.yaml new file mode 100644 index 0000000000..4eabf615ef --- /dev/null +++ b/tricycle_controller/config/tricycle_drive_controller.yaml @@ -0,0 +1,43 @@ +tricycle_controller: + ros__parameters: + # Model + traction_joint_name: traction_joint # Name of traction joint in URDF + steering_joint_name: steering_joint # Name of steering joint in URDF + wheel_radius: 0.0 # Radius of front wheel + wheelbase: 0.0 # Distance between center of back wheels and front wheel + + # Odometry + odom_frame_id: odom + base_frame_id: base_link + publish_rate: 50.0 # publish rate of odom and tf + open_loop: false # if True, uses cmd_vel instead of hardware interface feedback to compute odometry + enable_odom_tf: true # If True, publishes odom<-base_link TF + odom_only_twist: false # If True, publishes on /odom only linear.x and angular.z; Useful for computing odometry in another node, e.g robot_localization's ekf + pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source + twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source + velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom + + # Rate Limiting + traction: # All values should be positive + # min_velocity: 0.0 + # max_velocity: 1000.0 + # min_acceleration: 0.0 + max_acceleration: 5.0 + # min_deceleration: 0.0 + max_deceleration: 8.0 + # min_jerk: 0.0 + # max_jerk: 1000.0 + steering: + min_position: -1.57 + max_position: 1.57 + # min_velocity: 0.0 + max_velocity: 1.0 + # min_acceleration: 0.0 + # max_acceleration: 1000.0 + + # cmd_vel input + cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received + use_stamped_vel: false # Set to True if using TwistStamped. + + # Debug + publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s. diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst new file mode 100644 index 0000000000..2271b8c9cf --- /dev/null +++ b/tricycle_controller/doc/userdoc.rst @@ -0,0 +1,24 @@ +.. _tricycle_controller_userdoc: + +tricycle_controller +===================== + +Controller for mobile robots with tricycle drive. +Input for control are robot base_link twist commands which are translated to traction and steering +commands for the tricycle drive base. Odometry is computed from hardware feedback and published. + +Velocity commands +----------------- + +The controller works with a velocity twist from which it extracts +the x component of the linear velocity and the z component of the angular velocity. +Velocities on other components are ignored. + + +Other features +-------------- + + Realtime-safe implementation. + Odometry publishing + Velocity, acceleration and jerk limits + Automatic stop after command timeout \ No newline at end of file diff --git a/tricycle_controller/include/tricycle_controller/odometry.hpp b/tricycle_controller/include/tricycle_controller/odometry.hpp new file mode 100644 index 0000000000..4a958a93c6 --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/odometry.hpp @@ -0,0 +1,75 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#ifndef TRICYCLE_CONTROLLER__ODOMETRY_HPP_ +#define TRICYCLE_CONTROLLER__ODOMETRY_HPP_ + +#include + +#include "rclcpp/time.hpp" +#include "rcppmath/rolling_mean_accumulator.hpp" + +namespace tricycle_controller +{ +class Odometry +{ +public: + explicit Odometry(size_t velocity_rolling_window_size = 10); + + bool update(double left_vel, double right_vel, const rclcpp::Duration & dt); + void updateOpenLoop(double linear, double angular, const rclcpp::Duration & dt); + void resetOdometry(); + + double getX() const { return x_; } + double getY() const { return y_; } + double getHeading() const { return heading_; } + double getLinear() const { return linear_; } + double getAngular() const { return angular_; } + + void setWheelParams(double wheel_separation, double wheel_radius); + void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); + +private: + using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; + + void integrateRungeKutta2(double linear, double angular); + void integrateExact(double linear, double angular); + void resetAccumulators(); + + // Current pose: + double x_; // [m] + double y_; // [m] + double heading_; // [rad] + + // Current velocity: + double linear_; // [m/s] + double angular_; // [rad/s] + + // Wheel kinematic parameters [m]: + double wheelbase_; + double wheel_radius_; + + // Rolling mean accumulators for the linear and angular velocities: + size_t velocity_rolling_window_size_; + RollingMeanAccumulator linear_accumulator_; + RollingMeanAccumulator angular_accumulator_; +}; + +} // namespace tricycle_controller + +#endif // TRICYCLE_CONTROLLER__ODOMETRY_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/steering_limiter.hpp b/tricycle_controller/include/tricycle_controller/steering_limiter.hpp new file mode 100644 index 0000000000..51ecac4cbd --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/steering_limiter.hpp @@ -0,0 +1,98 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#ifndef TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_ +#define TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_ + +#include + +namespace tricycle_controller +{ + +class SteeringLimiter +{ +public: + /** + * \brief Constructor + * \param [in] min_position Minimum position [m] or [rad] + * \param [in] max_position Maximum position [m] or [rad] + * \param [in] min_velocity Minimum velocity [m/s] or [rad/s] + * \param [in] max_velocity Maximum velocity [m/s] or [rad/s] + * \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2] + * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] + */ + SteeringLimiter( + double min_position = NAN, double max_position = NAN, + double min_velocity = NAN, double max_velocity = NAN, + double min_acceleration = NAN, double max_acceleration = NAN); + + /** + * \brief Limit the position, velocity and acceleration + * \param [in, out] p position [m] or [rad] + * \param [in] p0 Previous position to p [m] or [rad] + * \param [in] p1 Previous position to p0 [m] or [rad] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit(double & p, double p0, double p1, double dt); + + /** + * \brief Limit the jerk + * \param [in, out] p position [m] or [rad] + * \param [in] p0 Previous position to p [m] or [rad] + * \param [in] p1 Previous position to p0 [m] or [rad] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_position(double & p); + + /** + * \brief Limit the velocity + * \param [in, out] p position [m] + * \return Limiting factor (1.0 if none) + */ + double limit_velocity(double & p, double p0, double dt); + + /** + * \brief Limit the acceleration + * \param [in, out] p Position [m] or [rad] + * \param [in] p0 Previous position [m] or [rad] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_acceleration(double & p, double p0, double p1, double dt); + +private: + + // Position limits: + double min_position_; + double max_position_; + + // Velocity limits: + double min_velocity_; + double max_velocity_; + + // Acceleration limits: + double min_acceleration_; + double max_acceleration_; + +}; + +} // namespace tricycle_controller + +#endif // TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp new file mode 100644 index 0000000000..4c0f297ee8 --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp @@ -0,0 +1,104 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#ifndef TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_ +#define TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_ + +#include + +namespace tricycle_controller +{ + +class TractionLimiter +{ +public: + /** + * \brief Constructor + * \param [in] min_velocity Minimum velocity [m/s] or [rad/s] + * \param [in] max_velocity Maximum velocity [m/s] or [rad/s] + * \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2] + * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] + * \param [in] min_deceleration Minimum deceleration [m/s^2] or [rad/s^2] + * \param [in] max_deceleration Maximum deceleration [m/s^2] or [rad/s^2] + * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 + * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + */ + TractionLimiter( + double min_velocity = NAN, double max_velocity = NAN, + double min_acceleration = NAN, double max_acceleration = NAN, + double min_deceleration = NAN, double max_deceleration = NAN, + double min_jerk = NAN, double max_jerk = NAN); + + /** + * \brief Limit the velocity and acceleration + * \param [in, out] v Velocity [m/s] or [rad/s] + * \param [in] v0 Previous velocity to v [m/s] or [rad/s] + * \param [in] v1 Previous velocity to v0 [m/s] or [rad/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit(double & v, double v0, double v1, double dt); + + /** + * \brief Limit the velocity + * \param [in, out] v Velocity [m/s] or [rad/s] + * \return Limiting factor (1.0 if none) + */ + double limit_velocity(double & v); + + /** + * \brief Limit the acceleration + * \param [in, out] v Velocity [m/s] or [rad/s] + * \param [in] v0 Previous velocity [m/s] or [rad/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_acceleration(double & v, double v0, double dt); + + /** + * \brief Limit the jerk + * \param [in, out] v Velocity [m/s] or [rad/s] + * \param [in] v0 Previous velocity to v [m/s] or [rad/s] + * \param [in] v1 Previous velocity to v0 [m/s] or [rad/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control + */ + double limit_jerk(double & v, double v0, double v1, double dt); + +private: + // Velocity limits: + double min_velocity_; + double max_velocity_; + + // Acceleration limits: + double min_acceleration_; + double max_acceleration_; + + // Deceleration limits: + double min_deceleration_; + double max_deceleration_; + + // Jerk limits: + double min_jerk_; + double max_jerk_; +}; + +} // namespace tricycle_controller + +#endif // TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp new file mode 100644 index 0000000000..c5ba1c62e3 --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -0,0 +1,185 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#ifndef TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_ +#define TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "ackermann_msgs/msg/ackermann_drive.hpp" +#include "controller_interface/controller_interface.hpp" +#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 "std_srvs/srv/empty.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_box.h" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "tf2_msgs/msg/tf_message.hpp" +#include "tricycle_controller/odometry.hpp" +#include "tricycle_controller/traction_limiter.hpp" +#include "tricycle_controller/steering_limiter.hpp" +#include "tricycle_controller/visibility_control.h" + +namespace tricycle_controller +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class TricycleController : public controller_interface::ControllerInterface +{ + using Twist = geometry_msgs::msg::Twist; + using TwistStamped = geometry_msgs::msg::TwistStamped; + using AckermannDrive = ackermann_msgs::msg::AckermannDrive; + +public: + TRICYCLE_CONTROLLER_PUBLIC + TricycleController(); + + TRICYCLE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + TRICYCLE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + TRICYCLE_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_init() override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; + +protected: + struct TractionHandle + { + std::reference_wrapper velocity_state; + std::reference_wrapper velocity_command; + }; + struct SteeringHandle + { + std::reference_wrapper position_state; + std::reference_wrapper position_command; + }; + + CallbackReturn get_traction( + const std::string & traction_joint_name, std::vector & joint); + CallbackReturn get_steering( + const std::string & steering_joint_name, std::vector & joint); + double convert_trans_rot_vel_to_steering_angle(double v, double omega, double wheelbase); + std::tuple twist_to_ackermann(double linear_command, double angular_command); + + std::string traction_joint_name_; + std::string steering_joint_name_; + + // HACK: put into vector to avoid initializing structs because they have no default constructors + std::vector traction_joint_; + std::vector steering_joint_; + + struct WheelParams + { + double wheelbase = 0.0; + double radius = 0.0; + } wheel_params_; + + struct OdometryParams + { + bool open_loop = false; + bool enable_odom_tf = false; + bool odom_only_twist = false; // for doing the pose integration in seperate node + std::string base_frame_id = "base_link"; + std::string odom_frame_id = "odom"; + std::array pose_covariance_diagonal; + std::array twist_covariance_diagonal; + } odom_params_; + + bool publish_ackermann_command_ = false; + std::shared_ptr> ackermann_command_publisher_ = nullptr; + std::shared_ptr> + realtime_ackermann_command_publisher_ = nullptr; + + Odometry odometry_; + + std::shared_ptr> odometry_publisher_ = nullptr; + std::shared_ptr> + realtime_odometry_publisher_ = nullptr; + + std::shared_ptr> odometry_transform_publisher_ = + nullptr; + std::shared_ptr> + realtime_odometry_transform_publisher_ = nullptr; + + // Timeout to consider cmd_vel commands old + std::chrono::milliseconds cmd_vel_timeout_{500}; + + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr + velocity_command_unstamped_subscriber_ = nullptr; + + realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + + rclcpp::Service::SharedPtr reset_odom_service_; + + std::queue previous_commands_; // last two commands + + // speed limiters + TractionLimiter limiter_traction_; + SteeringLimiter limiter_steering_; + + // publish rate limiter + double publish_rate_ = 50.0; + rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); + rclcpp::Time previous_publish_timestamp_{0}; + + bool is_halted = false; + bool use_stamped_vel_ = true; + + void reset_odometry( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr res); + bool reset(); + void halt(); +}; +} // namespace tricycle_controller +#endif // TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/visibility_control.h b/tricycle_controller/include/tricycle_controller/visibility_control.h new file mode 100644 index 0000000000..bc9b34898b --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/visibility_control.h @@ -0,0 +1,53 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#ifndef TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define TRICYCLE_CONTROLLER_EXPORT __attribute__((dllexport)) +#define TRICYCLE_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define TRICYCLE_CONTROLLER_EXPORT __declspec(dllexport) +#define TRICYCLE_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef TRICYCLE_CONTROLLER_BUILDING_DLL +#define TRICYCLE_CONTROLLER_PUBLIC TRICYCLE_CONTROLLER_EXPORT +#else +#define TRICYCLE_CONTROLLER_PUBLIC TRICYCLE_CONTROLLER_IMPORT +#endif +#define TRICYCLE_CONTROLLER_PUBLIC_TYPE TRICYCLE_CONTROLLER_PUBLIC +#define TRICYCLE_CONTROLLER_LOCAL +#else +#define TRICYCLE_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define TRICYCLE_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define TRICYCLE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define TRICYCLE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define TRICYCLE_CONTROLLER_PUBLIC +#define TRICYCLE_CONTROLLER_LOCAL +#endif +#define TRICYCLE_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml new file mode 100644 index 0000000000..9dec765d60 --- /dev/null +++ b/tricycle_controller/package.xml @@ -0,0 +1,35 @@ + + + + tricycle_controller + 1.0.0 + Controller for a tricycle drive mobile base + Tony Najjar + Apache License 2.0 + Tony Najjar + + ament_cmake + + controller_interface + geometry_msgs + hardware_interface + nav_msgs + std_srvs + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs + ackermann_msgs + + pluginlib + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + \ No newline at end of file diff --git a/tricycle_controller/src/odometry.cpp b/tricycle_controller/src/odometry.cpp new file mode 100644 index 0000000000..e0e670480a --- /dev/null +++ b/tricycle_controller/src/odometry.cpp @@ -0,0 +1,121 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#include "tricycle_controller/odometry.hpp" + +namespace tricycle_controller +{ +Odometry::Odometry(size_t velocity_rolling_window_size) +: x_(0.0), + y_(0.0), + heading_(0.0), + linear_(0.0), + angular_(0.0), + wheelbase_(0.0), + wheel_radius_(0.0), + velocity_rolling_window_size_(velocity_rolling_window_size), + linear_accumulator_(velocity_rolling_window_size), + angular_accumulator_(velocity_rolling_window_size) +{ +} + +bool Odometry::update(double Ws, double alpha, const rclcpp::Duration & dt) +{ + // using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf + double Vs = Ws * wheel_radius_; + double Vx = Vs * std::cos(alpha); + double theta_dot = Vs * std::sin(alpha) / wheelbase_; + + // Integrate odometry: + integrateExact(Vx * dt.seconds(), theta_dot * dt.seconds()); + + // Estimate speeds using a rolling mean to filter them out: + linear_accumulator_.accumulate(Vx); + angular_accumulator_.accumulate(theta_dot); + + linear_ = linear_accumulator_.getRollingMean(); + angular_ = angular_accumulator_.getRollingMean(); + + return true; +} + +void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Duration & dt) +{ + /// Save last linear and angular velocity: + linear_ = linear; + angular_ = angular; + + /// Integrate odometry: + integrateExact(linear * dt.seconds(), angular * dt.seconds()); +} + +void Odometry::resetOdometry() +{ + x_ = 0.0; + y_ = 0.0; + heading_ = 0.0; + resetAccumulators(); +} + +void Odometry::setWheelParams(double wheelbase, double wheel_radius) +{ + wheelbase_ = wheelbase; + wheel_radius_ = wheel_radius; +} + +void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) +{ + velocity_rolling_window_size_ = velocity_rolling_window_size; + + resetAccumulators(); +} + +void Odometry::integrateRungeKutta2(double linear, double angular) +{ + const double direction = heading_ + angular * 0.5; + + /// Runge-Kutta 2nd order integration: + x_ += linear * cos(direction); + y_ += linear * sin(direction); + heading_ += angular; +} + +void Odometry::integrateExact(double linear, double angular) +{ + if (fabs(angular) < 1e-6) + { + integrateRungeKutta2(linear, angular); + } + else + { + /// Exact integration (should solve problems when angular is zero): + const double heading_old = heading_; + const double r = linear / angular; + heading_ += angular; + x_ += r * (sin(heading_) - sin(heading_old)); + y_ += -r * (cos(heading_) - cos(heading_old)); + } +} + +void Odometry::resetAccumulators() +{ + linear_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); + angular_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); +} + +} // namespace tricycle_controller diff --git a/tricycle_controller/src/steering_limiter.cpp b/tricycle_controller/src/steering_limiter.cpp new file mode 100644 index 0000000000..6912144bd0 --- /dev/null +++ b/tricycle_controller/src/steering_limiter.cpp @@ -0,0 +1,116 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#include +#include + +#include "rcppmath/clamp.hpp" +#include "tricycle_controller/steering_limiter.hpp" + +namespace tricycle_controller +{ +SteeringLimiter::SteeringLimiter( + double min_position, double max_position, double min_velocity, double max_velocity, + double min_acceleration, double max_acceleration) +: min_position_(min_position), + max_position_(max_position), + min_velocity_(min_velocity), + max_velocity_(max_velocity), + min_acceleration_(min_acceleration), + max_acceleration_(max_acceleration) +{ + if (!std::isnan(min_position_) && std::isnan(max_position_)) max_position_ = -min_position_; + if (!std::isnan(max_position_) && std::isnan(min_position_)) min_position_ = -max_position_; + + if (!std::isnan(min_velocity_) && std::isnan(max_velocity_)) + max_velocity_ = 1000.0; // Arbitrarily big number + if (!std::isnan(max_velocity_) && std::isnan(min_velocity_)) min_velocity_ = 0.0; + + if (!std::isnan(min_acceleration_) && std::isnan(max_acceleration_)) max_acceleration_ = 1000.0; + if (!std::isnan(max_acceleration_) && std::isnan(min_acceleration_)) min_acceleration_ = 0.0; + + const std::string error = + "The positive limit will be applied to both directions. Setting different limits for positive " + "and negative directions is not supported. Actuators are " + "assumed to have the same constraints in both directions"; + + if (min_velocity_ < 0 || max_velocity_ < 0) + { + throw std::invalid_argument("Velocity cannot be negative." + error); + } + + if (min_acceleration_ < 0 || max_acceleration_ < 0) + { + throw std::invalid_argument("Acceleration cannot be negative." + error); + } +} + +double SteeringLimiter::limit(double & p, double p0, double p1, double dt) +{ + const double tmp = p; + + if (!std::isnan(min_acceleration_) && !std::isnan(max_acceleration_)) + limit_acceleration(p, p0, p1, dt); + if (!std::isnan(min_velocity_) && !std::isnan(max_velocity_)) limit_velocity(p, p0, dt); + if (!std::isnan(min_position_) && !std::isnan(max_position_)) limit_position(p); + + return tmp != 0.0 ? p / tmp : 1.0; +} + +double SteeringLimiter::limit_position(double & p) +{ + const double tmp = p; + p = rcppmath::clamp(p, min_position_, max_position_); + + return tmp != 0.0 ? p / tmp : 1.0; +} + +double SteeringLimiter::limit_velocity(double & p, double p0, double dt) +{ + const double tmp = p; + + const double dv_min = min_velocity_ * dt; + const double dv_max = max_velocity_ * dt; + + double dv = rcppmath::clamp((double)std::abs(p - p0), dv_min, dv_max); + dv *= (p - p0 >= 0 ? 1 : -1); + p = p0 + dv; + + return tmp != 0.0 ? p / tmp : 1.0; +} + +double SteeringLimiter::limit_acceleration(double & p, double p0, double p1, double dt) +{ + const double tmp = p; + + const double dv = p - p0; + const double dp0 = p0 - p1; + + const double dt2 = 2. * dt * dt; + + const double da_min = min_acceleration_ * dt2; + const double da_max = max_acceleration_ * dt2; + + double da = rcppmath::clamp((double)std::abs(dv - dp0), da_min, da_max); + da *= (dv - dp0 >= 0 ? 1 : -1); + p = p0 + dp0 + da; + + return tmp != 0.0 ? p / tmp : 1.0; +} + +} // namespace tricycle_controller diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp new file mode 100644 index 0000000000..c2a0d88ab4 --- /dev/null +++ b/tricycle_controller/src/traction_limiter.cpp @@ -0,0 +1,142 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#include +#include +#include + +#include "rcppmath/clamp.hpp" +#include "tricycle_controller/traction_limiter.hpp" + +namespace tricycle_controller +{ +TractionLimiter::TractionLimiter( + double min_velocity, double max_velocity, double min_acceleration, double max_acceleration, + double min_deceleration, double max_deceleration, double min_jerk, double max_jerk) +: min_velocity_(min_velocity), + max_velocity_(max_velocity), + min_acceleration_(min_acceleration), + max_acceleration_(max_acceleration), + min_deceleration_(min_deceleration), + max_deceleration_(max_deceleration), + min_jerk_(min_jerk), + max_jerk_(max_jerk) +{ + if (!std::isnan(min_velocity_) && std::isnan(max_velocity_)) + max_velocity_ = 1000.0; // Arbitrarily big number + if (!std::isnan(max_velocity_) && std::isnan(min_velocity_)) min_velocity_ = 0.0; + + if (!std::isnan(min_acceleration_) && std::isnan(max_acceleration_)) max_acceleration_ = 1000.0; + if (!std::isnan(max_acceleration_) && std::isnan(min_acceleration_)) min_acceleration_ = 0.0; + + if (!std::isnan(min_deceleration_) && std::isnan(max_deceleration_)) max_deceleration_ = 1000.0; + if (!std::isnan(max_deceleration_) && std::isnan(min_acceleration_)) min_deceleration_ = 0.0; + + if (!std::isnan(min_jerk_) && std::isnan(max_jerk_)) max_jerk_ = 1000.0; + if (!std::isnan(max_jerk_) && std::isnan(min_jerk_)) min_jerk_ = 0.0; + + const std::string error = + "The positive limit will be applied to both directions. Setting different limits for positive " + "and negative directions is not supported. Actuators are " + "assumed to have the same constraints in both directions"; + if (min_velocity_ < 0 || max_velocity_ < 0) + { + throw std::invalid_argument("Velocity cannot be negative." + error); + } + + if (min_acceleration_ < 0 || max_acceleration_ < 0) + { + throw std::invalid_argument("Acceleration cannot be negative." + error); + } + + if (min_deceleration_ < 0 || max_deceleration_ < 0) + { + throw std::invalid_argument("Deceleration cannot be negative." + error); + } + + if (min_jerk_ < 0 || max_jerk_ < 0) + { + throw std::invalid_argument("Jerk cannot be negative." + error); + } +} + +double TractionLimiter::limit(double & v, double v0, double v1, double dt) +{ + const double tmp = v; + + if (!std::isnan(min_jerk_) && !std::isnan(max_jerk_)) limit_jerk(v, v0, v1, dt); + if (!std::isnan(min_acceleration_) && !std::isnan(max_acceleration_)) + limit_acceleration(v, v0, dt); + if (!std::isnan(min_velocity_) && !std::isnan(max_velocity_)) limit_velocity(v); + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double TractionLimiter::limit_velocity(double & v) +{ + const double tmp = v; + + v = rcppmath::clamp((double)std::abs(v), min_velocity_, max_velocity_); + + v *= tmp >= 0 ? 1 : -1; + return tmp != 0.0 ? v / tmp : 1.0; +} + +double TractionLimiter::limit_acceleration(double & v, double v0, double dt) +{ + const double tmp = v; + + double dv_min; + double dv_max; + if (abs(v) >= abs(v0)) + { + dv_min = min_acceleration_ * dt; + dv_max = max_acceleration_ * dt; + } + else + { + dv_min = min_deceleration_ * dt; + dv_max = max_deceleration_ * dt; + } + double dv = rcppmath::clamp((double)std::abs(v - v0), dv_min, dv_max); + dv *= (v - v0 >= 0 ? 1 : -1); + v = v0 + dv; + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double TractionLimiter::limit_jerk(double & v, double v0, double v1, double dt) +{ + const double tmp = v; + + const double dv = v - v0; + const double dv0 = v0 - v1; + + const double dt2 = 2. * dt * dt; + + const double da_min = min_jerk_ * dt2; + const double da_max = max_jerk_ * dt2; + + double da = rcppmath::clamp((double)std::abs(dv - dv0), da_min, da_max); + da *= (dv - dv0 >= 0 ? 1 : -1); + v = v0 + dv0 + da; + + return tmp != 0.0 ? v / tmp : 1.0; +} + +} // namespace tricycle_controller diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp new file mode 100644 index 0000000000..7cec107a91 --- /dev/null +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -0,0 +1,668 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tricycle_controller/tricycle_controller.hpp" + +namespace +{ +constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; +constexpr auto DEFAULT_ACKERMANN_OUT_TOPIC = "~/cmd_ackermann"; +constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; +constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; +constexpr auto DEFAULT_RESET_ODOM_SERVICE = "~/reset_odometry"; +} // namespace + +namespace tricycle_controller +{ +using namespace std::chrono_literals; +using controller_interface::interface_configuration_type; +using controller_interface::InterfaceConfiguration; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using lifecycle_msgs::msg::State; + +TricycleController::TricycleController() : controller_interface::ControllerInterface() {} + +CallbackReturn TricycleController::on_init() +{ + try + { + // with the lifecycle node being initialized, we can declare parameters + auto_declare("traction_joint_name", std::string()); + auto_declare("steering_joint_name", std::string()); + + auto_declare("wheelbase", wheel_params_.wheelbase); + auto_declare("wheel_radius", wheel_params_.radius); + + auto_declare("odom_frame_id", odom_params_.odom_frame_id); + auto_declare("base_frame_id", odom_params_.base_frame_id); + auto_declare>("pose_covariance_diagonal", std::vector()); + auto_declare>("twist_covariance_diagonal", std::vector()); + auto_declare("open_loop", odom_params_.open_loop); + auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); + auto_declare("odom_only_twist", odom_params_.odom_only_twist); + + auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count()); + auto_declare("publish_ackermann_command", publish_ackermann_command_); + auto_declare("velocity_rolling_window_size", 10); + auto_declare("use_stamped_vel", use_stamped_vel_); + auto_declare("publish_rate", publish_rate_); + + auto_declare("traction.max_velocity", NAN); + auto_declare("traction.min_velocity", NAN); + auto_declare("traction.max_acceleration", NAN); + auto_declare("traction.min_acceleration", NAN); + auto_declare("traction.max_deceleration", NAN); + auto_declare("traction.min_deceleration", NAN); + auto_declare("traction.max_jerk", NAN); + auto_declare("traction.min_jerk", NAN); + + auto_declare("steering.max_position", NAN); + auto_declare("steering.min_position", NAN); + auto_declare("steering.max_velocity", NAN); + auto_declare("steering.min_velocity", NAN); + auto_declare("steering.max_acceleration", NAN); + auto_declare("steering.min_acceleration", NAN); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +InterfaceConfiguration TricycleController::command_interface_configuration() const +{ + InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); + command_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); + return command_interfaces_config; +} + +InterfaceConfiguration TricycleController::state_interface_configuration() const +{ + InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); + state_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); + return state_interfaces_config; +} + +controller_interface::return_type TricycleController::update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (get_state().id() == State::PRIMARY_STATE_INACTIVE) + { + if (!is_halted) + { + halt(); + is_halted = true; + } + return controller_interface::return_type::OK; + } + std::shared_ptr last_command_msg; + received_velocity_msg_ptr_.get(last_command_msg); + if (last_command_msg == nullptr) + { + RCLCPP_WARN(get_node()->get_logger(), "Velocity message received was a nullptr."); + return controller_interface::return_type::ERROR; + } + + const auto age_of_last_command = time - last_command_msg->header.stamp; + // Brake if cmd_vel has timeout, override the stored command + if (age_of_last_command > cmd_vel_timeout_) + { + last_command_msg->twist.linear.x = 0.0; + last_command_msg->twist.angular.z = 0.0; + } + + // command may be limited further by Limiters, + // without affecting the stored twist command + TwistStamped command = *last_command_msg; + double & linear_command = command.twist.linear.x; + double & angular_command = command.twist.angular.z; + double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s + double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians + + if (odom_params_.open_loop) + { + odometry_.updateOpenLoop(linear_command, angular_command, period); + } + else + { + if (std::isnan(Ws_read) || std::isnan(alpha_read)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Could not read feeback value"); + return controller_interface::return_type::ERROR; + } + odometry_.update(Ws_read, alpha_read, period); + } + + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getHeading()); + + if (previous_publish_timestamp_ + publish_period_ < time) + { + previous_publish_timestamp_ += publish_period_; + + if (realtime_odometry_publisher_->trylock()) + { + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.stamp = time; + if (!odom_params_.odom_only_twist) + { + odometry_message.pose.pose.position.x = odometry_.getX(); + odometry_message.pose.pose.position.y = odometry_.getY(); + odometry_message.pose.pose.orientation.x = orientation.x(); + odometry_message.pose.pose.orientation.y = orientation.y(); + odometry_message.pose.pose.orientation.z = orientation.z(); + odometry_message.pose.pose.orientation.w = orientation.w(); + } + odometry_message.twist.twist.linear.x = odometry_.getLinear(); + odometry_message.twist.twist.angular.z = odometry_.getAngular(); + realtime_odometry_publisher_->unlockAndPublish(); + } + + if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + { + auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + transform.header.stamp = time; + transform.transform.translation.x = odometry_.getX(); + transform.transform.translation.y = odometry_.getY(); + transform.transform.rotation.x = orientation.x(); + transform.transform.rotation.y = orientation.y(); + transform.transform.rotation.z = orientation.z(); + transform.transform.rotation.w = orientation.w(); + realtime_odometry_transform_publisher_->unlockAndPublish(); + } + } + + // Compute wheel velocity and angle + auto [alpha_write, Ws_write] = twist_to_ackermann(linear_command, angular_command); + + // Reduce wheel speed until the target angle has been reached + double alpha_delta = abs(alpha_write - alpha_read); + double scale; + if (alpha_delta < M_PI / 6) + { + scale = 1; + } + else if (alpha_delta > M_PI_2) + { + scale = 0.01; + } + else + { + // TODO: find the best function, e.g convex power functions + scale = cos(alpha_delta); + } + Ws_write *= scale; + + auto & last_command = previous_commands_.back(); + auto & second_to_last_command = previous_commands_.front(); + + limiter_traction_.limit( + Ws_write, last_command.speed, second_to_last_command.speed, period.seconds()); + + limiter_steering_.limit( + alpha_write, last_command.steering_angle, second_to_last_command.steering_angle, + period.seconds()); + + previous_commands_.pop(); + AckermannDrive ackermann_command; + ackermann_command.speed = + Ws_write; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel speed (rad/s) + ackermann_command.steering_angle = alpha_write; + previous_commands_.emplace(ackermann_command); + + // Publish ackermann command + if (publish_ackermann_command_ && realtime_ackermann_command_publisher_->trylock()) + { + auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; + realtime_ackermann_command.speed = + Ws_write; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel speed (rad/s) + realtime_ackermann_command.steering_angle = alpha_write; + realtime_ackermann_command_publisher_->unlockAndPublish(); + } + + traction_joint_[0].velocity_command.get().set_value(Ws_write); + steering_joint_[0].position_command.get().set_value(alpha_write); + return controller_interface::return_type::OK; +} + +CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) +{ + auto logger = get_node()->get_logger(); + + // update parameters + traction_joint_name_ = get_node()->get_parameter("traction_joint_name").as_string(); + steering_joint_name_ = get_node()->get_parameter("steering_joint_name").as_string(); + if (traction_joint_name_.empty()) + { + RCLCPP_ERROR(logger, "'traction_joint_name' parameter was empty"); + return CallbackReturn::ERROR; + } + if (steering_joint_name_.empty()) + { + RCLCPP_ERROR(logger, "'steering_joint_name' parameter was empty"); + return CallbackReturn::ERROR; + } + + wheel_params_.wheelbase = get_node()->get_parameter("wheelbase").as_double(); + wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); + + odometry_.setWheelParams(wheel_params_.wheelbase, wheel_params_.radius); + odometry_.setVelocityRollingWindowSize( + get_node()->get_parameter("velocity_rolling_window_size").as_int()); + + odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string(); + odom_params_.base_frame_id = get_node()->get_parameter("base_frame_id").as_string(); + + auto pose_diagonal = get_node()->get_parameter("pose_covariance_diagonal").as_double_array(); + std::copy( + pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); + + auto twist_diagonal = get_node()->get_parameter("twist_covariance_diagonal").as_double_array(); + std::copy( + twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); + + odom_params_.open_loop = get_node()->get_parameter("open_loop").as_bool(); + odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); + odom_params_.odom_only_twist = get_node()->get_parameter("odom_only_twist").as_bool(); + + cmd_vel_timeout_ = std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()}; + publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); + use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); + + publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); + publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); + + try + { + limiter_traction_ = TractionLimiter( + get_node()->get_parameter("traction.min_velocity").as_double(), + get_node()->get_parameter("traction.max_velocity").as_double(), + get_node()->get_parameter("traction.min_acceleration").as_double(), + get_node()->get_parameter("traction.max_acceleration").as_double(), + get_node()->get_parameter("traction.min_deceleration").as_double(), + get_node()->get_parameter("traction.max_deceleration").as_double(), + get_node()->get_parameter("traction.min_jerk").as_double(), + get_node()->get_parameter("traction.max_jerk").as_double()); + } + catch (const std::invalid_argument & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error configuring traction limiter: %s", e.what()); + return CallbackReturn::ERROR; + } + + try + { + limiter_steering_ = SteeringLimiter( + get_node()->get_parameter("steering.min_position").as_double(), + get_node()->get_parameter("steering.max_position").as_double(), + get_node()->get_parameter("steering.min_velocity").as_double(), + get_node()->get_parameter("steering.max_velocity").as_double(), + get_node()->get_parameter("steering.min_acceleration").as_double(), + get_node()->get_parameter("steering.max_acceleration").as_double()); + } + catch (const std::invalid_argument & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error configuring steering limiter: %s", e.what()); + return CallbackReturn::ERROR; + } + + if (!reset()) + { + return CallbackReturn::ERROR; + } + + const TwistStamped empty_twist; + received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); + + // Fill last two commands with default constructed commands + const AckermannDrive empty_ackermann_drive; + previous_commands_.emplace(empty_ackermann_drive); + previous_commands_.emplace(empty_ackermann_drive); + + // initialize ackermann command publisher + if (publish_ackermann_command_) + { + ackermann_command_publisher_ = get_node()->create_publisher( + DEFAULT_ACKERMANN_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_ackermann_command_publisher_ = + std::make_shared>( + ackermann_command_publisher_); + } + + // initialize command subscriber + if (use_stamped_vel_) + { + velocity_command_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) + { + RCLCPP_WARN( + get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) + { + RCLCPP_WARN_ONCE( + get_node()->get_logger(), + "Received TwistStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->get_clock()->now(); + } + received_velocity_msg_ptr_.set(std::move(msg)); + }); + } + else + { + velocity_command_unstamped_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) + { + RCLCPP_WARN( + get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + + // Write fake header in the stored stamped command + std::shared_ptr twist_stamped; + received_velocity_msg_ptr_.get(twist_stamped); + twist_stamped->twist = *msg; + twist_stamped->header.stamp = get_node()->get_clock()->now(); + }); + } + + // initialize odometry publisher and messasge + odometry_publisher_ = get_node()->create_publisher( + DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_odometry_publisher_ = + std::make_shared>( + odometry_publisher_); + + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.frame_id = odom_params_.odom_frame_id; + odometry_message.child_frame_id = odom_params_.base_frame_id; + + previous_publish_timestamp_ = get_node()->get_clock()->now(); + + // initialize odom values zeros + odometry_message.twist = + geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); + + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + odometry_message.pose.covariance[diagonal_index] = odom_params_.pose_covariance_diagonal[index]; + odometry_message.twist.covariance[diagonal_index] = + odom_params_.twist_covariance_diagonal[index]; + } + + // initialize transform publisher and message + if (odom_params_.enable_odom_tf) + { + odometry_transform_publisher_ = get_node()->create_publisher( + DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_odometry_transform_publisher_ = + std::make_shared>( + odometry_transform_publisher_); + + // keeping track of odom and base_link transforms only + auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; + odometry_transform_message.transforms.resize(1); + odometry_transform_message.transforms.front().header.frame_id = odom_params_.odom_frame_id; + odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id; + } + + // Create odom reset service + reset_odom_service_ = get_node()->create_service( + DEFAULT_RESET_ODOM_SERVICE, std::bind( + &TricycleController::reset_odometry, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::on_activate(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_node()->get_logger(), "On activate: Initialize Joints"); + + // Initialize the joints + const auto wheel_front_result = get_traction(traction_joint_name_, traction_joint_); + const auto steering_result = get_steering(steering_joint_name_, steering_joint_); + if (wheel_front_result == CallbackReturn::ERROR || steering_result == CallbackReturn::ERROR) + { + return CallbackReturn::ERROR; + } + if (traction_joint_.empty() || steering_joint_.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Either steering or traction interfaces are non existent"); + return CallbackReturn::ERROR; + } + + is_halted = false; + subscriber_is_active_ = true; + + RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::on_deactivate(const rclcpp_lifecycle::State &) +{ + subscriber_is_active_ = false; + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::on_cleanup(const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return CallbackReturn::ERROR; + } + + received_velocity_msg_ptr_.set(std::make_shared()); + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::on_error(const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +void TricycleController::reset_odometry( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*req*/, + std::shared_ptr /*res*/) +{ + odometry_.resetOdometry(); + RCLCPP_INFO(get_node()->get_logger(), "Odometry successfully reset"); +} + +bool TricycleController::reset() +{ + odometry_.resetOdometry(); + + // release the old queue + std::queue empty_ackermann_drive; + std::swap(previous_commands_, empty_ackermann_drive); + + traction_joint_.clear(); + steering_joint_.clear(); + + subscriber_is_active_ = false; + velocity_command_subscriber_.reset(); + velocity_command_unstamped_subscriber_.reset(); + + received_velocity_msg_ptr_.set(nullptr); + is_halted = false; + return true; +} + +CallbackReturn TricycleController::on_shutdown(const rclcpp_lifecycle::State &) +{ + return CallbackReturn::SUCCESS; +} + +void TricycleController::halt() +{ + traction_joint_[0].velocity_command.get().set_value(0.0); + steering_joint_[0].position_command.get().set_value(0.0); +} + +CallbackReturn TricycleController::get_traction( + const std::string & traction_joint_name, std::vector & joint) +{ + RCLCPP_INFO(get_node()->get_logger(), "Get Wheel Joint Instance"); + + // Lookup the velocity state interface + const auto state_handle = std::find_if( + state_interfaces_.cbegin(), state_interfaces_.cend(), + [&traction_joint_name](const auto & interface) + { + return interface.get_prefix_name() == traction_joint_name && + interface.get_interface_name() == HW_IF_VELOCITY; + }); + if (state_handle == state_interfaces_.cend()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint state handle for %s", + traction_joint_name.c_str()); + return CallbackReturn::ERROR; + } + + // Lookup the velocity command interface + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&traction_joint_name](const auto & interface) + { + return interface.get_prefix_name() == traction_joint_name && + interface.get_interface_name() == HW_IF_VELOCITY; + }); + if (command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint state handle for %s", + traction_joint_name.c_str()); + return CallbackReturn::ERROR; + } + + // Create the traction joint instance + joint.emplace_back(TractionHandle{std::ref(*state_handle), std::ref(*command_handle)}); + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::get_steering( + const std::string & steering_joint_name, std::vector & joint) +{ + RCLCPP_INFO(get_node()->get_logger(), "Get Steering Joint Instance"); + + // Lookup the velocity state interface + const auto state_handle = std::find_if( + state_interfaces_.cbegin(), state_interfaces_.cend(), + [&steering_joint_name](const auto & interface) + { + return interface.get_prefix_name() == steering_joint_name && + interface.get_interface_name() == HW_IF_POSITION; + }); + if (state_handle == state_interfaces_.cend()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint state handle for %s", + steering_joint_name.c_str()); + return CallbackReturn::ERROR; + } + + // Lookup the velocity command interface + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&steering_joint_name](const auto & interface) + { + return interface.get_prefix_name() == steering_joint_name && + interface.get_interface_name() == HW_IF_POSITION; + }); + if (command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint state handle for %s", + steering_joint_name.c_str()); + return CallbackReturn::ERROR; + } + + // Create the steering joint instance + joint.emplace_back(SteeringHandle{std::ref(*state_handle), std::ref(*command_handle)}); + return CallbackReturn::SUCCESS; +} + +double TricycleController::convert_trans_rot_vel_to_steering_angle( + double Vx, double theta_dot, double wheelbase) +{ + if (theta_dot == 0 || Vx == 0) + { + return 0; + } + return std::atan(theta_dot * wheelbase / Vx); +} + +std::tuple TricycleController::twist_to_ackermann(double Vx, double theta_dot) +{ + // using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf + double alpha, Ws; + + if (Vx == 0 && theta_dot != 0) + { // is spin action + alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; + Ws = abs(theta_dot) * wheel_params_.wheelbase / wheel_params_.radius; + return std::make_tuple(alpha, Ws); + } + + alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, wheel_params_.wheelbase); + Ws = Vx / (wheel_params_.radius * std::cos(alpha)); + return std::make_tuple(alpha, Ws); +} + +} // namespace tricycle_controller + +#include +PLUGINLIB_EXPORT_CLASS( + tricycle_controller::TricycleController, controller_interface::ControllerInterface) diff --git a/tricycle_controller/test/config/test_tricycle_controller.yaml b/tricycle_controller/test/config/test_tricycle_controller.yaml new file mode 100644 index 0000000000..d81d722219 --- /dev/null +++ b/tricycle_controller/test/config/test_tricycle_controller.yaml @@ -0,0 +1,20 @@ + +test_tricycle_controller: + ros__parameters: + traction_joint_name: traction_joint + steering_joint_name: steering_joint + wheel_radius: 0.125 + wheelbase: 1.252 + use_stamped_vel: false + enable_odom_tf: false + use_sim_time: false + pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + odom_only_twist: true + publish_ackermann_command: true + traction: + max_acceleration: 5.0 + max_deceleration: 8.0 + steering: + max_position: 1.57 # pi/2 + max_velocity: 1.0 \ No newline at end of file diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp new file mode 100644 index 0000000000..088cddbabe --- /dev/null +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -0,0 +1,43 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadTricycleController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_tricycle_controller", "tricycle_controller/TricycleController")); + + rclcpp::shutdown(); +} diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp new file mode 100644 index 0000000000..a1f09ddaf8 --- /dev/null +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -0,0 +1,350 @@ +// Copyright 2022 Pixel Robotics. +// +// 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. + +/* + * Author: Tony Najjar + */ + +#include + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tricycle_controller/tricycle_controller.hpp" + +using CallbackReturn = controller_interface::CallbackReturn; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using lifecycle_msgs::msg::State; +using testing::SizeIs; + +class TestableTricycleController : public tricycle_controller::TricycleController +{ +public: + using TricycleController::TricycleController; + std::shared_ptr getLastReceivedTwist() + { + std::shared_ptr ret; + received_velocity_msg_ptr_.get(ret); + return ret; + } + + /** + * @brief wait_for_twist block until a new twist is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + * + * @return true if new twist msg was received, false if timeout + */ + bool wait_for_twist( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) + { + rclcpp::WaitSet wait_set; + wait_set.add_subscription(velocity_command_subscriber_); + + if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) + { + executor.spin_some(); + return true; + } + return false; + } +}; + +class TestTricycleController : public ::testing::Test +{ +protected: + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + + void SetUp() override + { + controller_ = std::make_unique(); + pub_node = std::make_shared("velocity_publisher"); + velocity_publisher = pub_node->create_publisher( + controller_name + "/cmd_vel", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + /// Publish velocity msgs + /** + * linear - magnitude of the linear command in the geometry_msgs::twist message + * angular - the magnitude of the angular command in geometry_msgs::twist message + */ + void publish(double linear, double angular) + { + int wait_count = 0; + auto topic = velocity_publisher->get_topic_name(); + while (pub_node->count_subscribers(topic) == 0) + { + if (wait_count >= 5) + { + auto error_msg = std::string("publishing to ") + topic + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + + geometry_msgs::msg::TwistStamped velocity_message; + velocity_message.header.stamp = pub_node->get_clock()->now(); + velocity_message.twist.linear.x = linear; + velocity_message.twist.angular.z = angular; + velocity_publisher->publish(velocity_message); + } + + /// \brief wait for the subscriber and publisher to completely setup + void waitForSetup() + { + constexpr std::chrono::seconds TIMEOUT{2}; + auto clock = pub_node->get_clock(); + auto start = clock->now(); + while (velocity_publisher->get_subscription_count() <= 0) + { + if ((clock->now() - start) > TIMEOUT) + { + FAIL(); + } + rclcpp::spin_some(pub_node); + } + } + + void assignResources() + { + std::vector state_ifs; + state_ifs.emplace_back(steering_joint_pos_state_); + state_ifs.emplace_back(traction_joint_vel_state_); + + std::vector command_ifs; + command_ifs.emplace_back(steering_joint_pos_cmd_); + command_ifs.emplace_back(traction_joint_vel_cmd_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + const std::string controller_name = "test_tricycle_controller"; + std::unique_ptr controller_; + + const std::string traction_joint_name = "traction_joint"; + const std::string steering_joint_name = "steering_joint"; + double position_ = 0.1; + double velocity_ = 0.2; + + hardware_interface::StateInterface steering_joint_pos_state_{ + steering_joint_name, HW_IF_POSITION, &position_}; + + hardware_interface::StateInterface traction_joint_vel_state_{ + traction_joint_name, HW_IF_VELOCITY, &velocity_}; + + hardware_interface::CommandInterface steering_joint_pos_cmd_{ + steering_joint_name, HW_IF_POSITION, &position_}; + + hardware_interface::CommandInterface traction_joint_vel_cmd_{ + traction_joint_name, HW_IF_VELOCITY, &velocity_}; + + rclcpp::Node::SharedPtr pub_node; + rclcpp::Publisher::SharedPtr velocity_publisher; +}; + +TEST_F(TestTricycleController, configure_fails_without_parameters) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestTricycleController, configure_fails_if_only_traction_or_steering_side_defined) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(std::string()))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(std::string()))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F(TestTricycleController, activate_fails_without_resources_assigned) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + // We implicitly test that by default position feedback is required + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + assignResources(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F(TestTricycleController, cleanup) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 1.2)); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.12)); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResources(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // send msg + const double linear = 1.0; + const double angular = 1.0; + publish(linear, angular); + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + state = controller_->get_node()->deactivate(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + + // should be stopped + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + + executor.cancel(); +} + +TEST_F(TestTricycleController, correct_initialization_using_parameters) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 0.4)); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + auto state = controller_->get_node()->configure(); + assignResources(); + + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(position_, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_value()); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + // send msg + const double linear = 1.0; + const double angular = 0.0; + publish(linear, angular); + // wait for msg is be published to the system + ASSERT_TRUE(controller_->wait_for_twist(executor)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_value()); + + // deactivated + // wait so controller process the second point when deactivated + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} diff --git a/tricycle_controller/tricycle_controller.xml b/tricycle_controller/tricycle_controller.xml new file mode 100644 index 0000000000..568b13532b --- /dev/null +++ b/tricycle_controller/tricycle_controller.xml @@ -0,0 +1,7 @@ + + + + The tricycle controller transforms linear and angular velocity messages into signals for steering and traction joints for a tricycle drive robot. + + + From 74a3152604552960cdba3f58b9693db0394f981d Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 4 Aug 2022 10:26:09 +0100 Subject: [PATCH 011/126] Update version and maintainer --- tricycle_controller/package.xml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 9dec765d60..3864d636e2 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,8 +2,9 @@ tricycle_controller - 1.0.0 + 2.10.0 Controller for a tricycle drive mobile base + Bence Magyar Tony Najjar Apache License 2.0 Tony Najjar @@ -32,4 +33,4 @@ ament_cmake - \ No newline at end of file + From 647bd3e0c92a40aa945c943c937387eed33ac7ba Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 4 Aug 2022 10:28:17 +0100 Subject: [PATCH 012/126] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 8 ++++++++ joint_trajectory_controller/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 8 ++++++++ velocity_controllers/CHANGELOG.rst | 3 +++ 14 files changed, 54 insertions(+) create mode 100644 tricycle_controller/CHANGELOG.rst diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index f582845fe7..294b73484a 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- * Formatting changes from pre-commit (`#400 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 30863470a1..284d3cab3e 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 790834ea9e..9c1247c5b7 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 16fac9b49f..df23e7c7cf 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 98df8e9bd7..850c0a5d3d 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- * Formatting changes from pre-commit (`#400 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index acd580d511..062782fbbb 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 58295ee22b..1f818f4dd4 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use explicit type in joint_state_broadcaster test (`#403 `_) + This use of `auto` is causing a static assert on RHEL. Explicitly + specifying the type seems to resolve the failure and allow the test to + be compiled. +* Contributors: Scott K Logan + 2.10.0 (2022-08-01) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index f96bd2c9b4..2b893a5702 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- * Make JTC callbacks methods with clear names (`#397 `_) #abi-breaking diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 66cb16f92a..c1fc2069f5 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index adf3ff3538..5911417c1d 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Tricycle controller (`#345 `_) +* Contributors: Tony Najjar + 2.10.0 (2022-08-01) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index c7b22b0183..f5099c3648 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 3607695579..0afb4ad6f0 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- * Formatting changes from pre-commit (`#400 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst new file mode 100644 index 0000000000..8eccb72ea5 --- /dev/null +++ b/tricycle_controller/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tricycle_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Tricycle controller (`#345 `_) +* Contributors: Bence Magyar, Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 6bc28fc062..74e9d1d78b 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.10.0 (2022-08-01) ------------------- From ca21178966c612b18f0d23aade2e0cf2c7a80790 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 4 Aug 2022 10:36:55 +0100 Subject: [PATCH 013/126] 2.11.0 --- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 30 files changed, 44 insertions(+), 44 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 294b73484a..5c40544d25 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index ae2696d998..d82a4a2df8 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.10.0 + 2.11.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 284d3cab3e..3d2babf2ed 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 0e415ba867..e716631d9e 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.10.0 + 2.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 9c1247c5b7..a3d5156859 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index c857ab92ab..868d83fdea 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.10.0 + 2.11.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index df23e7c7cf..9b1d48c875 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 3c81a32680..071701c5cd 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.10.0 + 2.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 850c0a5d3d..c06969c439 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 42ba2dc7f6..a68306ee4e 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.10.0 + 2.11.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 062782fbbb..82c99bf60f 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 52ba10b381..dec5c6b5e4 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.10.0 + 2.11.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 1f818f4dd4..3030632664 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- * Use explicit type in joint_state_broadcaster test (`#403 `_) This use of `auto` is causing a static assert on RHEL. Explicitly specifying the type seems to resolve the failure and allow the test to diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 0048dc4c2b..12db7f0b2d 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.10.0 + 2.11.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 2b893a5702..d435127b2f 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 02b27cdf83..66bb5b0999 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.10.0 + 2.11.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index c1fc2069f5..1affcdc4b8 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 8a2fee3598..3b7eb27975 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.10.0 + 2.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 5911417c1d..e53cadd749 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- * Tricycle controller (`#345 `_) * Contributors: Tony Najjar diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index aebcf25ba6..2298913120 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.10.0 + 2.11.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index f5099c3648..7d319f9034 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 924e7b0bca..a57a484e88 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.10.0 + 2.11.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 4c8fba2fab..959ad0eb4d 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.10.0", + version="2.11.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 0afb4ad6f0..27bc6030cb 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 62e703bc3b..70b8201345 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.10.0 + 2.11.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index ab2db18405..5f24382cb4 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.10.0", + version="2.11.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 8eccb72ea5..7c929f4432 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- * Tricycle controller (`#345 `_) * Contributors: Bence Magyar, Tony Najjar diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 3864d636e2..409ee6bc40 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.10.0 + 2.11.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 74e9d1d78b..b9de02a603 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.11.0 (2022-08-04) +------------------- 2.10.0 (2022-08-01) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index fac41cadbf..b1d500f1a1 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.10.0 + 2.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 48ade9c8f5a6a681d59305b6ddb624d753de7cfe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 6 Aug 2022 08:54:37 +0200 Subject: [PATCH 014/126] Small fixes for JTC. (#390) variables in JTC to not clutter other PR with them. fixes of updating parameters on renewed configuration of JTC that were missed --- .../src/joint_trajectory_controller.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index e04f66754c..6d81c1d2b1 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -68,7 +68,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() state_publish_rate_ = auto_declare("state_publish_rate", 50.0); action_monitor_rate_ = auto_declare("action_monitor_rate", 20.0); - std::string interpolation_string = auto_declare( + const std::string interpolation_string = auto_declare( "interpolation_method", interpolation_methods::InterpolationMethodMap.at( interpolation_methods::DEFAULT_INTERPOLATION)); interpolation_method_ = interpolation_methods::from_string(interpolation_string); @@ -697,12 +697,20 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( allow_integration_in_goal_trajectories_ = get_node()->get_parameter("allow_integration_in_goal_trajectories").get_value(); + const std::string interpolation_string = + get_node()->get_parameter("interpolation_method").as_string(); + interpolation_method_ = interpolation_methods::from_string(interpolation_string); + RCLCPP_INFO( + logger, "Using '%s' interpolation method.", + interpolation_methods::InterpolationMethodMap.at(interpolation_method_).c_str()); + joint_command_subscriber_ = get_node()->create_subscription( "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1)); // State publisher + state_publish_rate_ = get_node()->get_parameter("state_publish_rate").get_value(); RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate_); if (state_publish_rate_ > 0.0) { @@ -746,6 +754,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( RCLCPP_INFO(logger, "Goals with partial set of joints are allowed"); } + action_monitor_rate_ = get_node()->get_parameter("action_monitor_rate").get_value(); RCLCPP_INFO(logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate_); action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / action_monitor_rate_); From 7d8a26925566a651284087b124b8d56b6854b0a9 Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer <31753650+mechwiz@users.noreply.github.com> Date: Sun, 7 Aug 2022 16:29:25 -0400 Subject: [PATCH 015/126] [JTC] Hold position if tolerance is violated even during non-active goal (#368) * hold position if tolerance is violated even during non-active goal * rename abort Co-authored-by: Michael Wiznitzer --- .../src/joint_trajectory_controller.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 6d81c1d2b1..21501dfb67 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -353,6 +353,11 @@ controller_interface::return_type JointTrajectoryController::update( // to be satisfied or violated within the goal_time_tolerance } } + else if (tolerance_violated_while_moving) + { + set_hold_position(); + RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); + } } } From 4d3404f8bf394b2f11dd1b4fd9be4070b4bf5767 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 10 Aug 2022 18:04:03 +0200 Subject: [PATCH 016/126] Adjust CI configuration to ros2_control repository. (#408) * Remove old CI setup. * Adding adjusted CI configurationt to ros2_control repository. * Fixup line for private repositories. * Run format only on PR. * Adjust repos files. --- .github/workflows/README.md | 24 ++++++++++ .github/workflows/ci-coverage-build.yml | 48 +++++++++++++++++++ .github/workflows/ci-format.yml | 6 +-- .github/workflows/ci-ros-lint.yml | 44 ++++++----------- .github/workflows/foxy-abi-compatibility.yml | 26 +++++----- ...y-build.yml => foxy-binary-build-main.yml} | 11 +++-- .../workflows/foxy-binary-build-testing.yml | 26 ++++++++++ .github/workflows/foxy-rhel-binary-build.yml | 33 +++++++++++++ ...ld.yml => foxy-semi-binary-build-main.yml} | 10 ++-- .../foxy-semi-binary-build-testing.yml | 25 ++++++++++ .../workflows/galactic-abi-compatibility.yml | 26 +++++----- ...ild.yml => galactic-binary-build-main.yml} | 11 +++-- .../galactic-binary-build-testing.yml | 26 ++++++++++ .../workflows/galactic-rhel-binary-build.yml | 33 +++++++++++++ ...ml => galactic-semi-binary-build-main.yml} | 10 ++-- .../galactic-semi-binary-build-testing.yml | 25 ++++++++++ .../workflows/humble-abi-compatibility.yml | 20 ++++++++ .../workflows/humble-binary-build-main.yml | 26 ++++++++++ .../workflows/humble-binary-build-testing.yml | 26 ++++++++++ .../workflows/humble-rhel-binary-build.yml | 33 +++++++++++++ .../humble-semi-binary-build-main.yml | 25 ++++++++++ .../humble-semi-binary-build-testing.yml | 25 ++++++++++ ...urce-build.yml => humble-source-build.yml} | 12 +++-- .../reusable-industrial-ci-with-cache.yml | 30 ++++-------- .../reusable-ros-tooling-source-build.yml | 28 +++++------ .../workflows/rolling-abi-compatibility.yml | 5 +- ...uild.yml => rolling-binary-build-main.yml} | 11 +++-- .../rolling-binary-build-testing.yml | 26 ++++++++++ .../workflows/rolling-rhel-binary-build.yml | 36 +++++++------- ...yml => rolling-semi-binary-build-main.yml} | 10 ++-- .../rolling-semi-binary-build-testing.yml | 25 ++++++++++ .github/workflows/rolling-source-build.yml | 4 ++ .pre-commit-config.yaml | 41 ++++++++++------ README.md | 11 ++--- ros2_controllers-not-released.foxy.repos | 5 ++ ros2_controllers-not-released.galactic.repos | 5 ++ ros2_controllers-not-released.humble.repos | 6 +++ ros2_controllers-not-released.rolling.repos | 5 ++ ros2_controllers.humble.repos | 13 +++++ ros2_controllers.rolling.repos | 10 ++-- 40 files changed, 658 insertions(+), 164 deletions(-) create mode 100644 .github/workflows/README.md create mode 100644 .github/workflows/ci-coverage-build.yml rename .github/workflows/{foxy-binary-build.yml => foxy-binary-build-main.yml} (60%) create mode 100644 .github/workflows/foxy-binary-build-testing.yml create mode 100644 .github/workflows/foxy-rhel-binary-build.yml rename .github/workflows/{foxy-semi-binary-build.yml => foxy-semi-binary-build-main.yml} (64%) create mode 100644 .github/workflows/foxy-semi-binary-build-testing.yml rename .github/workflows/{galactic-binary-build.yml => galactic-binary-build-main.yml} (60%) create mode 100644 .github/workflows/galactic-binary-build-testing.yml create mode 100644 .github/workflows/galactic-rhel-binary-build.yml rename .github/workflows/{galactic-semi-binary-build.yml => galactic-semi-binary-build-main.yml} (64%) create mode 100644 .github/workflows/galactic-semi-binary-build-testing.yml create mode 100644 .github/workflows/humble-abi-compatibility.yml create mode 100644 .github/workflows/humble-binary-build-main.yml create mode 100644 .github/workflows/humble-binary-build-testing.yml create mode 100644 .github/workflows/humble-rhel-binary-build.yml create mode 100644 .github/workflows/humble-semi-binary-build-main.yml create mode 100644 .github/workflows/humble-semi-binary-build-testing.yml rename .github/workflows/{galactic-source-build.yml => humble-source-build.yml} (56%) rename .github/workflows/{rolling-binary-build.yml => rolling-binary-build-main.yml} (60%) create mode 100644 .github/workflows/rolling-binary-build-testing.yml rename .github/workflows/{rolling-semi-binary-build.yml => rolling-semi-binary-build-main.yml} (64%) create mode 100644 .github/workflows/rolling-semi-binary-build-testing.yml create mode 100644 ros2_controllers-not-released.humble.repos create mode 100644 ros2_controllers.humble.repos diff --git a/.github/workflows/README.md b/.github/workflows/README.md new file mode 100644 index 0000000000..34bfb4cc03 --- /dev/null +++ b/.github/workflows/README.md @@ -0,0 +1,24 @@ +## Build status + +ROS2 Distro | Branch | Build status | Documentation | Released packages +:---------: | :----: | :----------: | :-----------: | :---------------: +**Rolling** | [`rolling`](https://github.com/ros-controls/ros2_controllers/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-testing.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-testing.yml?branch=master)
[![Rolling Source Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-source-build.yml?branch=master) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) +**Humble** | [`humble`](https://github.com/ros-controls/ros2_controllers/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-testing.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-testing.yml?branch=master)
[![Humble Source Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-source-build.yml?branch=master) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#humble) +**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_controllers/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-main.yml?branch=galactic)
[![Galactic Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-testing.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-testing.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-main.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-testing.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-testing.yml?branch=galactic) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#galactic) +**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_controllers/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-testing.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-testing.yml?branch=foxy) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#foxy) + + +### Explanation of different build types + +**NOTE**: There are three build stages checking current and future compatibility of the package. + +1. Binary builds - against released packages (main and testing) in ROS distributions. Shows that direct local build is possible. + + Uses repos file: `$NAME$-not-released..repos` + +1. Semi-binary builds - against released core ROS packages (main and testing), but the immediate dependencies are pulled from source. + Shows that local build with dependencies is possible and if fails there we can expect that after the next package sync we will not be able to build. + + Uses repos file: `$NAME$.repos` + +1. Source build - also core ROS packages are build from source. It shows potential issues in the mid future. diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml new file mode 100644 index 0000000000..d9b4ec4494 --- /dev/null +++ b/.github/workflows/ci-coverage-build.yml @@ -0,0 +1,48 @@ +name: Coverage Build +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + +jobs: + coverage: + name: coverage build + runs-on: ubuntu-22.04 + strategy: + fail-fast: false + env: + ROS_DISTRO: rolling + steps: + - uses: ros-tooling/setup-ros@0.3.4 + with: + required-ros-distributions: ${{ env.ROS_DISTRO }} + - uses: actions/checkout@v3 + - uses: ros-tooling/action-ros-ci@0.2.6 + with: + target-ros2-distro: ${{ env.ROS_DISTRO }} + import-token: ${{ secrets.GITHUB_TOKEN }} + # build all packages listed in the meta package + package-name: + diff_drive_controller + + vcs-repo-file-url: | + https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers-not-released.${{ env.ROS_DISTRO }}.repos?token=${{ secrets.GITHUB_TOKEN }} + colcon-defaults: | + { + "build": { + "mixin": ["coverage-gcc"] + } + } + colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + - uses: codecov/codecov-action@v3.1.0 + with: + file: ros_ws/lcov/total_coverage.info + flags: unittests + name: codecov-umbrella + - uses: actions/upload-artifact@v3.1.0 + with: + name: colcon-logs-coverage-rolling + path: ros_ws/log diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index dffa28d1c0..fba9f85911 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -4,11 +4,7 @@ name: Format on: - workflow_dispatch: pull_request: - push: - branches: - - master jobs: pre-commit: @@ -20,7 +16,7 @@ jobs: with: python-version: 3.9.7 - name: Install system hooks - run: sudo apt install -qq clang-format-11 cppcheck + run: sudo apt install -qq clang-format-12 cppcheck - uses: pre-commit/action@v2.0.3 with: extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 8eec08251f..9902dbdaaf 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -9,49 +9,33 @@ jobs: strategy: fail-fast: false matrix: - linter: [copyright, cppcheck, lint_cmake] + linter: [cppcheck, copyright, lint_cmake] steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v3 - uses: ros-tooling/setup-ros@v0.2 - uses: ros-tooling/action-ros-lint@v0.1 with: - distribution: galactic + distribution: rolling linter: ${{ matrix.linter }} package-name: - diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - joint_state_broadcaster - joint_trajectory_controller - gripper_controllers - position_controllers - ros2_controllers - ros2_controllers_test_nodes - velocity_controllers + diff_drive_controller - ament_lint_cpplint: - name: ament_lint_cpplint + + ament_lint_100: + name: ament_${{ matrix.linter }} runs-on: ubuntu-20.04 strategy: fail-fast: false + matrix: + linter: [cpplint] steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v3 - uses: ros-tooling/setup-ros@v0.2 - uses: ros-tooling/action-ros-lint@v0.1 with: - distribution: galactic + distribution: rolling linter: cpplint - arguments: "--filter=-whitespace/newline" + arguments: "--linelength=100 --filter=-whitespace/newline" package-name: - diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - joint_state_broadcaster - joint_trajectory_controller - gripper_controllers - position_controllers - ros2_controllers - ros2_controllers_test_nodes - velocity_controllers + diff_drive_controller + ros2_controllers diff --git a/.github/workflows/foxy-abi-compatibility.yml b/.github/workflows/foxy-abi-compatibility.yml index 7f9f1684ef..7ce17effd0 100644 --- a/.github/workflows/foxy-abi-compatibility.yml +++ b/.github/workflows/foxy-abi-compatibility.yml @@ -1,18 +1,20 @@ -name: ABI Compatibility Check +name: Foxy - ABI Compatibility Check on: + workflow_dispatch: + branches: + - foxy pull_request: branches: - foxy jobs: -# TODO(andyz): disabled because it started failing at the transition to Humble -# abi_check: -# runs-on: ubuntu-latest -# steps: -# - uses: actions/checkout@v3 -# - uses: ros-industrial/industrial_ci@master -# env: -# ROS_DISTRO: foxy -# ROS_REPO: main -# ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} -# NOT_TEST_BUILD: true + abi_check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: foxy + ROS_REPO: main + ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} + NOT_TEST_BUILD: true diff --git a/.github/workflows/foxy-binary-build.yml b/.github/workflows/foxy-binary-build-main.yml similarity index 60% rename from .github/workflows/foxy-binary-build.yml rename to .github/workflows/foxy-binary-build-main.yml index bd3050cd37..b306d7e44d 100644 --- a/.github/workflows/foxy-binary-build.yml +++ b/.github/workflows/foxy-binary-build-main.yml @@ -1,7 +1,11 @@ -name: Foxy Binary Build -# Build & test all dependencies from released (binary) packages. +name: Foxy Binary Build - main +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' on: + workflow_dispatch: + branches: + - foxy pull_request: branches: - foxy @@ -10,12 +14,13 @@ on: - foxy schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '08 18 * * *' + - cron: '03 1 * * *' jobs: binary: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: foxy + ros_repo: main upstream_workspace: ros2_controllers-not-released.foxy.repos ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-binary-build-testing.yml b/.github/workflows/foxy-binary-build-testing.yml new file mode 100644 index 0000000000..260751abea --- /dev/null +++ b/.github/workflows/foxy-binary-build-testing.yml @@ -0,0 +1,26 @@ +name: Foxy Binary Build - testing +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - foxy + pull_request: + branches: + - foxy + push: + branches: + - foxy + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: foxy + ros_repo: testing + upstream_workspace: ros2_controllers-not-released.foxy.repos + ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-rhel-binary-build.yml b/.github/workflows/foxy-rhel-binary-build.yml new file mode 100644 index 0000000000..8ce90c940f --- /dev/null +++ b/.github/workflows/foxy-rhel-binary-build.yml @@ -0,0 +1,33 @@ +name: Foxy RHEL Binary Build +on: + workflow_dispatch: + branches: + - foxy + pull_request: + branches: + - foxy + push: + branches: + - foxy + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + foxy_rhel_binary: + name: Foxy RHEL binary build + runs-on: ubuntu-latest + env: + ROS_DISTRO: foxy + container: jaronl/ros:foxy-alma + steps: + - uses: actions/checkout@v3 + with: + path: src/ros2_controllers + - run: | + rosdep update + rosdep install -iy --from-path src/ros2_controllers + source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash + colcon build + colcon test diff --git a/.github/workflows/foxy-semi-binary-build.yml b/.github/workflows/foxy-semi-binary-build-main.yml similarity index 64% rename from .github/workflows/foxy-semi-binary-build.yml rename to .github/workflows/foxy-semi-binary-build-main.yml index a6fc83fbd5..75c3295124 100644 --- a/.github/workflows/foxy-semi-binary-build.yml +++ b/.github/workflows/foxy-semi-binary-build-main.yml @@ -1,7 +1,10 @@ -name: Foxy Semi-Binary Build -# Build & test that compiles the main dependencies from source. +name: Foxy Semi-Binary Build - main +# description: 'Build & test that compiles the main dependencies from source.' on: + workflow_dispatch: + branches: + - foxy pull_request: branches: - foxy @@ -10,12 +13,13 @@ on: - foxy schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '13 18 * * *' + - cron: '33 1 * * *' jobs: semi_binary: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: foxy + ros_repo: main upstream_workspace: ros2_controllers.foxy.repos ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-semi-binary-build-testing.yml b/.github/workflows/foxy-semi-binary-build-testing.yml new file mode 100644 index 0000000000..f6d663cc2c --- /dev/null +++ b/.github/workflows/foxy-semi-binary-build-testing.yml @@ -0,0 +1,25 @@ +name: Foxy Semi-Binary Build - testing +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - foxy + pull_request: + branches: + - foxy + push: + branches: + - foxy + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: foxy + ros_repo: testing + upstream_workspace: ros2_controllers.foxy.repos + ref_for_scheduled_build: foxy diff --git a/.github/workflows/galactic-abi-compatibility.yml b/.github/workflows/galactic-abi-compatibility.yml index 8689791128..06a48ef9c7 100644 --- a/.github/workflows/galactic-abi-compatibility.yml +++ b/.github/workflows/galactic-abi-compatibility.yml @@ -1,18 +1,20 @@ -name: ABI Compatibility Check +name: Galactic - ABI Compatibility Check on: + workflow_dispatch: + branches: + - galactic pull_request: branches: - galactic jobs: -# TODO(andyz): disabled because it started failing at the transition to Humble -# abi_check: -# runs-on: ubuntu-latest -# steps: -# - uses: actions/checkout@v3 -# - uses: ros-industrial/industrial_ci@master -# env: -# ROS_DISTRO: galactic -# ROS_REPO: main -# ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} -# NOT_TEST_BUILD: true + abi_check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: galactic + ROS_REPO: main + ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} + NOT_TEST_BUILD: true diff --git a/.github/workflows/galactic-binary-build.yml b/.github/workflows/galactic-binary-build-main.yml similarity index 60% rename from .github/workflows/galactic-binary-build.yml rename to .github/workflows/galactic-binary-build-main.yml index eeac89e8d1..14fe56407c 100644 --- a/.github/workflows/galactic-binary-build.yml +++ b/.github/workflows/galactic-binary-build-main.yml @@ -1,7 +1,11 @@ -name: Galactic Binary Build -# Build & test all dependencies from released (binary) packages.' +name: Galactic Binary Build - main +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' on: + workflow_dispatch: + branches: + - galactic pull_request: branches: - galactic @@ -10,12 +14,13 @@ on: - galactic schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '08 18 * * *' + - cron: '03 1 * * *' jobs: binary: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: galactic + ros_repo: main upstream_workspace: ros2_controllers-not-released.galactic.repos ref_for_scheduled_build: galactic diff --git a/.github/workflows/galactic-binary-build-testing.yml b/.github/workflows/galactic-binary-build-testing.yml new file mode 100644 index 0000000000..c4b22a3a7a --- /dev/null +++ b/.github/workflows/galactic-binary-build-testing.yml @@ -0,0 +1,26 @@ +name: Galactic Binary Build - testing +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - galactic + pull_request: + branches: + - galactic + push: + branches: + - galactic + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: galactic + ros_repo: testing + upstream_workspace: ros2_controllers-not-released.galactic.repos + ref_for_scheduled_build: galactic diff --git a/.github/workflows/galactic-rhel-binary-build.yml b/.github/workflows/galactic-rhel-binary-build.yml new file mode 100644 index 0000000000..2116d5f4d5 --- /dev/null +++ b/.github/workflows/galactic-rhel-binary-build.yml @@ -0,0 +1,33 @@ +name: Galactic RHEL Binary Build +on: + workflow_dispatch: + branches: + - galactic + pull_request: + branches: + - galactic + push: + branches: + - galactic + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + galactic_rhel_binary: + name: Galactic RHEL binary build + runs-on: ubuntu-latest + env: + ROS_DISTRO: galactic + container: jaronl/ros:galactic-alma + steps: + - uses: actions/checkout@v3 + with: + path: src/ros2_controllers + - run: | + rosdep update + rosdep install -iy --from-path src/ros2_controllers + source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash + colcon build + colcon test diff --git a/.github/workflows/galactic-semi-binary-build.yml b/.github/workflows/galactic-semi-binary-build-main.yml similarity index 64% rename from .github/workflows/galactic-semi-binary-build.yml rename to .github/workflows/galactic-semi-binary-build-main.yml index f7f2cd0a53..7e498679cb 100644 --- a/.github/workflows/galactic-semi-binary-build.yml +++ b/.github/workflows/galactic-semi-binary-build-main.yml @@ -1,7 +1,10 @@ -name: Galactic Semi-Binary Build -# Build & test that compiles the main dependencies from source. +name: Galactic Semi-Binary Build - main +# description: 'Build & test that compiles the main dependencies from source.' on: + workflow_dispatch: + branches: + - galactic pull_request: branches: - galactic @@ -10,12 +13,13 @@ on: - galactic schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '13 18 * * *' + - cron: '33 1 * * *' jobs: semi_binary: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: galactic + ros_repo: main upstream_workspace: ros2_controllers.galactic.repos ref_for_scheduled_build: galactic diff --git a/.github/workflows/galactic-semi-binary-build-testing.yml b/.github/workflows/galactic-semi-binary-build-testing.yml new file mode 100644 index 0000000000..82a74a358b --- /dev/null +++ b/.github/workflows/galactic-semi-binary-build-testing.yml @@ -0,0 +1,25 @@ +name: Galactic Semi-Binary Build - testing +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - galactic + pull_request: + branches: + - galactic + push: + branches: + - galactic + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: galactic + ros_repo: testing + upstream_workspace: ros2_controllers.galactic.repos + ref_for_scheduled_build: galactic diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml new file mode 100644 index 0000000000..1be00cfc76 --- /dev/null +++ b/.github/workflows/humble-abi-compatibility.yml @@ -0,0 +1,20 @@ +name: Humble - ABI Compatibility Check +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + +jobs: + abi_check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: humble + ROS_REPO: main + ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} + NOT_TEST_BUILD: true diff --git a/.github/workflows/humble-binary-build-main.yml b/.github/workflows/humble-binary-build-main.yml new file mode 100644 index 0000000000..01708cf864 --- /dev/null +++ b/.github/workflows/humble-binary-build-main.yml @@ -0,0 +1,26 @@ +name: Humble Binary Build - main +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: humble + ros_repo: main + upstream_workspace: ros2_controllers-not-released.humble.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/humble-binary-build-testing.yml b/.github/workflows/humble-binary-build-testing.yml new file mode 100644 index 0000000000..73ed0a4859 --- /dev/null +++ b/.github/workflows/humble-binary-build-testing.yml @@ -0,0 +1,26 @@ +name: Humble Binary Build - testing +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: humble + ros_repo: testing + upstream_workspace: ros2_controllers-not-released.humble.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml new file mode 100644 index 0000000000..42a700a7db --- /dev/null +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -0,0 +1,33 @@ +name: Humble RHEL Binary Build +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + humble_rhel_binary: + name: Humble RHEL binary build + runs-on: ubuntu-latest + env: + ROS_DISTRO: humble + container: jaronl/ros:humble-alma + steps: + - uses: actions/checkout@v3 + with: + path: src/ros2_controllers + - run: | + rosdep update + rosdep install -iy --from-path src/ros2_controllers + source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash + colcon build + colcon test diff --git a/.github/workflows/humble-semi-binary-build-main.yml b/.github/workflows/humble-semi-binary-build-main.yml new file mode 100644 index 0000000000..10b1186b79 --- /dev/null +++ b/.github/workflows/humble-semi-binary-build-main.yml @@ -0,0 +1,25 @@ +name: Humble Semi-Binary Build - main +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: humble + ros_repo: main + upstream_workspace: ros2_controllers.humble.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/humble-semi-binary-build-testing.yml b/.github/workflows/humble-semi-binary-build-testing.yml new file mode 100644 index 0000000000..ec73cc6b98 --- /dev/null +++ b/.github/workflows/humble-semi-binary-build-testing.yml @@ -0,0 +1,25 @@ +name: Humble Semi-Binary Build - testing +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: humble + ros_repo: testing + upstream_workspace: ros2_controllers.humble.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/galactic-source-build.yml b/.github/workflows/humble-source-build.yml similarity index 56% rename from .github/workflows/galactic-source-build.yml rename to .github/workflows/humble-source-build.yml index d1c3aa97e8..6ab92814dc 100644 --- a/.github/workflows/galactic-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -1,8 +1,11 @@ -name: Galactic Source Build +name: Humble Source Build on: + workflow_dispatch: + branches: + - master push: branches: - - galactic + - master schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' @@ -11,5 +14,6 @@ jobs: source: uses: ./.github/workflows/reusable-ros-tooling-source-build.yml with: - ros_distro: galactic - ref: galactic + ros_distro: humble + ref: master + ros2_repo_branch: master-humble diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml index 57ea222979..0ff359d091 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -20,8 +20,8 @@ on: required: true type: string ros_repo: - description: 'ROS_REPO to run for industrial_ci. Possible values: "all", "main", "testing"' - default: 'all' + description: 'ROS_REPO to run for industrial_ci. Possible values: "main", "testing"' + default: 'main' required: false type: string os_code_name: @@ -49,27 +49,23 @@ on: jobs: reusable_industrial_ci_with_cache: - name: ${{ inputs.os_code_name }} ${{ matrix.ROS_REPO }} ${{ inputs.ros_distro }} - strategy: - fail-fast: false - matrix: - ROS_REPO: [ main, testing ] + name: ${{ inputs.ros_distro }} ${{ inputs.ros_repo }} ${{ inputs.os_code_name }} runs-on: ubuntu-latest env: CCACHE_DIR: ${{ github.workspace }}/${{ inputs.ccache_dir }} BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }} - CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.os_code_name }}-${{ matrix.ROS_REPO }}-${{ github.job }} + CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.os_code_name }}-${{ inputs.ros_repo }}-${{ github.job }} steps: - name: Checkout ${{ inputs.ref }} when build is not scheduled - if: ${{ (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) && github.event_name != 'schedule' }} + if: ${{ github.event_name != 'schedule' }} uses: actions/checkout@v3 - name: Checkout ${{ inputs.ref }} on scheduled build - if: ${{ (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) && github.event_name == 'schedule' }} + if: ${{ github.event_name == 'schedule' }} uses: actions/checkout@v3 with: ref: ${{ inputs.ref_for_scheduled_build }} - name: cache target_ws - if: ${{ ! matrix.env.CCOV && (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) }} + if: ${{ ! matrix.env.CCOV }} uses: pat-s/always-upload-cache@v2.1.5 with: path: ${{ env.BASEDIR }}/target_ws @@ -77,7 +73,6 @@ jobs: restore-keys: | target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} - name: cache ccache - if: ${{ inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO }} uses: pat-s/always-upload-cache@v2.1.5 with: path: ${{ env.CCACHE_DIR }} @@ -85,22 +80,17 @@ jobs: restore-keys: | ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} ccache-${{ env.CACHE_PREFIX }} - - if: ${{ inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO }} - uses: 'ros-industrial/industrial_ci@master' + - uses: 'ros-industrial/industrial_ci@master' env: UPSTREAM_WORKSPACE: ${{ inputs.upstream_workspace }} ROS_DISTRO: ${{ inputs.ros_distro }} - ROS_REPO: ${{ matrix.ROS_REPO }} + ROS_REPO: ${{ inputs.ros_repo }} OS_CODE_NAME: ${{ inputs.os_code_name }} BEFORE_INSTALL_UPSTREAM_DEPENDENCIES: ${{ inputs.before_install_upstream_dependencies }} - name: prepare target_ws for cache - if: ${{ always() && ! matrix.env.CCOV && (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) }} + if: ${{ always() && ! matrix.env.CCOV }} run: | du -sh ${{ env.BASEDIR }}/target_ws sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete sudo rm -rf ${{ env.BASEDIR }}/target_ws/src du -sh ${{ env.BASEDIR }}/target_ws - - name: Is job skipped? - if: ${{ ! (inputs.ros_repo == 'all' || inputs.ros_repo == matrix.ROS_REPO) }} - run: | - echo "This job is skpped!" diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 287809991d..2ad549b0f8 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -13,41 +13,37 @@ on: description: 'Reference on which the repo should be checkout. Usually is this name of a branch or a tag.' required: true type: string + ros2_repo_branch: + description: 'Branch in the ros2/ros2 repozitory from which ".repos" should be used. Possible values: master (Rolling), humble, galactic, foxy.' + default: 'master' + required: false + type: string jobs: reusable_ros_tooling_source_build: - name: ${{ inputs.ros_distro }} ubuntu-20.04 - runs-on: ubuntu-20.04 + name: ${{ inputs.ros_distro }} ubuntu-22.04 + runs-on: ubuntu-22.04 strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@v0.3 + - uses: ros-tooling/setup-ros@0.3.4 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@v0.2 + - uses: ros-tooling/action-ros-ci@0.2.6 with: target-ros2-distro: ${{ inputs.ros_distro }} # build all packages listed in the meta package package-name: diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - joint_state_broadcaster - joint_trajectory_controller - gripper_controllers - position_controllers - ros2_controllers - ros2_controllers_test_nodes - velocity_controllers + vcs-repo-file-url: | - https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ref }}/ros2.repos + https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - uses: actions/upload-artifact@v1 with: - name: colcon-logs-ubuntu-20.04 + name: colcon-logs-ubuntu-22.04 path: ros_ws/log diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index 1b327f58fb..4589e92e3b 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -1,5 +1,8 @@ -name: ABI Compatibility Check +name: Rolling - ABI Compatibility Check on: + workflow_dispatch: + branches: + - master pull_request: branches: - master diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build-main.yml similarity index 60% rename from .github/workflows/rolling-binary-build.yml rename to .github/workflows/rolling-binary-build-main.yml index 7e0a2e04d6..b51bbabe29 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build-main.yml @@ -1,7 +1,11 @@ -name: Rolling Binary Build -# Build & test all dependencies from released (binary) packages. +name: Rolling Binary Build - main +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' on: + workflow_dispatch: + branches: + - master pull_request: branches: - master @@ -10,12 +14,13 @@ on: - master schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '08 18 * * *' + - cron: '03 1 * * *' jobs: binary: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: rolling + ros_repo: main upstream_workspace: ros2_controllers-not-released.rolling.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-binary-build-testing.yml b/.github/workflows/rolling-binary-build-testing.yml new file mode 100644 index 0000000000..a1db89b8d9 --- /dev/null +++ b/.github/workflows/rolling-binary-build-testing.yml @@ -0,0 +1,26 @@ +name: Rolling Binary Build - testing +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: rolling + ros_repo: testing + upstream_workspace: ros2_controllers-not-released.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index 62197a2db9..e85b439411 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -1,5 +1,8 @@ name: Rolling RHEL Binary Build on: + workflow_dispatch: + branches: + - master pull_request: branches: - master @@ -12,20 +15,19 @@ on: jobs: -# TODO(andyz): disabled because it started failing at the transition to Humble -# rolling_rhel_binary: -# name: Rolling RHEL binary build -# runs-on: ubuntu-latest -# env: -# ROS_DISTRO: rolling -# container: jaronl/ros:rolling-alma -# steps: -# - uses: actions/checkout@v3 -# with: -# path: src/ros2_controllers -# - run: | -# rosdep update -# rosdep install -iy --from-path src/ros2_controllers -# source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash -# colcon build -# colcon test + rolling_rhel_binary: + name: Rolling RHEL binary build + runs-on: ubuntu-latest + env: + ROS_DISTRO: rolling + container: jaronl/ros:rolling-alma + steps: + - uses: actions/checkout@v3 + with: + path: src/ros2_controllers + - run: | + rosdep update + rosdep install -iy --from-path src/ros2_controllers + source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash + colcon build + colcon test diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build-main.yml similarity index 64% rename from .github/workflows/rolling-semi-binary-build.yml rename to .github/workflows/rolling-semi-binary-build-main.yml index 25666c0b0c..eca9483438 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build-main.yml @@ -1,7 +1,10 @@ -name: Rolling Semi-Binary Build -# Build & test that compiles the main dependencies from source. +name: Rolling Semi-Binary Build - main +# description: 'Build & test that compiles the main dependencies from source.' on: + workflow_dispatch: + branches: + - master pull_request: branches: - master @@ -10,12 +13,13 @@ on: - master schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '13 18 * * *' + - cron: '33 1 * * *' jobs: semi_binary: uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml with: ros_distro: rolling + ros_repo: main upstream_workspace: ros2_controllers.rolling.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-semi-binary-build-testing.yml b/.github/workflows/rolling-semi-binary-build-testing.yml new file mode 100644 index 0000000000..62214adae9 --- /dev/null +++ b/.github/workflows/rolling-semi-binary-build-testing.yml @@ -0,0 +1,25 @@ +name: Rolling Semi-Binary Build - testing +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + branches: + - master + pull_request: + branches: + - master + push: + branches: + - master + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml + with: + ros_distro: rolling + ros_repo: testing + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index 0a0ae8de43..1a137a795b 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -1,5 +1,8 @@ name: Rolling Source Build on: + workflow_dispatch: + branches: + - master push: branches: - master @@ -13,3 +16,4 @@ jobs: with: ros_distro: rolling ref: master + ros2_repo_branch: master-rolling diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index ba3dd242ac..fa8d75100e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.1.0 + rev: v4.3.0 hooks: - id: check-added-large-files - id: check-ast @@ -33,24 +33,29 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v2.31.1 + rev: v2.37.3 hooks: - - id: pyupgrade + - id: pyupgrade args: [--py36-plus] + - repo: https://github.com/psf/black + rev: 22.6.0 + hooks: + - id: black + args: ["--line-length=99"] + # PEP 257 - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257 rev: v0.3.3 hooks: - - id: pep257 - args: - ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] + - id: pep257 + args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/pycqa/flake8 - rev: 4.0.1 + rev: 5.0.4 hooks: - - id: flake8 - args: ["--ignore=E501"] + - id: flake8 + args: ["--ignore=E501"] # CPP hooks - repo: local @@ -58,10 +63,16 @@ repos: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format-11 + entry: clang-format-12 language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ - args: ["-fallback-style=none", "-i"] + args: ['-fallback-style=none', '-i'] + # The same options as in ament_cppcheck are used, but its not working... + #- repo: https://github.com/pocc/pre-commit-hooks + #rev: v1.1.1 + #hooks: + #- id: cppcheck + #args: ['--error-exitcode=1', '-f', '--inline-suppr', '-q', '-rp', '--suppress=internalAstError', '--suppress=unknownMacro', '--verbose'] - repo: local hooks: @@ -108,10 +119,10 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: 0.10.1 + rev: v1.0.0 hooks: - id: doc8 - args: ["--max-line-length=100", "--ignore=D001"] + args: ['--max-line-length=100', '--ignore=D001'] exclude: CHANGELOG\.rst$ - repo: https://github.com/pre-commit/pygrep-hooks @@ -128,5 +139,5 @@ repos: rev: v2.1.0 hooks: - id: codespell - args: ["--write-changes"] - exclude: CHANGELOG\.rst|\.(svg|pyc)$ + args: ['--write-changes'] + exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ diff --git a/README.md b/README.md index d7aa74a173..6a59d0054e 100644 --- a/README.md +++ b/README.md @@ -6,11 +6,10 @@ Commonly used and generalized controllers for ros2-control framework that are re ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: -**Rolling** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml?branch=master)
[![Rolling Source Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-source-build.yml?branch=master) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) -**Rolling - last Focal** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-last-focal.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-last-focal.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-last-focal.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-last-focal.yml?branch=master) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) -**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_controllers/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build.yml?branch=galactic)
[![Galactic Source Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-source-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-source-build.yml?branch=galactic) | [Documentation](https://control.ros.org/galactic/)
[API Reference](https://control.ros.org/galactic/api/) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#galactic) -**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_controllers/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build.yml?branch=foxy)
[![Foxy Source Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-source-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-source-build.yml?branch=foxy) | [Documentation](https://control.ros.org/foxy/)
[API Reference](https://control.ros.org/foxy/api/) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#foxy) - +**Rolling** | [`rolling`](https://github.com/ros-controls/ros2_controllers/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-main.yml?branch=master) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) +**Humble** | [`humble`](https://github.com/ros-controls/ros2_controllers/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-main.yml?branch=master) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#humble) +**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_controllers/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-binary-build-main.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_controllers/actions/workflows/galactic-semi-binary-build-main.yml?branch=galactic) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#galactic) +**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_controllers/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#foxy) ### Explanation of different build types @@ -25,8 +24,6 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages Uses repos file: `src/$NAME$/$NAME$.repos` -1. Source build - also core ROS packages are build from source. It shows potential issues in the mid future. - ## Acknowledgements diff --git a/ros2_controllers-not-released.foxy.repos b/ros2_controllers-not-released.foxy.repos index 56f46b6f79..1b3910e7e7 100644 --- a/ros2_controllers-not-released.foxy.repos +++ b/ros2_controllers-not-released.foxy.repos @@ -1 +1,6 @@ repositories: + ## EXAMPLE DEPENDENCY +# : +# type: git +# url: git@github.com:/.git +# version: master diff --git a/ros2_controllers-not-released.galactic.repos b/ros2_controllers-not-released.galactic.repos index 56f46b6f79..1b3910e7e7 100644 --- a/ros2_controllers-not-released.galactic.repos +++ b/ros2_controllers-not-released.galactic.repos @@ -1 +1,6 @@ repositories: + ## EXAMPLE DEPENDENCY +# : +# type: git +# url: git@github.com:/.git +# version: master diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos new file mode 100644 index 0000000000..1b3910e7e7 --- /dev/null +++ b/ros2_controllers-not-released.humble.repos @@ -0,0 +1,6 @@ +repositories: + ## EXAMPLE DEPENDENCY +# : +# type: git +# url: git@github.com:/.git +# version: master diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index 56f46b6f79..1b3910e7e7 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -1 +1,6 @@ repositories: + ## EXAMPLE DEPENDENCY +# : +# type: git +# url: git@github.com:/.git +# version: master diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos new file mode 100644 index 0000000000..3a667e7d9a --- /dev/null +++ b/ros2_controllers.humble.repos @@ -0,0 +1,13 @@ +repositories: + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: master + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: galactic-devel + realtime_tools: + type: git + url: https://github.com/ros-controls/realtime_tools.git + version: master diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 5b2339a137..3a667e7d9a 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -1,17 +1,13 @@ repositories: - ros-controls/ros2_control: + ros2_control: type: git url: https://github.com/ros-controls/ros2_control.git version: master control_msgs: type: git url: https://github.com/ros-controls/control_msgs.git - version: foxy-devel + version: galactic-devel realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git - version: foxy-devel - angles: - type: git - url: https://github.com/ros/angles.git - version: ros2 + version: master From c74e31a29255fa2f66f45874195d50a0f5cba471 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 10 Aug 2022 19:28:15 +0100 Subject: [PATCH 017/126] Reinstate JTC tests (#391) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix correct_initialization_using_parameters * Fix configure_state_ignores_command * Re-enable incorrect_initialization_using_interface_parameters * Re-enable cleanup and activate tests * Port gtest to gmock Co-authored-by: Denis Štogl --- joint_trajectory_controller/CMakeLists.txt | 10 +- .../interpolation_methods.hpp | 3 +- joint_trajectory_controller/package.xml | 2 +- .../src/joint_trajectory_controller.cpp | 58 +- .../src/trajectory.cpp | 3 +- .../test_load_joint_trajectory_controller.cpp | 2 +- .../test/test_trajectory.cpp | 2 +- .../test/test_trajectory_actions.cpp | 20 +- .../test/test_trajectory_controller.cpp | 1808 +++++++++-------- .../test/test_trajectory_controller_utils.hpp | 4 +- 10 files changed, 973 insertions(+), 939 deletions(-) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 793b736a05..5689e589f8 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -51,15 +51,15 @@ install(TARGETS ${PROJECT_NAME} ) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gtest(test_trajectory test/test_trajectory.cpp) + ament_add_gmock(test_trajectory test/test_trajectory.cpp) target_include_directories(test_trajectory PRIVATE include) target_link_libraries(test_trajectory ${PROJECT_NAME}) - ament_add_gtest(test_trajectory_controller + ament_add_gmock(test_trajectory_controller test/test_trajectory_controller.cpp ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml) set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) @@ -71,7 +71,7 @@ if(BUILD_TESTING) ${THIS_PACKAGE_INCLUDE_DEPENDS} ) - ament_add_gtest( + ament_add_gmock( test_load_joint_trajectory_controller test/test_load_joint_trajectory_controller.cpp ) @@ -84,7 +84,7 @@ if(BUILD_TESTING) ) # TODO(andyz): Disabled due to flakiness - # ament_add_gtest( + # ament_add_gmock( # test_trajectory_actions # test/test_trajectory_actions.cpp # ) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp index 1cebd56f8a..766be6e0f4 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp @@ -15,10 +15,11 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_ -#include #include #include +#include "rclcpp/rclcpp.hpp" + namespace joint_trajectory_controller { static const rclcpp::Logger LOGGER = diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 66bb5b0999..00e6e9f6cb 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -27,7 +27,7 @@ rclcpp_lifecycle trajectory_msgs - ament_cmake_gtest + ament_cmake_gmock controller_manager ros2_control_test_assets diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 21501dfb67..5898a6fe95 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -134,7 +134,8 @@ controller_interface::return_type JointTrajectoryController::update( auto compute_error_for_joint = [&]( JointTrajectoryPoint & error, int index, const JointTrajectoryPoint & current, - const JointTrajectoryPoint & desired) { + const JointTrajectoryPoint & desired) + { // error defined as the difference between current and desired error.positions[index] = angles::shortest_angular_distance(current.positions[index], desired.positions[index]); @@ -162,12 +163,13 @@ controller_interface::return_type JointTrajectoryController::update( // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not // changed, but its value only? auto assign_interface_from_point = - [&](auto & joint_interface, const std::vector & trajectory_point_interface) { - for (size_t index = 0; index < dof_; ++index) - { - joint_interface[index].get().set_value(trajectory_point_interface[index]); - } - }; + [&](auto & joint_interface, const std::vector & trajectory_point_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + joint_interface[index].get().set_value(trajectory_point_interface[index]); + } + }; // current state update state_current_.time_from_start.set__sec(0); @@ -368,12 +370,13 @@ controller_interface::return_type JointTrajectoryController::update( void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) { auto assign_point_from_interface = - [&](std::vector & trajectory_point_interface, const auto & joint_interface) { - for (size_t index = 0; index < dof_; ++index) - { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); - } - }; + [&](std::vector & trajectory_point_interface, const auto & joint_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index] = joint_interface[index].get().get_value(); + } + }; // Assign values from the hardware // Position states always exist @@ -406,17 +409,20 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto bool has_values = true; auto assign_point_from_interface = - [&](std::vector & trajectory_point_interface, const auto & joint_interface) { - for (size_t index = 0; index < dof_; ++index) - { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); - } - }; + [&](std::vector & trajectory_point_interface, const auto & joint_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index] = joint_interface[index].get().get_value(); + } + }; - auto interface_has_values = [](const auto & joint_interface) { - return std::find_if(joint_interface.begin(), joint_interface.end(), [](const auto & interface) { - return std::isnan(interface.get().get_value()); - }) == joint_interface.end(); + auto interface_has_values = [](const auto & joint_interface) + { + return std::find_if( + joint_interface.begin(), joint_interface.end(), + [](const auto & interface) + { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); }; // Assign values from the command interfaces as state. Therefore needs check for both. @@ -676,7 +682,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( } } - auto get_interface_list = [](const std::vector & interface_types) { + auto get_interface_list = [](const std::vector & interface_types) + { std::stringstream ss_interfaces; for (size_t index = 0; index < interface_types.size(); ++index) { @@ -1133,7 +1140,8 @@ void JointTrajectoryController::sort_to_local_joint_order( std::vector mapping_vector = mapping(trajectory_msg->joint_names, joint_names_); auto remap = [this]( const std::vector & to_remap, - const std::vector & mapping) -> std::vector { + const std::vector & mapping) -> std::vector + { if (to_remap.empty()) { return to_remap; diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index a65329d289..f28703efc9 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -179,7 +179,8 @@ void Trajectory::interpolate_between_points( output.velocities.resize(dim, 0.0); output.accelerations.resize(dim, 0.0); - auto generate_powers = [](int n, double x, double * powers) { + auto generate_powers = [](int n, double x, double * powers) + { powers[0] = 1.0; for (int i = 1; i <= n; ++i) { diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp index 7f80badebc..e0dfdc6353 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include "controller_manager/controller_manager.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index cef8ad00dd..93e3560c07 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -17,7 +17,7 @@ #include #include -#include "gtest/gtest.h" +#include "gmock/gmock.h" #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index cc91071e0c..7128c0bee2 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -87,16 +87,18 @@ class TestTrajectoryActions : public TrajectoryControllerTest { setup_controller_hw_ = true; - controller_hw_thread_ = std::thread([&]() { - // controller hardware cycle update loop - auto start_time = rclcpp::Clock().now(); - rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0); - auto end_time = start_time + wait; - while (rclcpp::Clock().now() < end_time) + controller_hw_thread_ = std::thread( + [&]() { - traj_controller_->update(rclcpp::Clock().now(), rclcpp::Clock().now() - start_time); - } - }); + // controller hardware cycle update loop + auto start_time = rclcpp::Clock().now(); + rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0); + auto end_time = start_time + wait; + while (rclcpp::Clock().now() < end_time) + { + traj_controller_->update(rclcpp::Clock().now(), rclcpp::Clock().now() - start_time); + } + }); // sometimes doesn't receive calls when we don't sleep std::this_thread::sleep_for(std::chrono::milliseconds(300)); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index e4be504717..576f6a30d7 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -25,7 +25,7 @@ #include #include -#include "gtest/gtest.h" +#include "gmock/gmock.h" #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" @@ -54,7 +54,6 @@ #include "trajectory_msgs/msg/joint_trajectory_point.hpp" using lifecycle_msgs::msg::State; -using test_trajectory_controllers::TestableJointTrajectoryController; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; @@ -62,26 +61,31 @@ bool is_same_sign(double val1, double val2) { return val1 * val2 >= 0.0; } void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } -TEST_P(TrajectoryControllerTestParameterized, configure) +TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { SetUpTrajectoryController(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(traj_controller_->get_node()->get_node_base_interface()); - const auto future_handle_ = std::async(std::launch::async, spin, &executor); + // const auto future_handle_ = std::async(std::launch::async, spin, &executor); const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // send msg - builtin_interfaces::msg::Duration time_from_start; - time_from_start.sec = 1; - time_from_start.nanosec = 0; - std::vector> points{{{3.3, 4.4, 5.5}}}; - publish(time_from_start, points, rclcpp::Time()); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + traj_controller_->update( + rclcpp::Time(static_cast(0.5 * 1e9)), rclcpp::Duration::from_seconds(0.5)); // hw position == 0 because controller is not activated EXPECT_EQ(0.0, joint_pos_[0]); @@ -237,13 +241,19 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) executor.add_node(traj_node->get_node_base_interface()); // send msg - builtin_interfaces::msg::Duration time_from_start; - time_from_start.sec = 1; - time_from_start.nanosec = 0; - std::vector> points{{{3.3, 4.4, 5.5}}}; - publish(time_from_start, points, rclcpp::Time()); + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + + traj_controller_->update( + rclcpp::Time(static_cast(0.5 * 1e9)), rclcpp::Duration::from_seconds(0.5)); auto state = traj_controller_->get_node()->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); @@ -269,7 +279,6 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // This call is replacing the way parameters are set via launch SetParameters(); - SetParameters(); // This call is replacing the way parameters are set via launch traj_controller_->configure(); auto state = traj_controller_->get_state(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); @@ -297,18 +306,16 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param traj_controller_->wait_for_trajectory(executor); // first update - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); // wait so controller process the second point when deactivated - std::this_thread::sleep_for(FIRST_POINT_TIME); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + traj_controller_->update( + rclcpp::Time(static_cast(0.25 * 1e9)), rclcpp::Duration::from_seconds(0.15)); // deactivated state = traj_controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - // TODO(denis): on my laptop I get delta of approx 0.1083. Is this me or is it something wrong? - // Come the flackiness here? - const auto allowed_delta = 0.11; // 0.05; + const auto allowed_delta = 0.05; if (traj_controller_->has_position_command_interface()) { EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); @@ -332,907 +339,919 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // cleanup state = traj_controller_->get_node()->cleanup(); - - // update loop receives a new msg and updates accordingly - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - - // check the traj_msg_home_ptr_ initialization code for the standard wait timing - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], allowed_delta); - EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], allowed_delta); - EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], allowed_delta); - - state = traj_controller_->get_node()->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - - executor.cancel(); -} - -TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) -{ - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {}, &executor); - subscribeToState(); - updateController(); - - // Spin to receive latest state - executor.spin_some(); - auto state = getState(); - - size_t n_joints = joint_names_.size(); - - for (unsigned int i = 0; i < n_joints; ++i) - { - EXPECT_EQ(joint_names_[i], state->joint_names[i]); - } - - // No trajectory by default, no desired state or error - EXPECT_TRUE(state->desired.positions.empty()); - EXPECT_TRUE(state->desired.velocities.empty()); - EXPECT_TRUE(state->desired.accelerations.empty()); - - EXPECT_EQ(n_joints, state->actual.positions.size()); - if ( - std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == - state_interface_types_.end()) - { - EXPECT_TRUE(state->actual.velocities.empty()); - } - else - { - EXPECT_EQ(n_joints, state->actual.velocities.size()); - } - if ( - std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == - state_interface_types_.end()) - { - EXPECT_TRUE(state->actual.accelerations.empty()); - } - else - { - EXPECT_EQ(n_joints, state->actual.accelerations.size()); - } - - EXPECT_TRUE(state->error.positions.empty()); - EXPECT_TRUE(state->error.velocities.empty()); - EXPECT_TRUE(state->error.accelerations.empty()); -} - -void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) -{ - rclcpp::Parameter state_publish_rate_param( - "state_publish_rate", static_cast(target_msg_count)); - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {state_publish_rate_param}, &executor); - - auto future_handle = std::async(std::launch::async, [&executor]() -> void { executor.spin(); }); - - using control_msgs::msg::JointTrajectoryControllerState; - - const int qos_level = 10; - int echo_received_counter = 0; - rclcpp::Subscription::SharedPtr subs = - traj_controller_->get_node()->create_subscription( - controller_name_ + "/state", qos_level, - [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); + // TODO(anyone): should the controller even allow calling update() when it is not active? + // update loop receives a new msg and updates accordingly + traj_controller_->update( + rclcpp::Time(static_cast(0.35 * 1e9)), rclcpp::Duration::from_seconds(0.1)); - // update for 1second - const auto start_time = rclcpp::Clock().now(); - const rclcpp::Duration wait = rclcpp::Duration::from_seconds(1.0); - const auto end_time = start_time + wait; - while (rclcpp::Clock().now() < end_time) - { - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - } + EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); + EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); + EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); - // We may miss the last message since time allowed is exactly the time needed - EXPECT_NEAR(target_msg_count, echo_received_counter, 1); + // state = traj_controller_->get_node()->configure(); + // ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); executor.cancel(); } -/** - * @brief test_state_publish_rate Test that state publish rate matches configure rate - */ -TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) -{ - test_state_publish_rate_target(10); -} +// TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {}, &executor); +// subscribeToState(); +// updateController(); -TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) -{ - test_state_publish_rate_target(0); -} +// // Spin to receive latest state +// executor.spin_some(); +// auto state = getState(); -/** - * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from internal controller order - */ -TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) -{ - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {}, &executor); - { - trajectory_msgs::msg::JointTrajectory traj_msg; - const std::vector jumbled_joint_names{ - joint_names_[1], joint_names_[2], joint_names_[0]}; - traj_msg.joint_names = jumbled_joint_names; - traj_msg.header.stamp = rclcpp::Time(0); - traj_msg.points.resize(1); - - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - traj_msg.points[0].positions.resize(3); - traj_msg.points[0].positions[0] = 2.0; - traj_msg.points[0].positions[1] = 3.0; - traj_msg.points[0].positions[2] = 1.0; - - if (traj_controller_->has_velocity_command_interface()) - { - traj_msg.points[0].velocities.resize(3); - traj_msg.points[0].velocities[0] = -0.1; - traj_msg.points[0].velocities[1] = -0.1; - traj_msg.points[0].velocities[2] = -0.1; - } - - if (traj_controller_->has_effort_command_interface()) - { - traj_msg.points[0].effort.resize(3); - traj_msg.points[0].effort[0] = -0.1; - traj_msg.points[0].effort[1] = -0.1; - traj_msg.points[0].effort[2] = -0.1; - } - - trajectory_publisher_->publish(traj_msg); - } - - traj_controller_->wait_for_trajectory(executor); - // update for 0.25 seconds - // TODO(destogl): Make this time a bit shorter to increase stability on the CI? - // Currently COMMON_THRESHOLD is adjusted. - updateController(rclcpp::Duration::from_seconds(0.25)); - - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); - } - - if (traj_controller_->has_velocity_command_interface()) - { - EXPECT_GT(0.0, joint_vel_[0]); - EXPECT_GT(0.0, joint_vel_[1]); - EXPECT_GT(0.0, joint_vel_[2]); - } - - if (traj_controller_->has_effort_command_interface()) - { - EXPECT_GT(0.0, joint_eff_[0]); - EXPECT_GT(0.0, joint_eff_[1]); - EXPECT_GT(0.0, joint_eff_[2]); - } -} +// size_t n_joints = joint_names_.size(); -/** - * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled joints - */ -TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) -{ - rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); - - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); - - const double initial_joint1_cmd = joint_pos_[0]; - const double initial_joint2_cmd = joint_pos_[1]; - const double initial_joint3_cmd = joint_pos_[2]; - trajectory_msgs::msg::JointTrajectory traj_msg; +// for (unsigned int i = 0; i < n_joints; ++i) +// { +// EXPECT_EQ(joint_names_[i], state->joint_names[i]); +// } - { - std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; - traj_msg.joint_names = partial_joint_names; - traj_msg.header.stamp = rclcpp::Time(0); - traj_msg.points.resize(1); - - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - traj_msg.points[0].positions.resize(2); - traj_msg.points[0].positions[0] = 2.0; - traj_msg.points[0].positions[1] = 1.0; - traj_msg.points[0].velocities.resize(2); - traj_msg.points[0].velocities[0] = - copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); - traj_msg.points[0].velocities[1] = - copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); - - trajectory_publisher_->publish(traj_msg); - } +// // No trajectory by default, no desired state or error +// EXPECT_TRUE(state->desired.positions.empty()); +// EXPECT_TRUE(state->desired.velocities.empty()); +// EXPECT_TRUE(state->desired.accelerations.empty()); + +// EXPECT_EQ(n_joints, state->actual.positions.size()); +// if ( +// std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == +// state_interface_types_.end()) +// { +// EXPECT_TRUE(state->actual.velocities.empty()); +// } +// else +// { +// EXPECT_EQ(n_joints, state->actual.velocities.size()); +// } +// if ( +// std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == +// state_interface_types_.end()) +// { +// EXPECT_TRUE(state->actual.accelerations.empty()); +// } +// else +// { +// EXPECT_EQ(n_joints, state->actual.accelerations.size()); +// } - traj_controller_->wait_for_trajectory(executor); - // update for 0.5 seconds - updateController(rclcpp::Duration::from_seconds(0.25)); +// EXPECT_TRUE(state->error.positions.empty()); +// EXPECT_TRUE(state->error.velocities.empty()); +// EXPECT_TRUE(state->error.accelerations.empty()); +// } - double threshold = 0.001; +// void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) +// { +// rclcpp::Parameter state_publish_rate_param( +// "state_publish_rate", static_cast(target_msg_count)); +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {state_publish_rate_param}, &executor); + +// auto future_handle = std::async( +// std::launch::async, [&executor]() -> void { executor.spin(); }); + +// using control_msgs::msg::JointTrajectoryControllerState; + +// const int qos_level = 10; +// int echo_received_counter = 0; +// rclcpp::Subscription::SharedPtr subs = +// traj_controller_->get_node()->create_subscription( +// controller_name_ + "/state", qos_level, +// [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); + +// // update for 1second +// const auto start_time = rclcpp::Clock().now(); +// const rclcpp::Duration wait = rclcpp::Duration::from_seconds(1.0); +// const auto end_time = start_time + wait; +// while (rclcpp::Clock().now() < end_time) +// { +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// } - if (traj_controller_->has_position_command_interface()) - { - EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); - EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) - << "Joint 3 command should be current position"; - } +// // We may miss the last message since time allowed is exactly the time needed +// EXPECT_NEAR(target_msg_count, echo_received_counter, 1); - if ( - std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != - command_interface_types_.end()) - { - // estimate the sign of the velocity - // joint rotates forward - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); - EXPECT_NEAR(0.0, joint_vel_[2], threshold) - << "Joint 3 velocity should be 0.0 since it's not in the goal"; - } +// executor.cancel(); +// } - if ( - std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != - command_interface_types_.end()) - { - // estimate the sign of the velocity - // joint rotates forward - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); - EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); - EXPECT_NEAR(0.0, joint_eff_[2], threshold) - << "Joint 3 effort should be 0.0 since it's not in the goal"; - } - // TODO(anyone): add here ckecks for acceleration commands +// /** +// * @brief test_state_publish_rate Test that state publish rate matches configure rate +// */ +// TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) +// { +// test_state_publish_rate_target(10); +// } - executor.cancel(); -} +// TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) +// { +// test_state_publish_rate_target(0); +// } -/** - * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled joints without allow_partial_joints_goal - */ -TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowed) -{ - rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); +// /** +// * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from +// * internal controller order +// */ +// TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {}, &executor); +// { +// trajectory_msgs::msg::JointTrajectory traj_msg; +// const std::vector jumbled_joint_names{ +// joint_names_[1], joint_names_[2], joint_names_[0]}; +// traj_msg.joint_names = jumbled_joint_names; +// traj_msg.header.stamp = rclcpp::Time(0); +// traj_msg.points.resize(1); + +// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); +// traj_msg.points[0].positions.resize(3); +// traj_msg.points[0].positions[0] = 2.0; +// traj_msg.points[0].positions[1] = 3.0; +// traj_msg.points[0].positions[2] = 1.0; + +// if (traj_controller_->has_velocity_command_interface()) +// { +// traj_msg.points[0].velocities.resize(3); +// traj_msg.points[0].velocities[0] = -0.1; +// traj_msg.points[0].velocities[1] = -0.1; +// traj_msg.points[0].velocities[2] = -0.1; +// } + +// if (traj_controller_->has_effort_command_interface()) +// { +// traj_msg.points[0].effort.resize(3); +// traj_msg.points[0].effort[0] = -0.1; +// traj_msg.points[0].effort[1] = -0.1; +// traj_msg.points[0].effort[2] = -0.1; +// } + +// trajectory_publisher_->publish(traj_msg); +// } - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); +// traj_controller_->wait_for_trajectory(executor); +// // update for 0.25 seconds +// // TODO(destogl): Make this time a bit shorter to increase stability on the CI? +// // Currently COMMON_THRESHOLD is adjusted. +// updateController(rclcpp::Duration::from_seconds(0.25)); + +// if (traj_controller_->has_position_command_interface()) +// { +// EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); +// EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); +// EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); +// } - const double initial_joint1_cmd = joint_pos_[0]; - const double initial_joint2_cmd = joint_pos_[1]; - const double initial_joint3_cmd = joint_pos_[2]; - const double initial_joint_vel = 0.0; - const double initial_joint_acc = 0.0; - trajectory_msgs::msg::JointTrajectory traj_msg; +// if (traj_controller_->has_velocity_command_interface()) +// { +// EXPECT_GT(0.0, joint_vel_[0]); +// EXPECT_GT(0.0, joint_vel_[1]); +// EXPECT_GT(0.0, joint_vel_[2]); +// } - { - std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; - traj_msg.joint_names = partial_joint_names; - traj_msg.header.stamp = rclcpp::Time(0); - traj_msg.points.resize(1); - - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - traj_msg.points[0].positions.resize(2); - traj_msg.points[0].positions[0] = 2.0; - traj_msg.points[0].positions[1] = 1.0; - traj_msg.points[0].velocities.resize(2); - traj_msg.points[0].velocities[0] = 2.0; - traj_msg.points[0].velocities[1] = 1.0; - - trajectory_publisher_->publish(traj_msg); - } +// if (traj_controller_->has_effort_command_interface()) +// { +// EXPECT_GT(0.0, joint_eff_[0]); +// EXPECT_GT(0.0, joint_eff_[1]); +// EXPECT_GT(0.0, joint_eff_[2]); +// } +// } - traj_controller_->wait_for_trajectory(executor); - // update for 0.5 seconds - updateController(rclcpp::Duration::from_seconds(0.25)); +// /** +// * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled +// * joints +// */ +// TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) +// { +// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); + +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + +// const double initial_joint1_cmd = joint_pos_[0]; +// const double initial_joint2_cmd = joint_pos_[1]; +// const double initial_joint3_cmd = joint_pos_[2]; +// trajectory_msgs::msg::JointTrajectory traj_msg; + +// { +// std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; +// traj_msg.joint_names = partial_joint_names; +// traj_msg.header.stamp = rclcpp::Time(0); +// traj_msg.points.resize(1); + +// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); +// traj_msg.points[0].positions.resize(2); +// traj_msg.points[0].positions[0] = 2.0; +// traj_msg.points[0].positions[1] = 1.0; +// traj_msg.points[0].velocities.resize(2); +// traj_msg.points[0].velocities[0] = +// copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); +// traj_msg.points[0].velocities[1] = +// copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); + +// trajectory_publisher_->publish(traj_msg); +// } - double threshold = 0.001; - EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], threshold) - << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], threshold) - << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) - << "All joints command should be current position because goal was rejected"; - - if ( - std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != - command_interface_types_.end()) - { - EXPECT_NEAR(initial_joint_vel, joint_vel_[0], threshold) - << "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(initial_joint_vel, joint_vel_[1], threshold) - << "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(initial_joint_vel, joint_vel_[2], threshold) - << "All joints velocities should be 0.0 because goal was rejected"; - } +// traj_controller_->wait_for_trajectory(executor); +// // update for 0.5 seconds +// updateController(rclcpp::Duration::from_seconds(0.25)); - if ( - std::find(command_interface_types_.begin(), command_interface_types_.end(), "acceleration") != - command_interface_types_.end()) - { - EXPECT_NEAR(initial_joint_acc, joint_acc_[0], threshold) - << "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(initial_joint_acc, joint_acc_[1], threshold) - << "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(initial_joint_acc, joint_acc_[2], threshold) - << "All joints accelerations should be 0.0 because goal was rejected"; - } +// double threshold = 0.001; - executor.cancel(); -} +// if (traj_controller_->has_position_command_interface()) +// { +// EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); +// EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); +// EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) +// << "Joint 3 command should be current position"; +// } -/** - * @brief invalid_message Test mismatched joint and reference vector sizes - */ -TEST_P(TrajectoryControllerTestParameterized, invalid_message) -{ - rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); - rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", false); - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController( - true, {partial_joints_parameters, allow_integration_parameters}, &executor); - - trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; - - good_traj_msg.joint_names = joint_names_; - good_traj_msg.header.stamp = rclcpp::Time(0); - good_traj_msg.points.resize(1); - good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - good_traj_msg.points[0].positions.resize(1); - good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; - good_traj_msg.points[0].velocities.resize(1); - good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; - EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); - - // Incompatible joint names - traj_msg = good_traj_msg; - traj_msg.joint_names = {"bad_name"}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // No position data - traj_msg = good_traj_msg; - traj_msg.points[0].positions.clear(); - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few positions - traj_msg = good_traj_msg; - traj_msg.points[0].positions = {1.0, 2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too many positions - traj_msg = good_traj_msg; - traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few velocities - traj_msg = good_traj_msg; - traj_msg.points[0].velocities = {1.0, 2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few accelerations - traj_msg = good_traj_msg; - traj_msg.points[0].accelerations = {1.0, 2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few efforts - traj_msg = good_traj_msg; - traj_msg.points[0].positions.clear(); - traj_msg.points[0].effort = {1.0, 2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Non-strictly increasing waypoint times - traj_msg = good_traj_msg; - traj_msg.points.push_back(traj_msg.points.front()); - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); -} +// if ( +// std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != +// command_interface_types_.end()) +// { +// // estimate the sign of the velocity +// // joint rotates forward +// EXPECT_TRUE( +// is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); +// EXPECT_TRUE( +// is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); +// EXPECT_NEAR(0.0, joint_vel_[2], threshold) +// << "Joint 3 velocity should be 0.0 since it's not in the goal"; +// } -/// With allow_integration_in_goal_trajectories parameter trajectory missing position or velocities -/// are accepted -TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted) -{ - rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {allow_integration_parameters}, &executor); - - trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; - - good_traj_msg.joint_names = joint_names_; - good_traj_msg.header.stamp = rclcpp::Time(0); - good_traj_msg.points.resize(1); - good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); - good_traj_msg.points[0].positions.resize(1); - good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; - good_traj_msg.points[0].velocities.resize(1); - good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; - good_traj_msg.points[0].accelerations.resize(1); - good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0}; - EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); - - // No position data - traj_msg = good_traj_msg; - traj_msg.points[0].positions.clear(); - EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // No position and velocity data - traj_msg = good_traj_msg; - traj_msg.points[0].positions.clear(); - traj_msg.points[0].velocities.clear(); - EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // All empty - traj_msg = good_traj_msg; - traj_msg.points[0].positions.clear(); - traj_msg.points[0].velocities.clear(); - traj_msg.points[0].accelerations.clear(); - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few positions - traj_msg = good_traj_msg; - traj_msg.points[0].positions = {1.0, 2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too many positions - traj_msg = good_traj_msg; - traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few velocities - traj_msg = good_traj_msg; - traj_msg.points[0].velocities = {1.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); - - // Incompatible data sizes, too few accelerations - traj_msg = good_traj_msg; - traj_msg.points[0].accelerations = {2.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); -} +// if ( +// std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != +// command_interface_types_.end()) +// { +// // estimate the sign of the velocity +// // joint rotates forward +// EXPECT_TRUE( +// is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); +// EXPECT_TRUE( +// is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); +// EXPECT_NEAR(0.0, joint_eff_[2], threshold) +// << "Joint 3 effort should be 0.0 since it's not in the goal"; +// } +// // TODO(anyone): add here ckecks for acceleration commands -/** - * @brief test_trajectory_replace Test replacing an existing trajectory - */ -TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) -{ - rclcpp::executors::SingleThreadedExecutor executor; - rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); - SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); - - subscribeToState(); - - std::vector> points_old{{{2., 3., 4.}}}; - std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; - std::vector> points_partial_new{{1.5}}; - std::vector> points_partial_new_velocities{{0.15}}; - - const auto delay = std::chrono::milliseconds(500); - builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; - publish(time_from_start, points_old, rclcpp::Time(), {}, points_old_velocities); - trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; - expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; - expected_desired.positions = {points_old[0].begin(), points_old[0].end()}; - expected_actual.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; - expected_desired.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; - // Check that we reached end of points_old trajectory - // Denis: delta was 0.1 with 0.2 works for me - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); - - RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); - points_partial_new_velocities[0][0] = - copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); - publish(time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); - - // Replaced trajectory is a mix of previous and current goal - expected_desired.positions[0] = points_partial_new[0][0]; - expected_desired.positions[1] = points_old[0][1]; - expected_desired.positions[2] = points_old[0][2]; - expected_desired.velocities[0] = points_partial_new_velocities[0][0]; - expected_desired.velocities[1] = 0.0; - expected_desired.velocities[2] = 0.0; - expected_actual = expected_desired; - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); -} +// executor.cancel(); +// } -/** - * @brief test_ignore_old_trajectory Sending an old trajectory replacing an existing trajectory - */ -TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) -{ - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {}, &executor); - subscribeToState(); - - // TODO(anyone): add expectations for velocities and accelerations - std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; - std::vector> points_new{{{-1., -2., -3.}}}; - - RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); - const auto delay = std::chrono::milliseconds(500); - builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; - publish(time_from_start, points_old, rclcpp::Time()); - trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; - expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; - expected_desired = expected_actual; - // Check that we reached end of points_old[0] trajectory - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - - RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); - // New trajectory will end before current time - rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); - expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; - expected_desired = expected_actual; - std::cout << "Sending old trajectory" << std::endl; - publish(time_from_start, points_new, new_traj_start); - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); -} +// /** +// * @brief test_partial_joint_list Test sending trajectories with a subset of the controlled +// * joints without allow_partial_joints_goal +// */ +// TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowed) +// { +// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); + +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + +// const double initial_joint1_cmd = joint_pos_[0]; +// const double initial_joint2_cmd = joint_pos_[1]; +// const double initial_joint3_cmd = joint_pos_[2]; +// const double initial_joint_vel = 0.0; +// const double initial_joint_acc = 0.0; +// trajectory_msgs::msg::JointTrajectory traj_msg; + +// { +// std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; +// traj_msg.joint_names = partial_joint_names; +// traj_msg.header.stamp = rclcpp::Time(0); +// traj_msg.points.resize(1); + +// traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); +// traj_msg.points[0].positions.resize(2); +// traj_msg.points[0].positions[0] = 2.0; +// traj_msg.points[0].positions[1] = 1.0; +// traj_msg.points[0].velocities.resize(2); +// traj_msg.points[0].velocities[0] = 2.0; +// traj_msg.points[0].velocities[1] = 1.0; + +// trajectory_publisher_->publish(traj_msg); +// } -TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) -{ - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {}, &executor); - subscribeToState(); - - std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; - std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; - - const auto delay = std::chrono::milliseconds(500); - builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; - publish(time_from_start, points_old, rclcpp::Time()); - trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; - expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; - expected_desired = expected_actual; - // Check that we reached end of points_old[0]trajectory - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - - RCLCPP_INFO( - traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); - // New trajectory first point is in the past, second is in the future - rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); - expected_actual.positions = {points_new[1].begin(), points_new[1].end()}; - expected_desired = expected_actual; - publish(time_from_start, points_new, new_traj_start); - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); -} +// traj_controller_->wait_for_trajectory(executor); +// // update for 0.5 seconds +// updateController(rclcpp::Duration::from_seconds(0.25)); + +// double threshold = 0.001; +// EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], threshold) +// << "All joints command should be current position because goal was rejected"; +// EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], threshold) +// << "All joints command should be current position because goal was rejected"; +// EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) +// << "All joints command should be current position because goal was rejected"; + +// if ( +// std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != +// command_interface_types_.end()) +// { +// EXPECT_NEAR(initial_joint_vel, joint_vel_[0], threshold) +// << "All joints velocities should be 0.0 because goal was rejected"; +// EXPECT_NEAR(initial_joint_vel, joint_vel_[1], threshold) +// << "All joints velocities should be 0.0 because goal was rejected"; +// EXPECT_NEAR(initial_joint_vel, joint_vel_[2], threshold) +// << "All joints velocities should be 0.0 because goal was rejected"; +// } -TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) -{ - SetUpTrajectoryController(); - auto traj_node = traj_controller_->get_node(); - RCLCPP_WARN( - traj_node->get_logger(), - "Test disabled until current_trajectory is taken into account when adding a new trajectory."); - // https://github.com/ros-controls/ros_controllers/blob/melodic-devel/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L149 - return; - - // TODO(anyone): use SetUpAndActivateTrajectoryController method instead of the next line - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(traj_node->get_node_base_interface()); - subscribeToState(); - rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); - traj_node->set_parameter(partial_joints_parameters); - traj_controller_->get_node()->configure(); - traj_controller_->get_node()->activate(); - - std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; - std::vector> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}}; - std::vector> partial_traj{ - {{-1., -2.}, - { - -2., - -4, - }}}; - std::vector> partial_traj_velocities{ - {{-0.1, -0.2}, - { - -0.2, - -0.4, - }}}; - const auto delay = std::chrono::milliseconds(500); - builtin_interfaces::msg::Duration points_delay{rclcpp::Duration(delay)}; - // Send full trajectory - publish(points_delay, full_traj, rclcpp::Time(), {}, full_traj_velocities); - // Sleep until first waypoint of full trajectory - - trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; - expected_actual.positions = {full_traj[0].begin(), full_traj[0].end()}; - expected_desired = expected_actual; - // Check that we reached end of points_old[0]trajectory and are starting points_old[1] - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); - - // Send partial trajectory starting after full trajecotry is complete - RCLCPP_INFO(traj_node->get_logger(), "Sending new trajectory in the future"); - publish( - points_delay, partial_traj, rclcpp::Clock().now() + delay * 2, {}, partial_traj_velocities); - // Wait until the end start and end of partial traj - - expected_actual.positions = {partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]}; - expected_desired = expected_actual; - - waitAndCompareState( - expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1); -} +// if ( +// std::find( +// command_interface_types_.begin(), command_interface_types_.end(), "acceleration") != +// command_interface_types_.end()) +// { +// EXPECT_NEAR(initial_joint_acc, joint_acc_[0], threshold) +// << "All joints accelerations should be 0.0 because goal was rejected"; +// EXPECT_NEAR(initial_joint_acc, joint_acc_[1], threshold) +// << "All joints accelerations should be 0.0 because goal was rejected"; +// EXPECT_NEAR(initial_joint_acc, joint_acc_[2], threshold) +// << "All joints accelerations should be 0.0 because goal was rejected"; +// } -TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) -{ - rclcpp::executors::SingleThreadedExecutor executor; - // default if false so it will not be actually set parameter - rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); - SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); - - // goal setup - std::vector first_goal = {3.3, 4.4, 5.5}; - std::vector> first_goal_velocities = {{0.33, 0.44, 0.55}}; - std::vector second_goal = {6.6, 8.8, 11.0}; - std::vector> second_goal_velocities = {{0.66, 0.88, 1.1}}; - double state_from_command_offset = 0.3; +// executor.cancel(); +// } - // send msg - builtin_interfaces::msg::Duration time_from_start; - time_from_start.sec = 1; - time_from_start.nanosec = 0; - std::vector> points{{first_goal}}; - publish(time_from_start, points, rclcpp::Time(), {}, first_goal_velocities); - traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(1.1)); +// /** +// * @brief invalid_message Test mismatched joint and reference vector sizes +// */ +// TEST_P(TrajectoryControllerTestParameterized, invalid_message) +// { +// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false); +// rclcpp::Parameter allow_integration_parameters( +// "allow_integration_in_goal_trajectories", false); +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController( +// true, {partial_joints_parameters, allow_integration_parameters}, &executor); + +// trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; + +// good_traj_msg.joint_names = joint_names_; +// good_traj_msg.header.stamp = rclcpp::Time(0); +// good_traj_msg.points.resize(1); +// good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); +// good_traj_msg.points[0].positions.resize(1); +// good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; +// good_traj_msg.points[0].velocities.resize(1); +// good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; +// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + +// // Incompatible joint names +// traj_msg = good_traj_msg; +// traj_msg.joint_names = {"bad_name"}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // No position data +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions.clear(); +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few positions +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions = {1.0, 2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too many positions +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few velocities +// traj_msg = good_traj_msg; +// traj_msg.points[0].velocities = {1.0, 2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few accelerations +// traj_msg = good_traj_msg; +// traj_msg.points[0].accelerations = {1.0, 2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few efforts +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions.clear(); +// traj_msg.points[0].effort = {1.0, 2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Non-strictly increasing waypoint times +// traj_msg = good_traj_msg; +// traj_msg.points.push_back(traj_msg.points.front()); +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +// } - if (traj_controller_->has_position_command_interface()) - { - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points, rclcpp::Time(), {}, second_goal_velocities); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there should be a "jump" in opposite direction from command - // (towards the state value) - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - - // Move joint back to the first goal - points = {{first_goal}}; - publish(time_from_start, points, rclcpp::Time()); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there should be a "jump" in the goal direction from command - // (towards the state value) - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - } +// /// With allow_integration_in_goal_trajectories parameter trajectory missing position or +// /// velocities are accepted +// TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted) +// { +// rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {allow_integration_parameters}, &executor); + +// trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg; + +// good_traj_msg.joint_names = joint_names_; +// good_traj_msg.header.stamp = rclcpp::Time(0); +// good_traj_msg.points.resize(1); +// good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); +// good_traj_msg.points[0].positions.resize(1); +// good_traj_msg.points[0].positions = {1.0, 2.0, 3.0}; +// good_traj_msg.points[0].velocities.resize(1); +// good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; +// good_traj_msg.points[0].accelerations.resize(1); +// good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0}; +// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(good_traj_msg)); + +// // No position data +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions.clear(); +// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // No position and velocity data +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions.clear(); +// traj_msg.points[0].velocities.clear(); +// EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // All empty +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions.clear(); +// traj_msg.points[0].velocities.clear(); +// traj_msg.points[0].accelerations.clear(); +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few positions +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions = {1.0, 2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too many positions +// traj_msg = good_traj_msg; +// traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few velocities +// traj_msg = good_traj_msg; +// traj_msg.points[0].velocities = {1.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + +// // Incompatible data sizes, too few accelerations +// traj_msg = good_traj_msg; +// traj_msg.points[0].accelerations = {2.0}; +// EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +// } - executor.cancel(); -} +// /** +// * @brief test_trajectory_replace Test replacing an existing trajectory +// */ +// TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); +// SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + +// subscribeToState(); + +// std::vector> points_old{{{2., 3., 4.}}}; +// std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; +// std::vector> points_partial_new{{1.5}}; +// std::vector> points_partial_new_velocities{{0.15}}; + +// const auto delay = std::chrono::milliseconds(500); +// builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; +// publish(time_from_start, points_old, rclcpp::Time(), {}, points_old_velocities); +// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; +// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; +// expected_desired.positions = {points_old[0].begin(), points_old[0].end()}; +// expected_actual.velocities = { +// points_old_velocities[0].begin(), points_old_velocities[0].end()}; +// expected_desired.velocities = { +// points_old_velocities[0].begin(), points_old_velocities[0].end()}; +// // Check that we reached end of points_old trajectory +// // Denis: delta was 0.1 with 0.2 works for me +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); + +// RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); +// points_partial_new_velocities[0][0] = +// copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); +// publish( +// time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); + +// // Replaced trajectory is a mix of previous and current goal +// expected_desired.positions[0] = points_partial_new[0][0]; +// expected_desired.positions[1] = points_old[0][1]; +// expected_desired.positions[2] = points_old[0][2]; +// expected_desired.velocities[0] = points_partial_new_velocities[0][0]; +// expected_desired.velocities[1] = 0.0; +// expected_desired.velocities[2] = 0.0; +// expected_actual = expected_desired; +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); +// } -TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) -{ - rclcpp::executors::SingleThreadedExecutor executor; - // default if false so it will not be actually set parameter - rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); - SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); +// /** +// * @brief test_ignore_old_trajectory Sending an old trajectory replacing an existing trajectory +// */ +// TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {}, &executor); +// subscribeToState(); + +// // TODO(anyone): add expectations for velocities and accelerations +// std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; +// std::vector> points_new{{{-1., -2., -3.}}}; + +// RCLCPP_INFO( +// traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); +// const auto delay = std::chrono::milliseconds(500); +// builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; +// publish(time_from_start, points_old, rclcpp::Time()); +// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; +// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; +// expected_desired = expected_actual; +// // Check that we reached end of points_old[0] trajectory +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + +// RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); +// // New trajectory will end before current time +// rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); +// expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; +// expected_desired = expected_actual; +// std::cout << "Sending old trajectory" << std::endl; +// publish(time_from_start, points_new, new_traj_start); +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); +// } - // goal setup - std::vector first_goal = {3.3, 4.4, 5.5}; - std::vector second_goal = {6.6, 8.8, 11.0}; - double state_from_command_offset = 0.3; +// TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// SetUpAndActivateTrajectoryController(true, {}, &executor); +// subscribeToState(); + +// std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; +// std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; + +// const auto delay = std::chrono::milliseconds(500); +// builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; +// publish(time_from_start, points_old, rclcpp::Time()); +// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; +// expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; +// expected_desired = expected_actual; +// // Check that we reached end of points_old[0]trajectory +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + +// RCLCPP_INFO( +// traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); +// // New trajectory first point is in the past, second is in the future +// rclcpp::Time new_traj_start = rclcpp::Clock().now() - delay - std::chrono::milliseconds(100); +// expected_actual.positions = {points_new[1].begin(), points_new[1].end()}; +// expected_desired = expected_actual; +// publish(time_from_start, points_new, new_traj_start); +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); +// } - // send msg - builtin_interfaces::msg::Duration time_from_start; - time_from_start.sec = 1; - time_from_start.nanosec = 0; - std::vector> points{{first_goal}}; - publish(time_from_start, points, rclcpp::Time()); - traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(1.1)); +// TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) +// { +// SetUpTrajectoryController(); +// auto traj_node = traj_controller_->get_node(); +// RCLCPP_WARN( +// traj_node->get_logger(), +// "Test disabled until current_trajectory is taken into account when adding a new trajectory."); +// // https://github.com/ros-controls/ros_controllers/blob/melodic-devel/ +// // joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L149 +// return; + +// // TODO(anyone): use SetUpAndActivateTrajectoryController method instead of the next line +// rclcpp::executors::SingleThreadedExecutor executor; +// executor.add_node(traj_node->get_node_base_interface()); +// subscribeToState(); +// rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); +// traj_node->set_parameter(partial_joints_parameters); +// traj_controller_->get_node()->configure(); +// traj_controller_->get_node()->activate(); + +// std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; +// std::vector> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}}; +// std::vector> partial_traj{ +// {{-1., -2.}, +// { +// -2., +// -4, +// }}}; +// std::vector> partial_traj_velocities{ +// {{-0.1, -0.2}, +// { +// -0.2, +// -0.4, +// }}}; +// const auto delay = std::chrono::milliseconds(500); +// builtin_interfaces::msg::Duration points_delay{rclcpp::Duration(delay)}; +// // Send full trajectory +// publish(points_delay, full_traj, rclcpp::Time(), {}, full_traj_velocities); +// // Sleep until first waypoint of full trajectory + +// trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; +// expected_actual.positions = {full_traj[0].begin(), full_traj[0].end()}; +// expected_desired = expected_actual; +// // Check that we reached end of points_old[0]trajectory and are starting points_old[1] +// waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + +// // Send partial trajectory starting after full trajecotry is complete +// RCLCPP_INFO(traj_node->get_logger(), "Sending new trajectory in the future"); +// publish( +// points_delay, partial_traj, rclcpp::Clock().now() + delay * 2, {}, partial_traj_velocities); +// // Wait until the end start and end of partial traj + +// expected_actual.positions = { +// partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]}; +// expected_desired = expected_actual; + +// waitAndCompareState( +// expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1); +// } - if (traj_controller_->has_position_command_interface()) - { - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points, rclcpp::Time()); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there **should not** be a "jump" in opposite direction from command - // (towards the state value) - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // There should not be backward commands - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - - // Move joint back to the first goal - points = {{first_goal}}; - publish(time_from_start, points, rclcpp::Time()); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there **should not** be a "jump" in the goal direction from command - // (towards the state value) - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // There should not be a jump toward commands - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], second_goal[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - } +// TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// // default if false so it will not be actually set parameter +// rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); +// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); - executor.cancel(); -} +// // goal setup +// std::vector first_goal = {3.3, 4.4, 5.5}; +// std::vector> first_goal_velocities = {{0.33, 0.44, 0.55}}; +// std::vector second_goal = {6.6, 8.8, 11.0}; +// std::vector> second_goal_velocities = {{0.66, 0.88, 1.1}}; +// double state_from_command_offset = 0.3; -// Testing that values are read from state interfaces when hardware is started for the first -// time and hardware state has offset --> this is indicated by NaN values in state interfaces -TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start) -{ - rclcpp::executors::SingleThreadedExecutor executor; - // default if false so it will not be actually set parameter - rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); +// // send msg +// builtin_interfaces::msg::Duration time_from_start; +// time_from_start.sec = 1; +// time_from_start.nanosec = 0; +// std::vector> points{{first_goal}}; +// publish(time_from_start, points, rclcpp::Time(), {}, first_goal_velocities); +// traj_controller_->wait_for_trajectory(executor); +// updateController(rclcpp::Duration::from_seconds(1.1)); + +// if (traj_controller_->has_position_command_interface()) +// { +// // JTC is executing trajectory in open-loop therefore: +// // - internal state does not have to be updated (in this test-case it shouldn't) +// // - internal command is updated +// EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + +// // State interface should have offset from the command before starting a new trajectory +// joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + +// // Move joint further in the same direction as before (to the second goal) +// points = {{second_goal}}; +// publish(time_from_start, points, rclcpp::Time(1.0), {}, second_goal_velocities); +// traj_controller_->wait_for_trajectory(executor); + +// // One the first update(s) there should be a "jump" in opposite direction from command +// // (towards the state value) +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// // Expect backward commands at first +// EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); +// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_LT(joint_pos_[0], first_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_LT(joint_pos_[0], first_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_LT(joint_pos_[0], first_goal[0]); + +// // Finally the second goal will be commanded/reached +// updateController(rclcpp::Duration::from_seconds(1.1)); +// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + +// // State interface should have offset from the command before starting a new trajectory +// joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + +// // Move joint back to the first goal +// points = {{first_goal}}; +// publish(time_from_start, points, rclcpp::Time()); +// traj_controller_->wait_for_trajectory(executor); + +// // One the first update(s) there should be a "jump" in the goal direction from command +// // (towards the state value) +// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// // Expect backward commands at first +// EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); +// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); +// EXPECT_GT(joint_pos_[0], first_goal[0]); + +// // Finally the first goal will be commanded/reached +// updateController(rclcpp::Duration::from_seconds(1.1)); +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// } - // set command values to NaN - for (size_t i = 0; i < 3; ++i) - { - joint_pos_[i] = std::numeric_limits::quiet_NaN(); - joint_vel_[i] = std::numeric_limits::quiet_NaN(); - joint_acc_[i] = std::numeric_limits::quiet_NaN(); - } - SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); +// executor.cancel(); +// } - auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); +// TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// // default if false so it will not be actually set parameter +// rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); +// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); - for (size_t i = 0; i < 3; ++i) - { - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); - - // check velocity - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && - std::find( - command_interface_types_.begin(), command_interface_types_.end(), - hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) - { - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); - } - - // check acceleration - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && - std::find( - command_interface_types_.begin(), command_interface_types_.end(), - hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) - { - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); - } - } +// // goal setup +// std::vector first_goal = {3.3, 4.4, 5.5}; +// std::vector second_goal = {6.6, 8.8, 11.0}; +// double state_from_command_offset = 0.3; - executor.cancel(); -} +// // send msg +// builtin_interfaces::msg::Duration time_from_start; +// time_from_start.sec = 1; +// time_from_start.nanosec = 0; +// std::vector> points{{first_goal}}; +// publish(time_from_start, points, rclcpp::Time()); +// traj_controller_->wait_for_trajectory(executor); +// updateController(rclcpp::Duration::from_seconds(1.1)); + +// if (traj_controller_->has_position_command_interface()) +// { +// // JTC is executing trajectory in open-loop therefore: +// // - internal state does not have to be updated (in this test-case it shouldn't) +// // - internal command is updated +// EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + +// // State interface should have offset from the command before starting a new trajectory +// joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + +// // Move joint further in the same direction as before (to the second goal) +// points = {{second_goal}}; +// publish(time_from_start, points, rclcpp::Time()); +// traj_controller_->wait_for_trajectory(executor); + +// // One the first update(s) there **should not** be a "jump" in opposite direction from +// // command (towards the state value) +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// // There should not be backward commands +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// EXPECT_LT(joint_pos_[0], second_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// EXPECT_LT(joint_pos_[0], second_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// EXPECT_LT(joint_pos_[0], second_goal[0]); + +// // Finally the second goal will be commanded/reached +// updateController(rclcpp::Duration::from_seconds(1.1)); +// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + +// // State interface should have offset from the command before starting a new trajectory +// joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + +// // Move joint back to the first goal +// points = {{first_goal}}; +// publish(time_from_start, points, rclcpp::Time()); +// traj_controller_->wait_for_trajectory(executor); + +// // One the first update(s) there **should not** be a "jump" in the goal direction from +// // command (towards the state value) +// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// // There should not be a jump toward commands +// EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// EXPECT_LT(joint_pos_[0], second_goal[0]); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// EXPECT_LT(joint_pos_[0], second_goal[0]); +// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// EXPECT_GT(joint_pos_[0], first_goal[0]); +// EXPECT_LT(joint_pos_[0], second_goal[0]); + +// // Finally the first goal will be commanded/reached +// updateController(rclcpp::Duration::from_seconds(1.1)); +// EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); +// } -// Testing that values are read from state interfaces when hardware is started after some values -// are set on the hardware commands -TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_controller_start) -{ - rclcpp::executors::SingleThreadedExecutor executor; - // default if false so it will not be actually set parameter - rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); +// executor.cancel(); +// } - // set command values to NaN - for (size_t i = 0; i < 3; ++i) - { - joint_pos_[i] = 3.1 + i; - joint_vel_[i] = 0.25 + i; - joint_acc_[i] = 0.02 + i / 10.0; - } - SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); +// // Testing that values are read from state interfaces when hardware is started for the first +// // time and hardware state has offset --> this is indicated by NaN values in state interfaces +// TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// // default if false so it will not be actually set parameter +// rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + +// // set command values to NaN +// for (size_t i = 0; i < 3; ++i) +// { +// joint_pos_[i] = std::numeric_limits::quiet_NaN(); +// joint_vel_[i] = std::numeric_limits::quiet_NaN(); +// joint_acc_[i] = std::numeric_limits::quiet_NaN(); +// } +// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); + +// auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + +// for (size_t i = 0; i < 3; ++i) +// { +// EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + +// // check velocity +// if ( +// std::find( +// state_interface_types_.begin(), state_interface_types_.end(), +// hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && +// std::find( +// command_interface_types_.begin(), command_interface_types_.end(), +// hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) +// { +// EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); +// } + +// // check acceleration +// if ( +// std::find( +// state_interface_types_.begin(), state_interface_types_.end(), +// hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && +// std::find( +// command_interface_types_.begin(), command_interface_types_.end(), +// hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) +// { +// EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); +// } +// } - auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); +// executor.cancel(); +// } - for (size_t i = 0; i < 3; ++i) - { - EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); - - // check velocity - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && - std::find( - command_interface_types_.begin(), command_interface_types_.end(), - hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) - { - EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); - } - - // check acceleration - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && - std::find( - command_interface_types_.begin(), command_interface_types_.end(), - hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) - { - EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); - } - } +// // Testing that values are read from state interfaces when hardware is started after some values +// // are set on the hardware commands +// TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_controller_start) +// { +// rclcpp::executors::SingleThreadedExecutor executor; +// // default if false so it will not be actually set parameter +// rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); + +// // set command values to NaN +// for (size_t i = 0; i < 3; ++i) +// { +// joint_pos_[i] = 3.1 + i; +// joint_vel_[i] = 0.25 + i; +// joint_acc_[i] = 0.02 + i / 10.0; +// } +// SetUpAndActivateTrajectoryController(true, {is_open_loop_parameters}, &executor, true); + +// auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); + +// for (size_t i = 0; i < 3; ++i) +// { +// EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); + +// // check velocity +// if ( +// std::find( +// state_interface_types_.begin(), state_interface_types_.end(), +// hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && +// std::find( +// command_interface_types_.begin(), command_interface_types_.end(), +// hardware_interface::HW_IF_VELOCITY) != command_interface_types_.end()) +// { +// EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); +// } + +// // check acceleration +// if ( +// std::find( +// state_interface_types_.begin(), state_interface_types_.end(), +// hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && +// std::find( +// command_interface_types_.begin(), command_interface_types_.end(), +// hardware_interface::HW_IF_ACCELERATION) != command_interface_types_.end()) +// { +// EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); +// } +// } - executor.cancel(); -} +// executor.cancel(); +// } -// TODO(andyz): disabled because they started failing at the transition to Humble -/* // position controllers INSTANTIATE_TEST_SUITE_P( PositionTrajectoryControllers, TrajectoryControllerTestParameterized, @@ -1271,30 +1290,31 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"position", "velocity", "acceleration"}), std::vector({"position", "velocity", "acceleration"})))); -// only velocity controller -INSTANTIATE_TEST_SUITE_P( - OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, - ::testing::Values( - std::make_tuple( - std::vector({"velocity"}), std::vector({"position", "velocity"})), - std::make_tuple( - std::vector({"velocity"}), - std::vector({"position", "velocity", "acceleration"})))); - -// only effort controller -INSTANTIATE_TEST_SUITE_P( - OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized, - ::testing::Values( - std::make_tuple( - std::vector({"effort"}), std::vector({"position", "velocity"})), - std::make_tuple( - std::vector({"effort"}), - std::vector({"position", "velocity", "acceleration"})))); -*/ +// // only velocity controller +// INSTANTIATE_TEST_SUITE_P( +// OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, +// ::testing::Values( +// std::make_tuple( +// std::vector({"velocity"}), +// std::vector({"position", "velocity"})), +// std::make_tuple( +// std::vector({"velocity"}), +// std::vector({"position", "velocity", "acceleration"})))); + +// // only effort controller +// INSTANTIATE_TEST_SUITE_P( +// OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized, +// ::testing::Values( +// std::make_tuple( +// std::vector({"effort"}), std::vector({"position", "velocity"})), +// std::make_tuple( +// std::vector({"effort"}), +// std::vector({"position", "velocity", "acceleration"})))); TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) { - auto set_parameter_and_check_result = [&]() { + auto set_parameter_and_check_result = [&]() + { EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); SetParameters(); // This call is replacing the way parameters are set via launch traj_controller_->get_node()->configure(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 5c55d87972..f12bd8b8bc 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -265,7 +265,9 @@ class TrajectoryControllerTest : public ::testing::Test // I do not understand why spin_some provides only one message qos.keep_last(1); state_subscriber_ = traj_lifecycle_node->create_subscription( - controller_name_ + "/state", qos, [&](std::shared_ptr msg) { + controller_name_ + "/state", qos, + [&](std::shared_ptr msg) + { std::lock_guard guard(state_mutex_); state_msg_ = msg; }); From e718ae9b1048593d0c961f6a4f92f3813f616d10 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 15 Aug 2022 10:13:58 +0200 Subject: [PATCH 018/126] Fix formatting because pre-commit was not running on CI for some time. (#409) --- .pre-commit-config.yaml | 2 +- .../double_editor.py | 14 ++- .../joint_limits_urdf.py | 69 ++++++----- .../joint_trajectory_controller.py | 116 +++++++++--------- .../rqt_joint_trajectory_controller.py | 18 ++- .../update_combo.py | 7 +- .../rqt_joint_trajectory_controller/utils.py | 79 ++++++------ rqt_joint_trajectory_controller/setup.py | 1 - .../config/tricycle_drive_controller.yaml | 4 +- tricycle_controller/doc/userdoc.rst | 6 +- .../tricycle_controller/steering_limiter.hpp | 8 +- .../tricycle_controller/traction_limiter.hpp | 6 +- .../tricycle_controller.hpp | 7 +- tricycle_controller/src/steering_limiter.cpp | 4 +- tricycle_controller/src/traction_limiter.cpp | 6 +- .../src/tricycle_controller.cpp | 39 +++--- .../test/config/test_tricycle_controller.yaml | 2 +- .../test/test_load_tricycle_controller.cpp | 4 +- 18 files changed, 203 insertions(+), 189 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index fa8d75100e..58fe46dd90 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -139,5 +139,5 @@ repos: rev: v2.1.0 hooks: - id: codespell - args: ['--write-changes'] + args: ['--write-changes', '--uri-ignore-words-list=ist'] exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py index 3fdde9daf9..9a96639436 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/double_editor.py @@ -43,6 +43,7 @@ class DoubleEditor(QWidget): # DoubleEditor? """ Widget that allows to edit the value of a floating-point value. + Optionally subject to lower and upper bounds. """ @@ -59,15 +60,17 @@ def __init__(self, min_val, max_val): self._max_val = max_val # Load editor UI - ui_file = os.path.join(get_package_share_directory('rqt_joint_trajectory_controller'), - 'resource', 'double_editor.ui') + ui_file = os.path.join( + get_package_share_directory("rqt_joint_trajectory_controller"), + "resource", + "double_editor.ui", + ) loadUi(ui_file, self) # Setup widget ranges and slider scale factor self.slider.setRange(0, 100) self.slider.setSingleStep(1) - self._scale = (max_val - min_val) / \ - (self.slider.maximum() - self.slider.minimum()) + self._scale = (max_val - min_val) / (self.slider.maximum() - self.slider.minimum()) self.spin_box.setRange(min_val, max_val) self.spin_box.setSingleStep(self._scale) @@ -82,8 +85,7 @@ def _slider_to_val(self, sval): return self._min_val + self._scale * (sval - self.slider.minimum()) def _val_to_slider(self, val): - return round(self.slider.minimum() + (val - self._min_val) / - self._scale) + return round(self.slider.minimum() + (val - self._min_val) / self._scale) def _on_slider_changed(self): val = self._slider_to_val(self.slider.value()) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py index 6f339ba387..d7517b1847 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -1,5 +1,19 @@ #!/usr/bin/env python +# Copyright 2022 PAL Robotics S.L. +# +# 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. + # TODO: Use urdf_parser_py.urdf instead. I gave it a try, but got # Exception: Required attribute not set in XML: upper # upper is an optional attribute, so I don't understand what's going on @@ -19,7 +33,7 @@ def callback(msg): description = msg.data -def get_joint_limits(node, key='robot_description', use_smallest_joint_limits=True): +def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=True): global description use_small = use_smallest_joint_limits use_mimic = True @@ -39,53 +53,50 @@ def get_joint_limits(node, key='robot_description', use_smallest_joint_limits=Tr dependent_joints = {} if description != "": - robot = xml.dom.minidom.parseString(description)\ - .getElementsByTagName('robot')[0] + robot = xml.dom.minidom.parseString(description).getElementsByTagName("robot")[0] # Find all non-fixed joints for child in robot.childNodes: if child.nodeType is child.TEXT_NODE: continue - if child.localName == 'joint': - jtype = child.getAttribute('type') - if jtype == 'fixed': + if child.localName == "joint": + jtype = child.getAttribute("type") + if jtype == "fixed": continue - name = child.getAttribute('name') + name = child.getAttribute("name") try: - limit = child.getElementsByTagName('limit')[0] + limit = child.getElementsByTagName("limit")[0] except: continue - if jtype == 'continuous': + if jtype == "continuous": minval = -pi maxval = pi else: try: - minval = float(limit.getAttribute('lower')) - maxval = float(limit.getAttribute('upper')) + minval = float(limit.getAttribute("lower")) + maxval = float(limit.getAttribute("upper")) except: continue try: - maxvel = float(limit.getAttribute('velocity')) + maxvel = float(limit.getAttribute("velocity")) except: continue - safety_tags = child.getElementsByTagName('safety_controller') + safety_tags = child.getElementsByTagName("safety_controller") if use_small and len(safety_tags) == 1: tag = safety_tags[0] - if tag.hasAttribute('soft_lower_limit'): - minval = max(minval, - float(tag.getAttribute('soft_lower_limit'))) - if tag.hasAttribute('soft_upper_limit'): - maxval = min(maxval, - float(tag.getAttribute('soft_upper_limit'))) - - mimic_tags = child.getElementsByTagName('mimic') + if tag.hasAttribute("soft_lower_limit"): + minval = max(minval, float(tag.getAttribute("soft_lower_limit"))) + if tag.hasAttribute("soft_upper_limit"): + maxval = min(maxval, float(tag.getAttribute("soft_upper_limit"))) + + mimic_tags = child.getElementsByTagName("mimic") if use_mimic and len(mimic_tags) == 1: tag = mimic_tags[0] - entry = {'parent': tag.getAttribute('joint')} - if tag.hasAttribute('multiplier'): - entry['factor'] = float(tag.getAttribute('multiplier')) - if tag.hasAttribute('offset'): - entry['offset'] = float(tag.getAttribute('offset')) + entry = {"parent": tag.getAttribute("joint")} + if tag.hasAttribute("multiplier"): + entry["factor"] = float(tag.getAttribute("multiplier")) + if tag.hasAttribute("offset"): + entry["offset"] = float(tag.getAttribute("offset")) dependent_joints[name] = entry continue @@ -93,8 +104,8 @@ def get_joint_limits(node, key='robot_description', use_smallest_joint_limits=Tr if name in dependent_joints: continue - joint = {'min_position': minval, 'max_position': maxval} - joint["has_position_limits"] = jtype != 'continuous' - joint['max_velocity'] = maxvel + joint = {"min_position": minval, "max_position": maxval} + joint["has_position_limits"] = jtype != "continuous" + joint["max_velocity"] = maxvel free_joints[name] = joint return free_joints diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 7cb9ea5346..ab55d6eb78 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -99,7 +99,7 @@ class JointTrajectoryController(Plugin): def __init__(self, context): super().__init__(context) - self.setObjectName('JointTrajectoryController') + self.setObjectName("JointTrajectoryController") self._node = rclpy.node.Node("rqt_joint_trajectory_controller") self._executor = None self._executor_thread = None @@ -107,17 +107,19 @@ def __init__(self, context): # Create QWidget and extend it with all the attributes and children # from the UI file self._widget = QWidget() - ui_file = os.path.join(get_package_share_directory('rqt_joint_trajectory_controller'), - 'resource', - 'joint_trajectory_controller.ui') + ui_file = os.path.join( + get_package_share_directory("rqt_joint_trajectory_controller"), + "resource", + "joint_trajectory_controller.ui", + ) loadUi(ui_file, self._widget) - self._widget.setObjectName('JointTrajectoryControllerUi') + self._widget.setObjectName("JointTrajectoryControllerUi") ns = self._node.get_namespace()[1:-1] - self._widget.controller_group.setTitle('ns: ' + ns) + self._widget.controller_group.setTitle("ns: " + ns) # Setup speed scaler speed_scaling = DoubleEditor(1.0, 100.0) - speed_scaling.spin_box.setSuffix('%') + speed_scaling.spin_box.setSuffix("%") speed_scaling.spin_box.setValue(50.0) speed_scaling.spin_box.setDecimals(0) speed_scaling.setEnabled(False) @@ -132,8 +134,9 @@ def __init__(self, context): # plugin at once, these lines add number to make it easy to # tell from pane to pane. if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + - (' (%d)' % context.serial_number())) + self._widget.setWindowTitle( + self._widget.windowTitle() + (" (%d)" % context.serial_number()) + ) # Add widget to the user interface context.add_widget(self._widget) @@ -151,22 +154,19 @@ def __init__(self, context): # Timer for updating the joint widgets from the controller state self._update_act_pos_timer = QTimer(self) - self._update_act_pos_timer.setInterval(int(1000.0 / - self._widget_update_freq)) + self._update_act_pos_timer.setInterval(int(1000.0 / self._widget_update_freq)) self._update_act_pos_timer.timeout.connect(self._update_joint_widgets) # Timer for controller manager updates self._list_cm = ControllerManagerLister() self._update_cm_list_timer = QTimer(self) - self._update_cm_list_timer.setInterval(int(1000.0 / - self._ctrlrs_update_freq)) + self._update_cm_list_timer.setInterval(int(1000.0 / self._ctrlrs_update_freq)) self._update_cm_list_timer.timeout.connect(self._update_cm_list) self._update_cm_list_timer.start() # Timer for running controller updates self._update_jtc_list_timer = QTimer(self) - self._update_jtc_list_timer.setInterval(int(1000.0 / - self._ctrlrs_update_freq)) + self._update_jtc_list_timer.setInterval(int(1000.0 / self._ctrlrs_update_freq)) self._update_jtc_list_timer.timeout.connect(self._update_jtc_list) self._update_jtc_list_timer.start() @@ -176,7 +176,7 @@ def __init__(self, context): w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change) w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change) - self._cmd_pub = None # Controller command publisher + self._cmd_pub = None # Controller command publisher self._state_sub = None # Controller state subscriber self._list_controllers = None @@ -191,13 +191,13 @@ def shutdown_plugin(self): self._unregister_executor() def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('cm_ns', self._cm_ns) - instance_settings.set_value('jtc_name', self._jtc_name) + instance_settings.set_value("cm_ns", self._cm_ns) + instance_settings.set_value("jtc_name", self._jtc_name) def restore_settings(self, plugin_settings, instance_settings): # Restore last session's controller_manager, if present self._update_cm_list() - cm_ns = instance_settings.value('cm_ns') + cm_ns = instance_settings.value("cm_ns") cm_combo = self._widget.cm_combo cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())] try: @@ -205,7 +205,7 @@ def restore_settings(self, plugin_settings, instance_settings): cm_combo.setCurrentIndex(idx) # Restore last session's controller, if running self._update_jtc_list() - jtc_name = instance_settings.value('jtc_name') + jtc_name = instance_settings.value("jtc_name") jtc_combo = self._widget.jtc_combo jtc_list = [jtc_combo.itemText(i) for i in range(jtc_combo.count())] try: @@ -217,10 +217,10 @@ def restore_settings(self, plugin_settings, instance_settings): pass # def trigger_configuration(self): - # Comment in to signal that the plugin has a way to configure - # This will enable a setting button (gear icon) in each dock widget - # title bar - # Usually used to open a modal configuration dialog + # Comment in to signal that the plugin has a way to configure + # This will enable a setting button (gear icon) in each dock widget + # title bar + # Usually used to open a modal configuration dialog def _update_cm_list(self): update_combo(self._widget.cm_combo, self._list_cm()) @@ -237,10 +237,11 @@ def _update_jtc_list(self): if running_jtc and not self._robot_joint_limits: self._robot_joint_limits = get_joint_limits(self._node) # Lazy evaluation valid_jtc = [] - if (self._robot_joint_limits): + if self._robot_joint_limits: for jtc_info in running_jtc: - has_limits = all(name in self._robot_joint_limits - for name in _jtc_joint_names(jtc_info)) + has_limits = all( + name in self._robot_joint_limits for name in _jtc_joint_names(jtc_info) + ) if has_limits: valid_jtc.append(jtc_info) valid_jtc_names = [data.name for data in valid_jtc] @@ -252,9 +253,9 @@ def _on_speed_scaling_change(self, val): self._speed_scale = val / self._speed_scaling_widget.slider.maximum() def _on_joint_state_change(self, actual_pos): - assert(len(actual_pos) == len(self._joint_pos)) + assert len(actual_pos) == len(self._joint_pos) for name in actual_pos.keys(): - self._joint_pos[name]['position'] = actual_pos[name] + self._joint_pos[name]["position"] = actual_pos[name] def _on_cm_change(self, cm_ns): self._cm_ns = cm_ns @@ -299,8 +300,9 @@ def _on_jtc_enabled(self, val): def _load_jtc(self): # Initialize joint data corresponding to selected controller running_jtc = self._running_jtc_info() - self._joint_names = next(_jtc_joint_names(x) for x in running_jtc - if x.name == self._jtc_name) + self._joint_names = next( + _jtc_joint_names(x) for x in running_jtc if x.name == self._jtc_name + ) for name in self._joint_names: self._joint_pos[name] = {} @@ -309,35 +311,33 @@ def _load_jtc(self): layout = self._widget.joint_group.layout() for name in self._joint_names: limits = self._robot_joint_limits[name] - joint_widget = DoubleEditor(limits['min_position'], - limits['max_position']) + joint_widget = DoubleEditor(limits["min_position"], limits["max_position"]) layout.addRow(name, joint_widget) # NOTE: Using partial instead of a lambda because lambdas # "will not evaluate/look up the argument values before it is # effectively called, breaking situations like using a loop # variable inside it" from functools import partial + par = partial(self._update_single_cmd_cb, name=name) joint_widget.valueChanged.connect(par) except: # TODO: Can we do better than swallow the exception? from sys import exc_info - print('Unexpected error:', exc_info()[0]) + + print("Unexpected error:", exc_info()[0]) # Enter monitor mode (sending commands disabled) self._on_jtc_enabled(False) # Setup ROS interfaces jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name) - state_topic = jtc_ns + '/state' - cmd_topic = jtc_ns + '/joint_trajectory' - self._state_sub = self._node.create_subscription(JointTrajectoryControllerState, - state_topic, - self._state_cb, - 1) - self._cmd_pub = self._node.create_publisher(JointTrajectory, - cmd_topic, - 1) + state_topic = jtc_ns + "/state" + cmd_topic = jtc_ns + "/joint_trajectory" + self._state_sub = self._node.create_subscription( + JointTrajectoryControllerState, state_topic, self._state_cb, 1 + ) + self._cmd_pub = self._node.create_publisher(JointTrajectory, cmd_topic, 1) self.jointStateChanged.connect(self._on_joint_state_change) @@ -381,10 +381,10 @@ def _running_jtc_info(self): from .utils import filter_by_type, filter_by_state controller_list = self._list_controllers() - jtc_list = filter_by_type(controller_list, - 'JointTrajectoryController', - match_substring=True) - running_jtc_list = filter_by_state(jtc_list, 'active') + jtc_list = filter_by_type( + controller_list, "JointTrajectoryController", match_substring=True + ) + running_jtc_list = filter_by_state(jtc_list, "active") return running_jtc_list def _unregister_cmd_pub(self): @@ -412,7 +412,7 @@ def _state_cb(self, msg): self.jointStateChanged.emit(actual_pos) def _update_single_cmd_cb(self, val, name): - self._joint_pos[name]['command'] = val + self._joint_pos[name]["command"] = val def _update_cmd_cb(self): dur = [] @@ -420,13 +420,13 @@ def _update_cmd_cb(self): traj.joint_names = self._joint_names point = JointTrajectoryPoint() for name in traj.joint_names: - pos = self._joint_pos[name]['position'] + pos = self._joint_pos[name]["position"] cmd = pos try: - cmd = self._joint_pos[name]['command'] + cmd = self._joint_pos[name]["command"] except (KeyError): pass - max_vel = self._robot_joint_limits[name]['max_velocity'] + max_vel = self._robot_joint_limits[name]["max_velocity"] dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur)) point.positions.append(cmd) duration = rclpy.duration.Duration(seconds=(max(dur) / self._speed_scale)) @@ -440,7 +440,7 @@ def _update_joint_widgets(self): for id in range(len(joint_widgets)): joint_name = self._joint_names[id] try: - joint_pos = self._joint_pos[joint_name]['position'] + joint_pos = self._joint_pos[joint_name]["position"] joint_widgets[id].setValue(joint_pos) except (KeyError): pass # Can happen when first connected to controller @@ -449,8 +449,7 @@ def _joint_widgets(self): # TODO: Cache instead of compute every time? widgets = [] layout = self._widget.joint_group.layout() for row_id in range(layout.rowCount()): - widgets.append(layout.itemAt(row_id, - QFormLayout.FieldRole).widget()) + widgets.append(layout.itemAt(row_id, QFormLayout.FieldRole).widget()) return widgets @@ -470,6 +469,7 @@ def _jtc_joint_names(jtc_info): def _resolve_controller_ns(cm_ns, controller_name): """ Resolve a controller's namespace from that of the controller manager. + Controllers are assumed to live one level above the controller manager, e.g. @@ -491,9 +491,9 @@ def _resolve_controller_ns(cm_ns, controller_name): @return Controller namespace @rtype str """ - assert(controller_name) - ns = cm_ns.rsplit('/', 1)[0] - if ns != '/': - ns += '/' + assert controller_name + ns = cm_ns.rsplit("/", 1)[0] + if ns != "/": + ns += "/" ns += controller_name return ns diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py index d0eb860fc5..847a9e04fb 100755 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller.py @@ -1,8 +1,22 @@ -#!/usr/bin/env python3 +#!/usr/bin/env python + +# Copyright 2022 PAL Robotics S.L. +# +# 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. import sys from rqt_gui.main import Main main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_joint_trajectory_controller')) +sys.exit(main.main(sys.argv, standalone="rqt_joint_trajectory_controller")) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py index 64e9565cd6..4702d7fbc5 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py @@ -48,9 +48,10 @@ def update_combo(combo, new_vals): def _is_permutation(a, b): - """ - @type a [] - @type b [] + """Check is arrays are permutation of each other. + + @type a [] first array with values to compare with the second one. + @type b [] second array with values to compare with the first one. @return True if C{a} is a permutation of C{b}, false otherwise @rtype bool """ diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py index a84eb2a5f8..577cc1ab07 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py @@ -23,32 +23,31 @@ from controller_manager_msgs.srv import ListControllers # Names of controller manager services, and their respective types -_LIST_CONTROLLERS_STR = 'list_controllers' -_LIST_CONTROLLERS_TYPE = 'controller_manager_msgs/srv/ListControllers' -_LIST_CONTROLLER_TYPES_STR = 'list_controller_types' -_LIST_CONTROLLER_TYPES_TYPE = 'controller_manager_msgs/srv/ListControllerTypes' -_LOAD_CONTROLLER_STR = 'load_controller' -_LOAD_CONTROLLER_TYPE = 'controller_manager_msgs/srv/LoadController' -_UNLOAD_CONTROLLER_STR = 'unload_controller' -_UNLOAD_CONTROLLER_TYPE = 'controller_manager_msgs/srv/UnloadController' -_SWITCH_CONTROLLER_STR = 'switch_controller' -_SWITCH_CONTROLLER_TYPE = 'controller_manager_msgs/srv/SwitchController' -_RELOAD_CONTROLLER_LIBS_STR = 'reload_controller_libraries' -_RELOAD_CONTROLLER_LIBS_TYPE = ('controller_manager_msgs/srv/' - 'ReloadControllerLibraries') +_LIST_CONTROLLERS_STR = "list_controllers" +_LIST_CONTROLLERS_TYPE = "controller_manager_msgs/srv/ListControllers" +_LIST_CONTROLLER_TYPES_STR = "list_controller_types" +_LIST_CONTROLLER_TYPES_TYPE = "controller_manager_msgs/srv/ListControllerTypes" +_LOAD_CONTROLLER_STR = "load_controller" +_LOAD_CONTROLLER_TYPE = "controller_manager_msgs/srv/LoadController" +_UNLOAD_CONTROLLER_STR = "unload_controller" +_UNLOAD_CONTROLLER_TYPE = "controller_manager_msgs/srv/UnloadController" +_SWITCH_CONTROLLER_STR = "switch_controller" +_SWITCH_CONTROLLER_TYPE = "controller_manager_msgs/srv/SwitchController" +_RELOAD_CONTROLLER_LIBS_STR = "reload_controller_libraries" +_RELOAD_CONTROLLER_LIBS_TYPE = "controller_manager_msgs/srv/" "ReloadControllerLibraries" # Map from service names to their respective type cm_services = { - _LIST_CONTROLLERS_STR: _LIST_CONTROLLERS_TYPE, - _LIST_CONTROLLER_TYPES_STR: _LIST_CONTROLLER_TYPES_TYPE, - _LOAD_CONTROLLER_STR: _LOAD_CONTROLLER_TYPE, - _UNLOAD_CONTROLLER_STR: _UNLOAD_CONTROLLER_TYPE, - _SWITCH_CONTROLLER_STR: _SWITCH_CONTROLLER_TYPE, - _RELOAD_CONTROLLER_LIBS_STR: _RELOAD_CONTROLLER_LIBS_TYPE + _LIST_CONTROLLERS_STR: _LIST_CONTROLLERS_TYPE, + _LIST_CONTROLLER_TYPES_STR: _LIST_CONTROLLER_TYPES_TYPE, + _LOAD_CONTROLLER_STR: _LOAD_CONTROLLER_TYPE, + _UNLOAD_CONTROLLER_STR: _UNLOAD_CONTROLLER_TYPE, + _SWITCH_CONTROLLER_STR: _SWITCH_CONTROLLER_TYPE, + _RELOAD_CONTROLLER_LIBS_STR: _RELOAD_CONTROLLER_LIBS_TYPE, } -def get_controller_managers(namespace='/', initial_guess=None): +def get_controller_managers(namespace="/", initial_guess=None): """ Get list of active controller manager namespaces. @@ -78,8 +77,7 @@ def get_controller_managers(namespace='/', initial_guess=None): # 1. Remove entries not found in current list # 2. Add new untracked controller managers ns_list[:] = [ns for ns in ns_list if ns in ns_list_curr] - ns_list += [ns for ns in ns_list_curr - if ns not in ns_list and is_controller_manager(node, ns)] + ns_list += [ns for ns in ns_list_curr if ns not in ns_list and is_controller_manager(node, ns)] return sorted(ns_list) @@ -97,8 +95,8 @@ def is_controller_manager(node, namespace): @rtype: bool """ cm_ns = namespace - if not cm_ns or cm_ns[-1] != '/': - cm_ns += '/' + if not cm_ns or cm_ns[-1] != "/": + cm_ns += "/" for srv_name in cm_services.keys(): if not _srv_exists(node, cm_ns + srv_name, cm_services[srv_name]): return False @@ -126,14 +124,14 @@ def _sloppy_get_controller_managers(node, namespace): ns_list = [] for srv_info in srv_list: - match_str = '/' + _LIST_CONTROLLERS_STR + match_str = "/" + _LIST_CONTROLLERS_STR # First element of srv_name is the service name if match_str in srv_info[0]: ns = srv_info[0].split(match_str)[0] - if ns == '': + if ns == "": # controller manager services live in root namespace # unlikely, but still possible - ns = '/' + ns = "/" ns_list.append(ns) return ns_list @@ -164,6 +162,7 @@ def _srv_exists(node, srv_name, srv_type): # ############################################################################### + class ControllerManagerLister: """ Convenience functor for querying the list of active controller managers. @@ -177,7 +176,7 @@ class ControllerManagerLister: >>> print(list_cm()) """ - def __init__(self, namespace='/'): + def __init__(self, namespace="/"): self._ns = namespace self._cm_list = [] @@ -201,14 +200,14 @@ class ControllerLister: >>> running_bar_ctrl = filter_by_type(running_ctrl, 'bar_base/bar') """ - def __init__(self, namespace='/controller_manager'): + def __init__(self, namespace="/controller_manager"): """ @param namespace Namespace of controller manager to monitor. @type namespace str """ self._node = rclpy.node.Node("controller_lister") - self._srv_name = namespace + '/' + _LIST_CONTROLLERS_STR + self._srv_name = namespace + "/" + _LIST_CONTROLLERS_STR self._srv_client = self._create_client() """ @@ -224,6 +223,7 @@ def __call__(self): def _create_client(self): return self._node.create_client(ListControllers, self._srv_name) + ############################################################################### # # Convenience methods for filtering controller state information @@ -244,7 +244,7 @@ def filter_by_name(ctrl_list, ctrl_name, match_substring=False): @return: Controllers matching the specified name @rtype: [controller_manager_msgs/ControllerState] """ - return _filter_by_attr(ctrl_list, 'name', ctrl_name, match_substring) + return _filter_by_attr(ctrl_list, "name", ctrl_name, match_substring) def filter_by_type(ctrl_list, ctrl_type, match_substring=False): @@ -260,7 +260,7 @@ def filter_by_type(ctrl_list, ctrl_type, match_substring=False): @return: Controllers matching the specified type @rtype: [controller_manager_msgs/ControllerState] """ - return _filter_by_attr(ctrl_list, 'type', ctrl_type, match_substring) + return _filter_by_attr(ctrl_list, "type", ctrl_type, match_substring) def filter_by_state(ctrl_list, ctrl_state, match_substring=False): @@ -276,12 +276,10 @@ def filter_by_state(ctrl_list, ctrl_state, match_substring=False): @return: Controllers matching the specified state @rtype: [controller_manager_msgs/ControllerState] """ - return _filter_by_attr(ctrl_list, 'state', ctrl_state, match_substring) + return _filter_by_attr(ctrl_list, "state", ctrl_state, match_substring) -def filter_by_hardware_interface(ctrl_list, - hardware_interface, - match_substring=False): +def filter_by_hardware_interface(ctrl_list, hardware_interface, match_substring=False): """ Filter controller state list by controller hardware interface. @@ -308,10 +306,7 @@ def filter_by_hardware_interface(ctrl_list, return list_out -def filter_by_resources(ctrl_list, - resources, - hardware_interface=None, - match_any=False): +def filter_by_resources(ctrl_list, resources, hardware_interface=None, match_any=False): """ Filter controller state list by claimed resources. @@ -342,8 +337,7 @@ def filter_by_resources(ctrl_list, list_out = [] for ctrl in ctrl_list: for resource_set in ctrl.claimed_resources: - if (hardware_interface is None or - hardware_interface == resource_set.hardware_interface): + if hardware_interface is None or hardware_interface == resource_set.hardware_interface: for res in resources: add_ctrl = not match_any # Initial flag value if res in resource_set.resources: @@ -372,6 +366,7 @@ def _filter_by_attr(list_in, attr_name, attr_val, match_substring=False): list_out.append(val) return list_out + ############################################################################### # # Convenience methods for finding potential controller configurations diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 5f24382cb4..f5dc707899 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -14,7 +14,6 @@ ("share/" + package_name, ["package.xml"]), ("share/" + package_name + "/resource", glob("resource/*.*")), ("share/" + package_name, ["plugin.xml"]), - ], install_requires=["setuptools"], zip_safe=True, diff --git a/tricycle_controller/config/tricycle_drive_controller.yaml b/tricycle_controller/config/tricycle_drive_controller.yaml index 4eabf615ef..c1077b13c3 100644 --- a/tricycle_controller/config/tricycle_drive_controller.yaml +++ b/tricycle_controller/config/tricycle_drive_controller.yaml @@ -1,4 +1,4 @@ -tricycle_controller: +tricycle_controller: ros__parameters: # Model traction_joint_name: traction_joint # Name of traction joint in URDF @@ -16,7 +16,7 @@ tricycle_controller: pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom - + # Rate Limiting traction: # All values should be positive # min_velocity: 0.0 diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index 2271b8c9cf..a248bbec96 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -10,8 +10,8 @@ commands for the tricycle drive base. Odometry is computed from hardware feedbac Velocity commands ----------------- -The controller works with a velocity twist from which it extracts -the x component of the linear velocity and the z component of the angular velocity. +The controller works with a velocity twist from which it extracts +the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. @@ -21,4 +21,4 @@ Other features Realtime-safe implementation. Odometry publishing Velocity, acceleration and jerk limits - Automatic stop after command timeout \ No newline at end of file + Automatic stop after command timeout diff --git a/tricycle_controller/include/tricycle_controller/steering_limiter.hpp b/tricycle_controller/include/tricycle_controller/steering_limiter.hpp index 51ecac4cbd..a16f0de0f0 100644 --- a/tricycle_controller/include/tricycle_controller/steering_limiter.hpp +++ b/tricycle_controller/include/tricycle_controller/steering_limiter.hpp @@ -23,7 +23,6 @@ namespace tricycle_controller { - class SteeringLimiter { public: @@ -37,9 +36,8 @@ class SteeringLimiter * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] */ SteeringLimiter( - double min_position = NAN, double max_position = NAN, - double min_velocity = NAN, double max_velocity = NAN, - double min_acceleration = NAN, double max_acceleration = NAN); + double min_position = NAN, double max_position = NAN, double min_velocity = NAN, + double max_velocity = NAN, double min_acceleration = NAN, double max_acceleration = NAN); /** * \brief Limit the position, velocity and acceleration @@ -78,7 +76,6 @@ class SteeringLimiter double limit_acceleration(double & p, double p0, double p1, double dt); private: - // Position limits: double min_position_; double max_position_; @@ -90,7 +87,6 @@ class SteeringLimiter // Acceleration limits: double min_acceleration_; double max_acceleration_; - }; } // namespace tricycle_controller diff --git a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp index 4c0f297ee8..ea0bb16025 100644 --- a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp +++ b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp @@ -23,7 +23,6 @@ namespace tricycle_controller { - class TractionLimiter { public: @@ -39,9 +38,8 @@ class TractionLimiter * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 */ TractionLimiter( - double min_velocity = NAN, double max_velocity = NAN, - double min_acceleration = NAN, double max_acceleration = NAN, - double min_deceleration = NAN, double max_deceleration = NAN, + double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN, + double max_acceleration = NAN, double min_deceleration = NAN, double max_deceleration = NAN, double min_jerk = NAN, double max_jerk = NAN); /** diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index c5ba1c62e3..2178008725 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include "ackermann_msgs/msg/ackermann_drive.hpp" @@ -32,16 +33,16 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "hardware_interface/handle.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "std_srvs/srv/empty.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_box.h" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "tricycle_controller/odometry.hpp" -#include "tricycle_controller/traction_limiter.hpp" #include "tricycle_controller/steering_limiter.hpp" +#include "tricycle_controller/traction_limiter.hpp" #include "tricycle_controller/visibility_control.h" namespace tricycle_controller @@ -125,7 +126,7 @@ class TricycleController : public controller_interface::ControllerInterface { bool open_loop = false; bool enable_odom_tf = false; - bool odom_only_twist = false; // for doing the pose integration in seperate node + bool odom_only_twist = false; // for doing the pose integration in separate node std::string base_frame_id = "base_link"; std::string odom_frame_id = "odom"; std::array pose_covariance_diagonal; diff --git a/tricycle_controller/src/steering_limiter.cpp b/tricycle_controller/src/steering_limiter.cpp index 6912144bd0..c1649f12dc 100644 --- a/tricycle_controller/src/steering_limiter.cpp +++ b/tricycle_controller/src/steering_limiter.cpp @@ -87,7 +87,7 @@ double SteeringLimiter::limit_velocity(double & p, double p0, double dt) const double dv_min = min_velocity_ * dt; const double dv_max = max_velocity_ * dt; - double dv = rcppmath::clamp((double)std::abs(p - p0), dv_min, dv_max); + double dv = rcppmath::clamp(std::fabs(p - p0), dv_min, dv_max); dv *= (p - p0 >= 0 ? 1 : -1); p = p0 + dv; @@ -106,7 +106,7 @@ double SteeringLimiter::limit_acceleration(double & p, double p0, double p1, dou const double da_min = min_acceleration_ * dt2; const double da_max = max_acceleration_ * dt2; - double da = rcppmath::clamp((double)std::abs(dv - dp0), da_min, da_max); + double da = rcppmath::clamp(std::fabs(dv - dp0), da_min, da_max); da *= (dv - dp0 >= 0 ? 1 : -1); p = p0 + dp0 + da; diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp index c2a0d88ab4..7865d4be71 100644 --- a/tricycle_controller/src/traction_limiter.cpp +++ b/tricycle_controller/src/traction_limiter.cpp @@ -91,7 +91,7 @@ double TractionLimiter::limit_velocity(double & v) { const double tmp = v; - v = rcppmath::clamp((double)std::abs(v), min_velocity_, max_velocity_); + v = rcppmath::clamp(std::fabs(v), min_velocity_, max_velocity_); v *= tmp >= 0 ? 1 : -1; return tmp != 0.0 ? v / tmp : 1.0; @@ -113,7 +113,7 @@ double TractionLimiter::limit_acceleration(double & v, double v0, double dt) dv_min = min_deceleration_ * dt; dv_max = max_deceleration_ * dt; } - double dv = rcppmath::clamp((double)std::abs(v - v0), dv_min, dv_max); + double dv = rcppmath::clamp(std::fabs(v - v0), dv_min, dv_max); dv *= (v - v0 >= 0 ? 1 : -1); v = v0 + dv; @@ -132,7 +132,7 @@ double TractionLimiter::limit_jerk(double & v, double v0, double v1, double dt) const double da_min = min_jerk_ * dt2; const double da_max = max_jerk_ * dt2; - double da = rcppmath::clamp((double)std::abs(dv - dv0), da_min, da_max); + double da = rcppmath::clamp(std::fabs(dv - dv0), da_min, da_max); da *= (dv - dv0 >= 0 ? 1 : -1); v = v0 + dv0 + da; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 7cec107a91..c930086701 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -151,7 +151,7 @@ controller_interface::return_type TricycleController::update( double & angular_command = command.twist.angular.z; double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians - + if (odom_params_.open_loop) { odometry_.updateOpenLoop(linear_command, angular_command, period); @@ -160,7 +160,7 @@ controller_interface::return_type TricycleController::update( { if (std::isnan(Ws_read) || std::isnan(alpha_read)) { - RCLCPP_ERROR(get_node()->get_logger(), "Could not read feeback value"); + RCLCPP_ERROR(get_node()->get_logger(), "Could not read feedback value"); return controller_interface::return_type::ERROR; } odometry_.update(Ws_read, alpha_read, period); @@ -208,7 +208,7 @@ controller_interface::return_type TricycleController::update( // Compute wheel velocity and angle auto [alpha_write, Ws_write] = twist_to_ackermann(linear_command, angular_command); - // Reduce wheel speed until the target angle has been reached + // Reduce wheel speed until the target angle has been reached double alpha_delta = abs(alpha_write - alpha_read); double scale; if (alpha_delta < M_PI / 6) @@ -221,7 +221,7 @@ controller_interface::return_type TricycleController::update( } else { - // TODO: find the best function, e.g convex power functions + // TODO(anyone): find the best function, e.g convex power functions scale = cos(alpha_delta); } Ws_write *= scale; @@ -238,8 +238,9 @@ controller_interface::return_type TricycleController::update( previous_commands_.pop(); AckermannDrive ackermann_command; - ackermann_command.speed = - Ws_write; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel speed (rad/s) + // speed in AckermannDrive is defined as desired forward speed (m/s) but it is used here as wheel + // speed (rad/s) + ackermann_command.speed = Ws_write; ackermann_command.steering_angle = alpha_write; previous_commands_.emplace(ackermann_command); @@ -247,8 +248,9 @@ controller_interface::return_type TricycleController::update( if (publish_ackermann_command_ && realtime_ackermann_command_publisher_->trylock()) { auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; - realtime_ackermann_command.speed = - Ws_write; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel speed (rad/s) + // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel + // speed (rad/s) + realtime_ackermann_command.speed = Ws_write; realtime_ackermann_command.steering_angle = alpha_write; realtime_ackermann_command_publisher_->unlockAndPublish(); } @@ -298,7 +300,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); odom_params_.odom_only_twist = get_node()->get_parameter("odom_only_twist").as_bool(); - cmd_vel_timeout_ = std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()}; + cmd_vel_timeout_ = + std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()}; publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); @@ -367,8 +370,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void - { + [this](const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { RCLCPP_WARN( @@ -390,8 +392,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { velocity_command_unstamped_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void - { + [this](const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { RCLCPP_WARN( @@ -558,8 +559,7 @@ CallbackReturn TricycleController::get_traction( // Lookup the velocity state interface const auto state_handle = std::find_if( state_interfaces_.cbegin(), state_interfaces_.cend(), - [&traction_joint_name](const auto & interface) - { + [&traction_joint_name](const auto & interface) { return interface.get_prefix_name() == traction_joint_name && interface.get_interface_name() == HW_IF_VELOCITY; }); @@ -574,8 +574,7 @@ CallbackReturn TricycleController::get_traction( // Lookup the velocity command interface const auto command_handle = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), - [&traction_joint_name](const auto & interface) - { + [&traction_joint_name](const auto & interface) { return interface.get_prefix_name() == traction_joint_name && interface.get_interface_name() == HW_IF_VELOCITY; }); @@ -600,8 +599,7 @@ CallbackReturn TricycleController::get_steering( // Lookup the velocity state interface const auto state_handle = std::find_if( state_interfaces_.cbegin(), state_interfaces_.cend(), - [&steering_joint_name](const auto & interface) - { + [&steering_joint_name](const auto & interface) { return interface.get_prefix_name() == steering_joint_name && interface.get_interface_name() == HW_IF_POSITION; }); @@ -616,8 +614,7 @@ CallbackReturn TricycleController::get_steering( // Lookup the velocity command interface const auto command_handle = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), - [&steering_joint_name](const auto & interface) - { + [&steering_joint_name](const auto & interface) { return interface.get_prefix_name() == steering_joint_name && interface.get_interface_name() == HW_IF_POSITION; }); diff --git a/tricycle_controller/test/config/test_tricycle_controller.yaml b/tricycle_controller/test/config/test_tricycle_controller.yaml index d81d722219..efecfe968f 100644 --- a/tricycle_controller/test/config/test_tricycle_controller.yaml +++ b/tricycle_controller/test/config/test_tricycle_controller.yaml @@ -17,4 +17,4 @@ test_tricycle_controller: max_deceleration: 8.0 steering: max_position: 1.57 # pi/2 - max_velocity: 1.0 \ No newline at end of file + max_velocity: 1.0 diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index 088cddbabe..d04b83ae27 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -36,8 +36,8 @@ TEST(TestLoadTricycleController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_tricycle_controller", "tricycle_controller/TricycleController")); + ASSERT_NO_THROW( + cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController")); rclcpp::shutdown(); } From cbb9feb9af95a99dd7e8366c0af0a3c067d37523 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 16 Aug 2022 07:29:55 +0100 Subject: [PATCH 019/126] Update reviewers from ros2_control repo --- .github/reviewer-lottery.yml | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml index a5a67c6b56..50c707e12b 100644 --- a/.github/reviewer-lottery.yml +++ b/.github/reviewer-lottery.yml @@ -14,19 +14,15 @@ groups: reviewers: 5 usernames: - rosterloh - - kellyprankin - progtologist - arne48 - DasRoteSkelett - - a10263790 - Serafadam - harderthan - jaron-l - malapatiravi - - erickisos - - Briancbn - - TomoyaFujita2016 - homalozoa + - erickisos - anfemosa - jackcenter - VX792 @@ -34,9 +30,7 @@ groups: - livanov93 - aprotyas - peterdavidfagan - - UsamaHamayun1 - duringhof - bijoua29 - - kasiceo - lm2292 - mcbed From 0da387d7ee533bd002ddbce5255450b4709168de Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 17 Aug 2022 11:42:00 -0600 Subject: [PATCH 020/126] Fix formatting CI job (#418) Signed-off-by: Tyler Weaver --- .../src/diff_drive_controller.cpp | 15 ++++++++++----- .../gripper_action_controller_impl.hpp | 15 ++++++--------- .../src/joint_state_broadcaster.cpp | 3 ++- .../src/tricycle_controller.cpp | 18 ++++++++++++------ 4 files changed, 30 insertions(+), 21 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 48ebdc9fb7..bfb5baf3c3 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -423,7 +423,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void { + [this](const std::shared_ptr msg) -> void + { if (!subscriber_is_active_) { RCLCPP_WARN( @@ -446,7 +447,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( velocity_command_unstamped_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void { + [this](const std::shared_ptr msg) -> void + { if (!subscriber_is_active_) { RCLCPP_WARN( @@ -595,7 +597,8 @@ controller_interface::CallbackReturn DiffDriveController::on_shutdown( void DiffDriveController::halt() { - const auto halt_wheels = [](auto & wheel_handles) { + const auto halt_wheels = [](auto & wheel_handles) + { for (const auto & wheel_handle : wheel_handles) { wheel_handle.velocity.get().set_value(0.0); @@ -625,7 +628,8 @@ controller_interface::CallbackReturn DiffDriveController::configure_side( const auto interface_name = feedback_type(); const auto state_handle = std::find_if( state_interfaces_.cbegin(), state_interfaces_.cend(), - [&wheel_name, &interface_name](const auto & interface) { + [&wheel_name, &interface_name](const auto & interface) + { return interface.get_prefix_name() == wheel_name && interface.get_interface_name() == interface_name; }); @@ -638,7 +642,8 @@ controller_interface::CallbackReturn DiffDriveController::configure_side( const auto command_handle = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), - [&wheel_name](const auto & interface) { + [&wheel_name](const auto & interface) + { return interface.get_prefix_name() == wheel_name && interface.get_interface_name() == HW_IF_VELOCITY; }); diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 0b5f791e78..ee07a9c0a0 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -238,9 +238,8 @@ controller_interface::CallbackReturn GripperActionController: { auto position_command_interface_it = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), - [](const hardware_interface::LoanedCommandInterface & command_interface) { - return command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; - }); + [](const hardware_interface::LoanedCommandInterface & command_interface) + { return command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); if (position_command_interface_it == command_interfaces_.end()) { RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position command interface"); @@ -256,9 +255,8 @@ controller_interface::CallbackReturn GripperActionController: } const auto position_state_interface_it = std::find_if( state_interfaces_.begin(), state_interfaces_.end(), - [](const hardware_interface::LoanedStateInterface & state_interface) { - return state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; - }); + [](const hardware_interface::LoanedStateInterface & state_interface) + { return state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); if (position_state_interface_it == state_interfaces_.end()) { RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position state interface"); @@ -274,9 +272,8 @@ controller_interface::CallbackReturn GripperActionController: } const auto velocity_state_interface_it = std::find_if( state_interfaces_.begin(), state_interfaces_.end(), - [](const hardware_interface::LoanedStateInterface & state_interface) { - return state_interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY; - }); + [](const hardware_interface::LoanedStateInterface & state_interface) + { return state_interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY; }); if (velocity_state_interface_it == state_interfaces_.end()) { RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 velocity state interface"); diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 65d9ec87c2..18d5f25e60 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -123,7 +123,8 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( "Publishing state interfaces defined in 'joints' and 'interfaces' parameters."); } - auto get_map_interface_parameter = [&](const std::string & interface) { + auto get_map_interface_parameter = [&](const std::string & interface) + { std::string interface_to_map = get_node() ->get_parameter(std::string("map_interface_to_joint_state.") + interface) diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index c930086701..114731242c 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -370,7 +370,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void { + [this](const std::shared_ptr msg) -> void + { if (!subscriber_is_active_) { RCLCPP_WARN( @@ -392,7 +393,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { velocity_command_unstamped_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void { + [this](const std::shared_ptr msg) -> void + { if (!subscriber_is_active_) { RCLCPP_WARN( @@ -559,7 +561,8 @@ CallbackReturn TricycleController::get_traction( // Lookup the velocity state interface const auto state_handle = std::find_if( state_interfaces_.cbegin(), state_interfaces_.cend(), - [&traction_joint_name](const auto & interface) { + [&traction_joint_name](const auto & interface) + { return interface.get_prefix_name() == traction_joint_name && interface.get_interface_name() == HW_IF_VELOCITY; }); @@ -574,7 +577,8 @@ CallbackReturn TricycleController::get_traction( // Lookup the velocity command interface const auto command_handle = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), - [&traction_joint_name](const auto & interface) { + [&traction_joint_name](const auto & interface) + { return interface.get_prefix_name() == traction_joint_name && interface.get_interface_name() == HW_IF_VELOCITY; }); @@ -599,7 +603,8 @@ CallbackReturn TricycleController::get_steering( // Lookup the velocity state interface const auto state_handle = std::find_if( state_interfaces_.cbegin(), state_interfaces_.cend(), - [&steering_joint_name](const auto & interface) { + [&steering_joint_name](const auto & interface) + { return interface.get_prefix_name() == steering_joint_name && interface.get_interface_name() == HW_IF_POSITION; }); @@ -614,7 +619,8 @@ CallbackReturn TricycleController::get_steering( // Lookup the velocity command interface const auto command_handle = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), - [&steering_joint_name](const auto & interface) { + [&steering_joint_name](const auto & interface) + { return interface.get_prefix_name() == steering_joint_name && interface.get_interface_name() == HW_IF_POSITION; }); From abd83df619ef94d33cccfcfdf0a23a7f2a44c82a Mon Sep 17 00:00:00 2001 From: Jaron <13423952+jaron-l@users.noreply.github.com> Date: Wed, 17 Aug 2022 15:42:57 -0500 Subject: [PATCH 021/126] build: :construction_worker: remove foxy rhel build (#420) - there is no foxy rhel docker image because it osrf doesn't publish rhel packages for foxy --- .github/workflows/foxy-rhel-binary-build.yml | 33 -------------------- 1 file changed, 33 deletions(-) delete mode 100644 .github/workflows/foxy-rhel-binary-build.yml diff --git a/.github/workflows/foxy-rhel-binary-build.yml b/.github/workflows/foxy-rhel-binary-build.yml deleted file mode 100644 index 8ce90c940f..0000000000 --- a/.github/workflows/foxy-rhel-binary-build.yml +++ /dev/null @@ -1,33 +0,0 @@ -name: Foxy RHEL Binary Build -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - push: - branches: - - foxy - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' - - -jobs: - foxy_rhel_binary: - name: Foxy RHEL binary build - runs-on: ubuntu-latest - env: - ROS_DISTRO: foxy - container: jaronl/ros:foxy-alma - steps: - - uses: actions/checkout@v3 - with: - path: src/ros2_controllers - - run: | - rosdep update - rosdep install -iy --from-path src/ros2_controllers - source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build - colcon test From 33ff4eadca892d5fcaab372d3ed0c7d8020303b1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 18 Aug 2022 09:20:59 +0200 Subject: [PATCH 022/126] Add all packages into ros-lint-ci (#419) --- .github/workflows/ci-ros-lint.yml | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 9902dbdaaf..d8f77a323a 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -19,6 +19,17 @@ jobs: linter: ${{ matrix.linter }} package-name: diff_drive_controller + effort_controllers + force_torque_sensor_broadcaster + forward_command_controller + joint_state_broadcaster + joint_trajectory_controller + gripper_controllers + position_controllers + ros2_controllers + ros2_controllers_test_nodes + tricycle_controller + velocity_controllers ament_lint_100: @@ -38,4 +49,14 @@ jobs: arguments: "--linelength=100 --filter=-whitespace/newline" package-name: diff_drive_controller + effort_controllers + force_torque_sensor_broadcaster + forward_command_controller + joint_state_broadcaster + joint_trajectory_controller + gripper_controllers + position_controllers ros2_controllers + ros2_controllers_test_nodes + tricycle_controller + velocity_controllers From 8c2547c8ed7e626ff5e18ecd1f079a20760b449c Mon Sep 17 00:00:00 2001 From: Jaron <13423952+jaron-l@users.noreply.github.com> Date: Thu, 18 Aug 2022 13:03:48 -0500 Subject: [PATCH 023/126] test: :white_check_mark: fix and add back joint_trajectory_controller state_topic_consistency (#415) --- .../test/test_trajectory_controller.cpp | 88 ++++++++++--------- 1 file changed, 45 insertions(+), 43 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 576f6a30d7..0d717515cf 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -356,55 +356,57 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param executor.cancel(); } -// TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) -// { -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {}, &executor); -// subscribeToState(); -// updateController(); +TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(true, {}, &executor); + subscribeToState(); + updateController(); -// // Spin to receive latest state -// executor.spin_some(); -// auto state = getState(); + // Spin to receive latest state + executor.spin_some(); + auto state = getState(); -// size_t n_joints = joint_names_.size(); + size_t n_joints = joint_names_.size(); -// for (unsigned int i = 0; i < n_joints; ++i) -// { -// EXPECT_EQ(joint_names_[i], state->joint_names[i]); -// } + for (unsigned int i = 0; i < n_joints; ++i) + { + EXPECT_EQ(joint_names_[i], state->joint_names[i]); + } -// // No trajectory by default, no desired state or error -// EXPECT_TRUE(state->desired.positions.empty()); -// EXPECT_TRUE(state->desired.velocities.empty()); -// EXPECT_TRUE(state->desired.accelerations.empty()); + // No trajectory by default, no desired state or error + EXPECT_TRUE(state->desired.positions.empty() || state->desired.positions == INITIAL_POS_JOINTS); + EXPECT_TRUE(state->desired.velocities.empty() || state->desired.velocities == INITIAL_VEL_JOINTS); + EXPECT_TRUE( + state->desired.accelerations.empty() || state->desired.accelerations == INITIAL_EFF_JOINTS); -// EXPECT_EQ(n_joints, state->actual.positions.size()); -// if ( -// std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == -// state_interface_types_.end()) -// { -// EXPECT_TRUE(state->actual.velocities.empty()); -// } -// else -// { -// EXPECT_EQ(n_joints, state->actual.velocities.size()); -// } -// if ( -// std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == -// state_interface_types_.end()) -// { -// EXPECT_TRUE(state->actual.accelerations.empty()); -// } -// else -// { -// EXPECT_EQ(n_joints, state->actual.accelerations.size()); -// } + EXPECT_EQ(n_joints, state->actual.positions.size()); + if ( + std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == + state_interface_types_.end()) + { + EXPECT_TRUE(state->actual.velocities.empty()); + } + else + { + EXPECT_EQ(n_joints, state->actual.velocities.size()); + } + if ( + std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == + state_interface_types_.end()) + { + EXPECT_TRUE(state->actual.accelerations.empty()); + } + else + { + EXPECT_EQ(n_joints, state->actual.accelerations.size()); + } -// EXPECT_TRUE(state->error.positions.empty()); -// EXPECT_TRUE(state->error.velocities.empty()); -// EXPECT_TRUE(state->error.accelerations.empty()); -// } + std::vector zeros(3, 0); + EXPECT_EQ(state->error.positions, zeros); + EXPECT_TRUE(state->error.velocities.empty() || state->error.velocities == zeros); + EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); +} // void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) // { From 9e08ee3d05ec9212349ff5b687afcd7c2add8a02 Mon Sep 17 00:00:00 2001 From: Jaron <13423952+jaron-l@users.noreply.github.com> Date: Thu, 18 Aug 2022 13:14:15 -0500 Subject: [PATCH 024/126] fix: :bug: make bare exceptions more narrow (#422) - fixes pre-commit pipeline --- .../rqt_joint_trajectory_controller/joint_limits_urdf.py | 6 +++--- .../joint_trajectory_controller.py | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py index d7517b1847..7914a76b17 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -66,7 +66,7 @@ def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=Tr name = child.getAttribute("name") try: limit = child.getElementsByTagName("limit")[0] - except: + except IndexError: continue if jtype == "continuous": minval = -pi @@ -75,11 +75,11 @@ def get_joint_limits(node, key="robot_description", use_smallest_joint_limits=Tr try: minval = float(limit.getAttribute("lower")) maxval = float(limit.getAttribute("upper")) - except: + except ValueError: continue try: maxvel = float(limit.getAttribute("velocity")) - except: + except ValueError: continue safety_tags = child.getElementsByTagName("safety_controller") if use_small and len(safety_tags) == 1: diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index ab55d6eb78..7e8ec4948e 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -321,7 +321,7 @@ def _load_jtc(self): par = partial(self._update_single_cmd_cb, name=name) joint_widget.valueChanged.connect(par) - except: + except Exception: # TODO: Can we do better than swallow the exception? from sys import exc_info @@ -350,7 +350,7 @@ def _unload_jtc(self): # Stop updating the joint positions try: self.jointStateChanged.disconnect(self._on_joint_state_change) - except: + except Exception: pass # Reset ROS interfaces From 93fa9fec93321960756429830b661ca5d0b3b013 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 21 Aug 2022 09:18:28 +0200 Subject: [PATCH 025/126] [JTC] Add additional parameter to enable configuration of interfaces for following controllers in a chain. (#380) * [JTC] Add additional parameter to enable configuration of interfaces for following controllers in a chain. * Use command_joint_states correctly in parameters depending on joint states. * Fixup tests. --- joint_trajectory_controller/doc/userdoc.rst | 27 ++++++- .../joint_trajectory_controller.hpp | 1 + .../src/joint_trajectory_controller.cpp | 32 ++++++-- .../test_load_joint_trajectory_controller.cpp | 3 +- .../test/test_trajectory_controller.cpp | 81 +++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 8 ++ 6 files changed, 143 insertions(+), 9 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index ec58a423f1..17c0c511c5 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -32,6 +32,26 @@ Other features Robust to system clock changes: Discontinuous system clock changes do not cause discontinuities in the execution of already queued trajectory segments. +ros2_control interfaces +------------------------ + +References +^^^^^^^^^^^ +(the controller is not yet implemented as chainable controller) + +States +^^^^^^^ +The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. +Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the [hardware_interface/hardware_interface_type_values.hpp](https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp). +Legal combinations of state interfaces are: +- ``position`` +- ``position`` and ``velocity`` +- ``position``, ``velocity`` and ``acceleration`` +- ``effort`` + +Commands +^^^^^^^^^ + Using Joint Trajectory Controller(s) ------------------------------------ @@ -84,8 +104,11 @@ A yaml file for using it could be: Details about parameters ^^^^^^^^^^^^^^^^^^^^^^^^ -joint (list(string)): - Joint names to control. +joints (list(string)): + Joint names to control and listen to. + +command_joints (list(string)): + Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names. command_interface (list(string)): Command interfaces provided by the hardware interface for all joints. diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 4c2f612112..79fe6bccf4 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -113,6 +113,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa protected: std::vector joint_names_; + std::vector command_joint_names_; std::vector command_interface_types_; std::vector state_interface_types_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5898a6fe95..64092e1ab5 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -56,6 +56,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() { // with the lifecycle node being initialized, we can declare parameters joint_names_ = auto_declare>("joints", joint_names_); + command_joint_names_ = + auto_declare>("command_joints", command_joint_names_); command_interface_types_ = auto_declare>("command_interfaces", command_interface_types_); state_interface_types_ = @@ -97,7 +99,7 @@ JointTrajectoryController::command_interface_configuration() const std::exit(EXIT_FAILURE); } conf.names.reserve(dof_ * command_interface_types_.size()); - for (const auto & joint_name : joint_names_) + for (const auto & joint_name : command_joint_names_) { for (const auto & interface_type : command_interface_types_) { @@ -493,6 +495,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( dof_ = joint_names_.size(); + // TODO(destogl): why is this here? Add comment or move if (!reset()) { return CallbackReturn::FAILURE; @@ -500,9 +503,25 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( if (joint_names_.empty()) { + // TODO(destogl): is this correct? Can we really move-on if no joint names are not provided? RCLCPP_WARN(logger, "'joints' parameter is empty."); } + command_joint_names_ = get_node()->get_parameter("command_joints").as_string_array(); + + if (command_joint_names_.empty()) + { + command_joint_names_ = joint_names_; + RCLCPP_INFO( + logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); + } + else if (command_joint_names_.size() != joint_names_.size()) + { + RCLCPP_ERROR( + logger, "'command_joints' parameter has to have the same size as 'joints' parameter."); + return CallbackReturn::FAILURE; + } + // Specialized, child controllers set interfaces before calling configure function. if (command_interface_types_.empty()) { @@ -592,12 +611,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // Init PID gains from ROS parameter server for (size_t i = 0; i < pids_.size(); ++i) { - const std::string prefix = "gains." + joint_names_[i]; + const std::string prefix = "gains." + command_joint_names_[i]; const auto k_p = auto_declare(prefix + ".p", 0.0); const auto k_i = auto_declare(prefix + ".i", 0.0); const auto k_d = auto_declare(prefix + ".d", 0.0); const auto i_clamp = auto_declare(prefix + ".i_clamp", 0.0); - ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + joint_names_[i], 0.0); + ff_velocity_scale_[i] = + auto_declare("ff_velocity_scale/" + command_joint_names_[i], 0.0); // Initialize PID pids_[i] = std::make_shared(k_p, k_i, k_d, i_clamp, -i_clamp); } @@ -702,7 +722,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_interface_list(command_interface_types_).c_str(), get_interface_list(state_interface_types_).c_str()); - default_tolerances_ = get_segment_tolerances(*get_node(), joint_names_); + default_tolerances_ = get_segment_tolerances(*get_node(), command_joint_names_); // Read parameters customizing controller for special cases open_loop_control_ = get_node()->get_parameter("open_loop_control").get_value(); @@ -738,7 +758,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( state_publisher_ = std::make_unique(publisher_); state_publisher_->lock(); - state_publisher_->msg_.joint_names = joint_names_; + state_publisher_->msg_.joint_names = command_joint_names_; state_publisher_->msg_.desired.positions.resize(dof_); state_publisher_->msg_.desired.velocities.resize(dof_); state_publisher_->msg_.desired.accelerations.resize(dof_); @@ -797,7 +817,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); auto index = std::distance(allowed_interface_types_.begin(), it); if (!controller_interface::get_ordered_interfaces( - command_interfaces_, joint_names_, interface, joint_command_interface_[index])) + command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp index e0dfdc6353..0f94d0b16c 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include "gmock/gmock.h" + #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" #include "rclcpp/executor.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0d717515cf..cc5b175281 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -95,6 +95,87 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) executor.cancel(); } +TEST_P(TrajectoryControllerTestParameterized, check_interface_names) +{ + SetUpTrajectoryController(); + + const auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + std::vector state_interface_names; + state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); + for (const auto & joint : joint_names_) + { + for (const auto & interface : state_interface_types_) + { + state_interface_names.push_back(joint + "/" + interface); + } + } + auto state_interfaces = traj_controller_->state_interface_configuration(); + EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); + ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); + + std::vector command_interface_names; + command_interface_names.reserve(joint_names_.size() * command_interface_types_.size()); + for (const auto & joint : joint_names_) + { + for (const auto & interface : command_interface_types_) + { + command_interface_names.push_back(joint + "/" + interface); + } + } + auto command_interfaces = traj_controller_->command_interface_configuration(); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ(command_interfaces.names.size(), joint_names_.size() * command_interface_types_.size()); + ASSERT_THAT( + command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); +} + +TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints) +{ + SetUpTrajectoryController(); + + // set command_joints parameter + const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_); + traj_controller_->get_node()->set_parameter({command_joint_names_param}); + + const auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + std::vector state_interface_names; + state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); + for (const auto & joint : joint_names_) + { + for (const auto & interface : state_interface_types_) + { + state_interface_names.push_back(joint + "/" + interface); + } + } + auto state_interfaces = traj_controller_->state_interface_configuration(); + EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); + ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); + + std::vector command_interface_names; + command_interface_names.reserve(command_joint_names_.size() * command_interface_types_.size()); + for (const auto & joint : command_joint_names_) + { + for (const auto & interface : command_interface_types_) + { + command_interface_names.push_back(joint + "/" + interface); + } + } + auto command_interfaces = traj_controller_->command_interface_configuration(); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + command_interfaces.names.size(), command_joint_names_.size() * command_interface_types_.size()); + ASSERT_THAT( + command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); +} + TEST_P(TrajectoryControllerTestParameterized, activate) { SetUpTrajectoryController(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index f12bd8b8bc..be11d274ad 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -81,6 +81,11 @@ class TestableJointTrajectoryController void set_joint_names(const std::vector & joint_names) { joint_names_ = joint_names; } + void set_command_joint_names(const std::vector & command_joint_names) + { + command_joint_names_ = command_joint_names; + } + void set_command_interfaces(const std::vector & command_interfaces) { command_interface_types_ = command_interfaces; @@ -115,6 +120,8 @@ class TrajectoryControllerTest : public ::testing::Test controller_name_ = "test_joint_trajectory_controller"; joint_names_ = {"joint1", "joint2", "joint3"}; + command_joint_names_ = { + "following_controller/joint1", "following_controller/joint2", "following_controller/joint3"}; joint_pos_.resize(joint_names_.size(), 0.0); joint_state_pos_.resize(joint_names_.size(), 0.0); joint_vel_.resize(joint_names_.size(), 0.0); @@ -396,6 +403,7 @@ class TrajectoryControllerTest : public ::testing::Test std::string controller_name_; std::vector joint_names_; + std::vector command_joint_names_; std::vector command_interface_types_; std::vector state_interface_types_; From 5e6ef29bc4c42461b435e8a168cc4081f7db354e Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sat, 27 Aug 2022 10:53:44 -0600 Subject: [PATCH 026/126] Generate params for ForceTorqueSensorBroadcaster (#395) Signed-off-by: Tyler Weaver --- .../CMakeLists.txt | 8 ++++ .../force_torque_sensor_broadcaster.hpp | 6 +-- force_torque_sensor_broadcaster/package.xml | 2 + .../src/force_torque_sensor_broadcaster.cpp | 41 +++++++---------- ..._torque_sensor_broadcaster_parameters.yaml | 44 +++++++++++++++++++ ros2_controllers.rolling.repos | 4 ++ 6 files changed, 77 insertions(+), 28 deletions(-) create mode 100644 force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index a710586700..cdb1fe2b35 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -26,6 +26,11 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +find_package(generate_parameter_library REQUIRED) +generate_parameter_library(force_torque_sensor_broadcaster_parameters + src/force_torque_sensor_broadcaster_parameters.yaml +) + add_library( ${PROJECT_NAME} SHARED @@ -38,6 +43,9 @@ ament_target_dependencies( ${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} ) +target_link_libraries(force_torque_sensor_broadcaster + force_torque_sensor_broadcaster_parameters +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "FORCE_TORQUE_SENSOR_BROADCASTER_BUILDING_DLL") diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 023c59cae0..6166adfd9a 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -25,6 +25,7 @@ #include "controller_interface/controller_interface.hpp" #include "force_torque_sensor_broadcaster/visibility_control.h" +#include "force_torque_sensor_broadcaster_parameters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -64,9 +65,8 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: - std::string sensor_name_; - std::array interface_names_; - std::string frame_id_; + std::shared_ptr param_listener_; + Params params_; std::unique_ptr force_torque_sensor_; diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 868d83fdea..de4a0e0513 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -11,6 +11,8 @@ ament_cmake + generate_parameter_library + controller_interface geometry_msgs hardware_interface diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index e88ef51cd2..9687f9dfee 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -32,14 +32,8 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_init() { try { - auto_declare("sensor_name", ""); - auto_declare("interface_names.force.x", ""); - auto_declare("interface_names.force.y", ""); - auto_declare("interface_names.force.z", ""); - auto_declare("interface_names.torque.x", ""); - auto_declare("interface_names.torque.y", ""); - auto_declare("interface_names.torque.z", ""); - auto_declare("frame_id", ""); + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -53,18 +47,14 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_init() controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - sensor_name_ = get_node()->get_parameter("sensor_name").as_string(); - interface_names_[0] = get_node()->get_parameter("interface_names.force.x").as_string(); - interface_names_[1] = get_node()->get_parameter("interface_names.force.y").as_string(); - interface_names_[2] = get_node()->get_parameter("interface_names.force.z").as_string(); - interface_names_[3] = get_node()->get_parameter("interface_names.torque.x").as_string(); - interface_names_[4] = get_node()->get_parameter("interface_names.torque.y").as_string(); - interface_names_[5] = get_node()->get_parameter("interface_names.torque.z").as_string(); + params_ = param_listener_->get_params(); const bool no_interface_names_defined = - std::count(interface_names_.begin(), interface_names_.end(), "") == 6; + params_.interface_names.force.x.empty() && params_.interface_names.force.y.empty() && + params_.interface_names.force.z.empty() && params_.interface_names.torque.x.empty() && + params_.interface_names.torque.y.empty() && params_.interface_names.torque.z.empty(); - if (sensor_name_.empty() && no_interface_names_defined) + if (params_.sensor_name.empty() && no_interface_names_defined) { RCLCPP_ERROR( get_node()->get_logger(), @@ -73,7 +63,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( return controller_interface::CallbackReturn::ERROR; } - if (!sensor_name_.empty() && !no_interface_names_defined) + if (!params_.sensor_name.empty() && !no_interface_names_defined) { RCLCPP_ERROR( get_node()->get_logger(), @@ -82,24 +72,25 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( return controller_interface::CallbackReturn::ERROR; } - frame_id_ = get_node()->get_parameter("frame_id").as_string(); - if (frame_id_.empty()) + if (params_.frame_id.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); return controller_interface::CallbackReturn::ERROR; } - if (!sensor_name_.empty()) + if (!params_.sensor_name.empty()) { force_torque_sensor_ = std::make_unique( - semantic_components::ForceTorqueSensor(sensor_name_)); + semantic_components::ForceTorqueSensor(params_.sensor_name)); } else { + auto const & force_names = params_.interface_names.force; + auto const & torque_names = params_.interface_names.torque; force_torque_sensor_ = std::make_unique( semantic_components::ForceTorqueSensor( - interface_names_[0], interface_names_[1], interface_names_[2], interface_names_[3], - interface_names_[4], interface_names_[5])); + force_names.x, force_names.y, force_names.z, torque_names.x, torque_names.y, + torque_names.z)); } try @@ -118,7 +109,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( } realtime_publisher_->lock(); - realtime_publisher_->msg_.header.frame_id = frame_id_; + realtime_publisher_->msg_.header.frame_id = params_.frame_id; realtime_publisher_->unlock(); RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml new file mode 100644 index 0000000000..059d5ab9a1 --- /dev/null +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -0,0 +1,44 @@ +force_torque_sensor_broadcaster: + frame_id: { + type: string, + default_value: "", + description: "Sensor's frame_id in which values are published.", + } + sensor_name: { + type: string, + default_value: "", + description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined.", + } + interface_names: + force: + x: { + type: string, + default_value: "", + description: "Name of the state interface with force values on 'x' axis.", + } + y: { + type: string, + default_value: "", + description: "Name of the state interface with force values on 'y' axis.", + } + z: { + type: string, + default_value: "", + description: "Name of the state interface with force values on 'z' axis.", + } + torque: + x: { + type: string, + default_value: "", + description: "Name of the state interface with torque values around 'x' axis.", + } + y: { + type: string, + default_value: "", + description: "Name of the state interface with torque values around 'y' axis.", + } + z: { + type: string, + default_value: "", + description: "Name of the state interface with torque values around 'z' axis.", + } diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 3a667e7d9a..6f9dc84a3f 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -11,3 +11,7 @@ repositories: type: git url: https://github.com/ros-controls/realtime_tools.git version: master + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main From fe89a549ac81d7baee4475a707ab3b0c869e1838 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 1 Sep 2022 01:39:46 -0500 Subject: [PATCH 027/126] Use a "steady clock" when measuring time differences (#427) * Use "steady" clocks when measuring changes in time * Check if the "publish rate" tests pass now --- .../test/test_trajectory.cpp | 7 +- .../test/test_trajectory_actions.cpp | 7 +- .../test/test_trajectory_controller.cpp | 77 +++++++++---------- .../test/test_trajectory_controller_utils.hpp | 7 +- 4 files changed, 50 insertions(+), 48 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index 93e3560c07..fbf198300b 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -40,6 +40,7 @@ const double EPS = 1e-8; TEST(TestTrajectory, initialize_trajectory) { + auto clock = rclcpp::Clock(RCL_STEADY_TIME); { auto empty_msg = std::make_shared(); empty_msg->header.stamp.sec = 2; @@ -49,7 +50,7 @@ TEST(TestTrajectory, initialize_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; - traj.sample(rclcpp::Clock().now(), DEFAULT_INTERPOLATION, expected_point, start, end); + traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -59,13 +60,13 @@ TEST(TestTrajectory, initialize_trajectory) auto empty_msg = std::make_shared(); empty_msg->header.stamp.sec = 0; empty_msg->header.stamp.nanosec = 0; - const auto now = rclcpp::Clock().now(); + const auto now = clock.now(); auto traj = joint_trajectory_controller::Trajectory(empty_msg); const auto traj_starttime = traj.time_from_start(); trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; - traj.sample(rclcpp::Clock().now(), DEFAULT_INTERPOLATION, expected_point, start, end); + traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 7128c0bee2..10dd3a6b90 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -91,12 +91,13 @@ class TestTrajectoryActions : public TrajectoryControllerTest [&]() { // controller hardware cycle update loop - auto start_time = rclcpp::Clock().now(); + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + auto start_time = clock.now(); rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0); auto end_time = start_time + wait; - while (rclcpp::Clock().now() < end_time) + while (clock.now() < end_time) { - traj_controller_->update(rclcpp::Clock().now(), rclcpp::Clock().now() - start_time); + traj_controller_->update(clock.now(), clock.now() - start_time); } }); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index cc5b175281..e758dc038f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -343,7 +343,6 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) state = traj_controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // update for 0.25 seconds - const auto start_time = rclcpp::Clock().now(); updateController(rclcpp::Duration::from_seconds(0.25)); // should be home pose again @@ -489,52 +488,52 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); } -// void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) -// { -// rclcpp::Parameter state_publish_rate_param( -// "state_publish_rate", static_cast(target_msg_count)); -// rclcpp::executors::SingleThreadedExecutor executor; -// SetUpAndActivateTrajectoryController(true, {state_publish_rate_param}, &executor); +void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) +{ + rclcpp::Parameter state_publish_rate_param( + "state_publish_rate", static_cast(target_msg_count)); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(true, {state_publish_rate_param}, &executor); -// auto future_handle = std::async( -// std::launch::async, [&executor]() -> void { executor.spin(); }); + auto future_handle = std::async(std::launch::async, [&executor]() -> void { executor.spin(); }); -// using control_msgs::msg::JointTrajectoryControllerState; + using control_msgs::msg::JointTrajectoryControllerState; -// const int qos_level = 10; -// int echo_received_counter = 0; -// rclcpp::Subscription::SharedPtr subs = -// traj_controller_->get_node()->create_subscription( -// controller_name_ + "/state", qos_level, -// [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); + const int qos_level = 10; + int echo_received_counter = 0; + rclcpp::Subscription::SharedPtr subs = + traj_controller_->get_node()->create_subscription( + controller_name_ + "/state", qos_level, + [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); -// // update for 1second -// const auto start_time = rclcpp::Clock().now(); -// const rclcpp::Duration wait = rclcpp::Duration::from_seconds(1.0); -// const auto end_time = start_time + wait; -// while (rclcpp::Clock().now() < end_time) -// { -// traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// } + // update for 1second + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + const auto start_time = clock.now(); + const rclcpp::Duration wait = rclcpp::Duration::from_seconds(1.0); + const auto end_time = start_time + wait; + while (clock.now() < end_time) + { + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + } -// // We may miss the last message since time allowed is exactly the time needed -// EXPECT_NEAR(target_msg_count, echo_received_counter, 1); + // We may miss the last message since time allowed is exactly the time needed + EXPECT_NEAR(target_msg_count, echo_received_counter, 1); -// executor.cancel(); -// } + executor.cancel(); +} -// /** -// * @brief test_state_publish_rate Test that state publish rate matches configure rate -// */ -// TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) -// { -// test_state_publish_rate_target(10); -// } +/** + * @brief test_state_publish_rate Test that state publish rate matches configure rate + */ +TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) +{ + test_state_publish_rate_target(10); +} -// TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) -// { -// test_state_publish_rate_target(0); -// } +TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) +{ + test_state_publish_rate_target(0); +} // /** // * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index be11d274ad..840a624eca 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -349,11 +349,12 @@ class TrajectoryControllerTest : public ::testing::Test void updateController(rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2)) { - const auto start_time = rclcpp::Clock().now(); + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + const auto start_time = clock.now(); const auto end_time = start_time + wait_time; - while (rclcpp::Clock().now() < end_time) + while (clock.now() < end_time) { - traj_controller_->update(rclcpp::Clock().now(), rclcpp::Clock().now() - start_time); + traj_controller_->update(clock.now(), clock.now() - start_time); } } From fe306da25f29cc44e3b1301b1301495dede8e058 Mon Sep 17 00:00:00 2001 From: Shota Aoki Date: Thu, 1 Sep 2022 18:07:44 +0900 Subject: [PATCH 028/126] Add an initialization of the gripper action command for safe startup. (#425) --- .../gripper_controllers/gripper_action_controller_impl.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index ee07a9c0a0..6fb9d61359 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -298,6 +298,7 @@ controller_interface::CallbackReturn GripperActionController: // Command - non RT version command_struct_.position_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = default_max_effort_; + command_.initRT(command_struct_); // Result pre_alloc_result_ = std::make_shared(); From dddab1dc7f63b028e83a4c99b55b0b36e647c8ef Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 1 Sep 2022 10:27:29 +0100 Subject: [PATCH 029/126] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 5 +++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 6 ++++++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 12 ++++++++++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 6 ++++++ tricycle_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 3 +++ 14 files changed, 66 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 5c40544d25..b2ac2dc2e3 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix formatting CI job (`#418 `_) +* Contributors: Tyler Weaver + 2.11.0 (2022-08-04) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 3d2babf2ed..4d1a88acf5 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-08-04) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index a3d5156859..c9f68ec19d 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Generate params for ForceTorqueSensorBroadcaster (`#395 `_) +* Contributors: Tyler Weaver + 2.11.0 (2022-08-04) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 9b1d48c875..d046dc0b5a 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-08-04) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index c06969c439..ccb4d7cefb 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add an initialization of the gripper action command for safe startup. (`#425 `_) +* Fix formatting CI job (`#418 `_) +* Contributors: Shota Aoki, Tyler Weaver + 2.11.0 (2022-08-04) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 82c99bf60f..960f7a41d0 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-08-04) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 3030632664..19fb75ae94 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix formatting CI job (`#418 `_) +* Contributors: Tyler Weaver + 2.11.0 (2022-08-04) ------------------- * Use explicit type in joint_state_broadcaster test (`#403 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index d435127b2f..a17cceb8ef 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use a "steady clock" when measuring time differences (`#427 `_) +* [JTC] Add additional parameter to enable configuration of interfaces for following controllers in a chain. (`#380 `_) +* test: :white_check_mark: fix and add back joint_trajectory_controller state_topic_consistency (`#415 `_) +* Reinstate JTC tests (`#391 `_) +* [JTC] Hold position if tolerance is violated even during non-active goal (`#368 `_) +* Small fixes for JTC. (`#390 `_) + variables in JTC to not clutter other PR with them. + fixes of updating parameters on renewed configuration of JTC that were missed +* Contributors: Andy Zelenak, Bence Magyar, Denis Štogl, Jaron Lundwall, Michael Wiznitzer + 2.11.0 (2022-08-04) ------------------- diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 1affcdc4b8..14caad1b0a 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-08-04) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index e53cadd749..ad8bd8a3ca 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-08-04) ------------------- * Tricycle controller (`#345 `_) diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 7d319f9034..284e045a54 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-08-04) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 27bc6030cb..493ec46581 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix: :bug: make bare exceptions more narrow (`#422 `_) +* Fix formatting because pre-commit was not running on CI for some time. (`#409 `_) +* Contributors: Denis Štogl, Jaron Lundwall + 2.11.0 (2022-08-04) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 7c929f4432..a15927cd1f 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix formatting CI job (`#418 `_) +* Fix formatting because pre-commit was not running on CI for some time. (`#409 `_) +* Contributors: Denis Štogl, Tyler Weaver + 2.11.0 (2022-08-04) ------------------- * Tricycle controller (`#345 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index b9de02a603..cdbde1fb06 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.11.0 (2022-08-04) ------------------- From 3425ebcd1293438c40596c1181fb2a5dbf815078 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 1 Sep 2022 10:32:15 +0100 Subject: [PATCH 030/126] 2.12.0 --- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 30 files changed, 44 insertions(+), 44 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index b2ac2dc2e3..b575411966 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- * Fix formatting CI job (`#418 `_) * Contributors: Tyler Weaver diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index d82a4a2df8..1d17d25b58 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.11.0 + 2.12.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 4d1a88acf5..6eca17389b 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- 2.11.0 (2022-08-04) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index e716631d9e..692f1928f3 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.11.0 + 2.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index c9f68ec19d..f2ce7457eb 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- * Generate params for ForceTorqueSensorBroadcaster (`#395 `_) * Contributors: Tyler Weaver diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index de4a0e0513..11bb64374d 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.11.0 + 2.12.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index d046dc0b5a..5755b526e2 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- 2.11.0 (2022-08-04) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 071701c5cd..73943f9d75 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.11.0 + 2.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index ccb4d7cefb..bbf0f65e73 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- * Add an initialization of the gripper action command for safe startup. (`#425 `_) * Fix formatting CI job (`#418 `_) * Contributors: Shota Aoki, Tyler Weaver diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index a68306ee4e..6403d5efa7 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.11.0 + 2.12.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 960f7a41d0..ce5149b6c7 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- 2.11.0 (2022-08-04) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index dec5c6b5e4..b6a28e765c 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.11.0 + 2.12.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 19fb75ae94..8fc784711b 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- * Fix formatting CI job (`#418 `_) * Contributors: Tyler Weaver diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 12db7f0b2d..9262419ec4 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.11.0 + 2.12.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index a17cceb8ef..967b3f5cee 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- * Use a "steady clock" when measuring time differences (`#427 `_) * [JTC] Add additional parameter to enable configuration of interfaces for following controllers in a chain. (`#380 `_) * test: :white_check_mark: fix and add back joint_trajectory_controller state_topic_consistency (`#415 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 00e6e9f6cb..158b399e09 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.11.0 + 2.12.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 14caad1b0a..9bdb24a322 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- 2.11.0 (2022-08-04) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 3b7eb27975..2a4a3811d2 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.11.0 + 2.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index ad8bd8a3ca..4bf2fb4664 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- 2.11.0 (2022-08-04) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 2298913120..e0436c2094 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.11.0 + 2.12.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 284e045a54..baa90562dc 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- 2.11.0 (2022-08-04) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index a57a484e88..c714c93d1f 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.11.0 + 2.12.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 959ad0eb4d..3c3f49a876 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.11.0", + version="2.12.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 493ec46581..2da4d40515 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- * fix: :bug: make bare exceptions more narrow (`#422 `_) * Fix formatting because pre-commit was not running on CI for some time. (`#409 `_) * Contributors: Denis Štogl, Jaron Lundwall diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 70b8201345..48fe677f28 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.11.0 + 2.12.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index f5dc707899..40cc4abdd0 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.11.0", + version="2.12.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index a15927cd1f..3875d2e729 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- * Fix formatting CI job (`#418 `_) * Fix formatting because pre-commit was not running on CI for some time. (`#409 `_) * Contributors: Denis Štogl, Tyler Weaver diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 409ee6bc40..aa36f2822e 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.11.0 + 2.12.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index cdbde1fb06..49ffeaee5b 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.12.0 (2022-09-01) +------------------- 2.11.0 (2022-08-04) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index b1d500f1a1..a2cfa040bb 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.11.0 + 2.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From b31635a514d75a3ded928479b1f60781ed3b7be3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 5 Sep 2022 18:33:35 +0200 Subject: [PATCH 031/126] Enable definition of all fields in JointTrajectory message when using test node. (#389) Co-authored-by: Bence Magyar --- .../publisher_joint_trajectory_controller.py | 103 +++++++++++++++--- 1 file changed, 86 insertions(+), 17 deletions(-) diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py index 77e04da356..301fe42cd4 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py @@ -30,9 +30,8 @@ def __init__(self): self.declare_parameter("controller_name", "position_trajectory_controller") self.declare_parameter("wait_sec_between_publish", 6) self.declare_parameter("goal_names", ["pos1", "pos2"]) - self.declare_parameter("joints") + self.declare_parameter("joints", [""]) self.declare_parameter("check_starting_point", False) - self.declare_parameter("starting_point_limits") # Read parameters controller_name = self.get_parameter("controller_name").value @@ -65,24 +64,96 @@ def __init__(self): self.joint_state_msg_received = False # Read all positions from parameters - self.goals = [] + self.goals = [] # List of JointTrajectoryPoint for name in goal_names: self.declare_parameter(name) goal = self.get_parameter(name).value - if goal is None or len(goal) == 0: - raise Exception(f'Values for goal "{name}" not set!') - float_goal = [] - for value in goal: - float_goal.append(float(value)) - self.goals.append(float_goal) + # TODO(anyone): remove this "if" part in ROS Iron + if isinstance(goal, list): + self.get_logger().warn( + f'Goal "{name}" is defined as a list. This is deprecated. ' + "Use the following structure:\n:\n " + "positions: [joint1, joint2, joint3, ...]\n " + "velocities: [v_joint1, v_joint2, ...]\n " + "accelerations: [a_joint1, a_joint2, ...]\n " + "effort: [eff_joint1, eff_joint2, ...]" + ) + + if goal is None or len(goal) == 0: + raise Exception(f'Values for goal "{name}" not set!') + + float_goal = [float(value) for value in goal] + + point = JointTrajectoryPoint() + point.positions = float_goal + point.time_from_start = Duration(sec=4) + + self.goals.append(point) + + else: + point = JointTrajectoryPoint() + + def get_sub_param(sub_param): + param_name = name + "." + sub_param + self.declare_parameter(param_name, [float()]) + param_value = self.get_parameter(param_name).value + + float_values = [] + + if len(param_value) != len(self.joints): + return [False, float_values] + + float_values = [float(value) for value in param_value] + + return [True, float_values] + + one_ok = False + + [ok, values] = get_sub_param("positions") + if ok: + point.positions = values + one_ok = True + + [ok, values] = get_sub_param("velocities") + if ok: + point.velocities = values + one_ok = True + + [ok, values] = get_sub_param("accelerations") + if ok: + point.accelerations = values + one_ok = True + + [ok, values] = get_sub_param("effort") + if ok: + point.effort = values + one_ok = True + + if one_ok: + point.time_from_start = Duration(sec=4) + self.goals.append(point) + self.get_logger().info(f'Goal "{name}" has definition {point}') + + else: + self.get_logger().warn( + f'Goal "{name}" definition is wrong. This goal will not be used. ' + "Use the following structure: \n:\n " + "positions: [joint1, joint2, joint3, ...]\n " + "velocities: [v_joint1, v_joint2, ...]\n " + "accelerations: [a_joint1, a_joint2, ...]\n " + "effort: [eff_joint1, eff_joint2, ...]" + ) + + if len(self.goals) < 1: + self.get_logger().error("No valid goal found. Exiting...") + exit(1) publish_topic = "/" + controller_name + "/" + "joint_trajectory" self.get_logger().info( - 'Publishing {} goals on topic "{}" every {} s'.format( - len(goal_names), publish_topic, wait_sec_between_publish - ) + f'Publishing {len(goal_names)} goals on topic "{publish_topic}" every ' + "{wait_sec_between_publish} s" ) self.publisher_ = self.create_publisher(JointTrajectory, publish_topic, 1) @@ -93,13 +164,11 @@ def timer_callback(self): if self.starting_point_ok: + self.get_logger().info(f"Sending goal {self.goals[self.i]}.") + traj = JointTrajectory() traj.joint_names = self.joints - point = JointTrajectoryPoint() - point.positions = self.goals[self.i] - point.time_from_start = Duration(sec=4) - - traj.points.append(point) + traj.points.append(self.goals[self.i]) self.publisher_.publish(traj) self.i += 1 From 0466d1d2105d182f400af7db2dd5e2c64af7318a Mon Sep 17 00:00:00 2001 From: Gilmar Correia Date: Tue, 6 Sep 2022 04:26:16 -0300 Subject: [PATCH 032/126] [JointStateBroadcaster] Reset internal variables to avoid duplication of joints (#431) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- joint_state_broadcaster/src/joint_state_broadcaster.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 18d5f25e60..c46d1fcd7a 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -207,6 +207,8 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_activate( controller_interface::CallbackReturn JointStateBroadcaster::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { + joint_names_.clear(); + return CallbackReturn::SUCCESS; } @@ -229,6 +231,7 @@ bool has_any_key( bool JointStateBroadcaster::init_joint_data() { + joint_names_.clear(); if (state_interfaces_.empty()) { return false; From a9b9a9ca70e396537d8976f9944ec23311143aa2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 7 Sep 2022 19:38:42 +0200 Subject: [PATCH 033/126] Fix source builds (#430) --- .github/workflows/humble-source-build.yml | 2 +- .github/workflows/rolling-source-build.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index 6ab92814dc..18c33b6d52 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -16,4 +16,4 @@ jobs: with: ros_distro: humble ref: master - ros2_repo_branch: master-humble + ros2_repo_branch: humble diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index 1a137a795b..40abcd1b0c 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -16,4 +16,4 @@ jobs: with: ros_distro: rolling ref: master - ros2_repo_branch: master-rolling + ros2_repo_branch: rolling From 96f1e597dc3ba25266270a1fc4f4cbbf0d187e47 Mon Sep 17 00:00:00 2001 From: Arshad Mehmood Date: Thu, 8 Sep 2022 01:44:49 +0800 Subject: [PATCH 034/126] Fix for high CPU usage by JTC in gzserver (#428) * Change type cast wall timer period from second to nanoseconds. create_wall_timer() expects delay in nanoseconds (duration object) however the type cast to seconds will result in 0 (if duration is less than 1s) and thus causing timer to be fired non stop resulting in very high CPU usage. * Reset smartpointer so that create_wall_timer() call can destroy previous trajectory timer. node->create_wall_timer() first removes timers associated with expired smartpointers before servicing current request. The JTC timer pointer gets overwrite only after the create_wall_timer() returns and thus not able to remove previous trajectory timer resulting in upto two timers running for JTC during trajectory execution. Althougth the previous timer does nothing but still get fired. Signed-off-by: Arshad Mehmood Signed-off-by: Arshad Mehmood --- .../src/joint_trajectory_controller.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 64092e1ab5..f5f5190caa 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1087,9 +1087,12 @@ void JointTrajectoryController::goal_accepted_callback( rt_goal->execute(); rt_active_goal_.writeFromNonRT(rt_goal); + // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list + goal_handle_timer_.reset(); + // Setup goal status checking timer goal_handle_timer_ = get_node()->create_wall_timer( - action_monitor_period_.to_chrono(), + action_monitor_period_.to_chrono(), std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); } From 7b448c95a06d543f6e3cf80a9deddf7f3d2a5aaa Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Sat, 17 Sep 2022 14:23:58 +0800 Subject: [PATCH 035/126] Fix rates in JTC userdoc.rst (#433) --- joint_trajectory_controller/doc/userdoc.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 17c0c511c5..d225624096 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -121,16 +121,16 @@ state_interfaces (list(string)): Values: position (mandatory) [velocity, [acceleration]]. Acceleration interface can only be used in combination with position and velocity. - Default: 50.0 - state_publish_rate (double): Publish-rate of the controller's "state" topic. - Default: 20.0 + Default: 50.0 action_monitor_rate (double): Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). + Default: 20.0 + allow_partial_joints_goal (boolean): Allow joint goals defining trajectory for only some joints. From 9c81373c5ade476b854fdf971b3007939156c9f2 Mon Sep 17 00:00:00 2001 From: Jaron <13423952+jaron-l@users.noreply.github.com> Date: Wed, 28 Sep 2022 04:24:22 -0500 Subject: [PATCH 036/126] ci: :construction_worker: update rhel container (#439) --- .github/workflows/galactic-rhel-binary-build.yml | 2 +- .github/workflows/humble-rhel-binary-build.yml | 2 +- .github/workflows/rolling-rhel-binary-build.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/galactic-rhel-binary-build.yml b/.github/workflows/galactic-rhel-binary-build.yml index 2116d5f4d5..5257ec0ee2 100644 --- a/.github/workflows/galactic-rhel-binary-build.yml +++ b/.github/workflows/galactic-rhel-binary-build.yml @@ -20,7 +20,7 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: galactic - container: jaronl/ros:galactic-alma + container: ghcr.io/ros-controls/ros:${{ env.ROS_DISTRO }}-rhel steps: - uses: actions/checkout@v3 with: diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index 42a700a7db..efa5ebbbb9 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -20,7 +20,7 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: humble - container: jaronl/ros:humble-alma + container: ghcr.io/ros-controls/ros:${{ env.ROS_DISTRO }}-rhel steps: - uses: actions/checkout@v3 with: diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index e85b439411..42adab88c7 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -20,7 +20,7 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: rolling - container: jaronl/ros:rolling-alma + container: ghcr.io/ros-controls/ros:${{ env.ROS_DISTRO }}-rhel steps: - uses: actions/checkout@v3 with: From 77a2c0658d89267eb7ee39a8aeb70d9bc39d89c6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 28 Sep 2022 20:54:28 +0200 Subject: [PATCH 037/126] Fix undeclared and wrong parameters in controllers. (#438) * Add missing parameter declaration in the joint state broadcaster. * Fix unsensible test in IMU Sensor Broadcaster. --- .../test/test_imu_sensor_broadcaster.cpp | 15 ++++++--------- .../src/joint_state_broadcaster.cpp | 1 + 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 42b53bf975..cc1398a9b8 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -102,7 +102,7 @@ void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & ASSERT_TRUE(subscription->take(imu_msg, msg_info)); } -TEST_F(IMUSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) +TEST_F(IMUSensorBroadcasterTest, SensorName_FrameId_NotSet) { SetUpIMUBroadcaster(); @@ -110,21 +110,18 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); } -TEST_F(IMUSensorBroadcasterTest, SensorName_FrameId_NotSet) +TEST_F(IMUSensorBroadcasterTest, SensorName_NotSet) { SetUpIMUBroadcaster(); - // set the 'interface_names' - imu_broadcaster_->get_node()->set_parameter( - {"interface_names.angular_velocity.x", "imu_sensor/angular_velocity.x"}); - imu_broadcaster_->get_node()->set_parameter( - {"interface_names.linear_acceleration.z", "imu_sensor/linear_acceleration.z"}); + // set the 'frame_id' + imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); - // configure failed, 'frame_id' parameter not set + // configure failed, 'sensor_name' parameter not set ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); } -TEST_F(IMUSensorBroadcasterTest, InterfaceNames_FrameId_NotSet) +TEST_F(IMUSensorBroadcasterTest, FrameId_NotSet) { SetUpIMUBroadcaster(); diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index c46d1fcd7a..69f5441a6d 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -53,6 +53,7 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_init() auto_declare("use_local_topics", false); auto_declare>("joints", std::vector({})); auto_declare>("interfaces", std::vector({})); + auto_declare>("extra_joints", std::vector({})); auto_declare( std::string("map_interface_to_joint_state.") + HW_IF_POSITION, HW_IF_POSITION); auto_declare( From 8d0345a37ee3cc8144a793cc7b464f42e4badf93 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 1 Oct 2022 12:44:03 +0200 Subject: [PATCH 038/126] [CI] Add new variable to reusable workflow set unique name (#442) - Add new variable to reusable workflow to set unique name for the cache - Variable cannot be used in the container definition --- .github/workflows/galactic-rhel-binary-build.yml | 2 +- .github/workflows/humble-rhel-binary-build.yml | 2 +- .github/workflows/reusable-industrial-ci-with-cache.yml | 2 +- .github/workflows/rolling-rhel-binary-build.yml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/galactic-rhel-binary-build.yml b/.github/workflows/galactic-rhel-binary-build.yml index 5257ec0ee2..518fcc186a 100644 --- a/.github/workflows/galactic-rhel-binary-build.yml +++ b/.github/workflows/galactic-rhel-binary-build.yml @@ -20,7 +20,7 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: galactic - container: ghcr.io/ros-controls/ros:${{ env.ROS_DISTRO }}-rhel + container: ghcr.io/ros-controls/ros:galactic-rhel steps: - uses: actions/checkout@v3 with: diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index efa5ebbbb9..8f4a65a9b5 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -20,7 +20,7 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: humble - container: ghcr.io/ros-controls/ros:${{ env.ROS_DISTRO }}-rhel + container: ghcr.io/ros-controls/ros:humble-rhel steps: - uses: actions/checkout@v3 with: diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml index 0ff359d091..490b680e7c 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -54,7 +54,7 @@ jobs: env: CCACHE_DIR: ${{ github.workspace }}/${{ inputs.ccache_dir }} BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }} - CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.os_code_name }}-${{ inputs.ros_repo }}-${{ github.job }} + CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.upstream_workspace }}-${{ inputs.ros_repo }}-${{ github.job }} steps: - name: Checkout ${{ inputs.ref }} when build is not scheduled if: ${{ github.event_name != 'schedule' }} diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index 42adab88c7..c418319cd7 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -20,7 +20,7 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: rolling - container: ghcr.io/ros-controls/ros:${{ env.ROS_DISTRO }}-rhel + container: ghcr.io/ros-controls/ros:rolling-rhel steps: - uses: actions/checkout@v3 with: From b157c0d35d42f850dbb48f1d35f91796344c2a5c Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sat, 1 Oct 2022 05:07:25 -0600 Subject: [PATCH 039/126] Generate Parameter Library for Joint Trajectory Controller (#384) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * generate_parameter_library for jtc Signed-off-by: Tyler Weaver * Update to latest generator Signed-off-by: Tyler Weaver * Use __map_joints feature Signed-off-by: Tyler Weaver * Update joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml Co-authored-by: AndyZe * Respond to feedback Signed-off-by: Tyler Weaver * Fix cppcheck errors Signed-off-by: Tyler Weaver * Add to repos file for CI Signed-off-by: Tyler Weaver * put ff_velocity_scale back to avoid changing the interface Signed-off-by: Tyler Weaver * update custom parameter validators Signed-off-by: Tyler Weaver Signed-off-by: Tyler Weaver * command_interfaces may not be empty. * Make joint names unique and not empty. * Apply suggestions from code review * Apply suggestions from code review * Fixup the tests and finish the merge. * Fixup linters. * Fixup bug. * Use executor in all cases to avoid test timeouts on the CI. * Add missing parameter declaration in the joint state broadcaster. * Fix unsensible test in IMU Sensor Broadcaster. * Update ros2_controllers.rolling.repos * Update ros2_controllers.humble.repos * Update ros2_controllers-not-released.humble.repos * Update ros2_controllers-not-released.rolling.repos * Update ros2_controllers.humble.repos * Update ros2_controllers-not-released.humble.repos * Update ros2_controllers-not-released.humble.repos * Remove using generate_parameters_library from source. Signed-off-by: Tyler Weaver Signed-off-by: Tyler Weaver Signed-off-by: Tyler Weaver Co-authored-by: AndyZe Co-authored-by: Denis Štogl Co-authored-by: Denis Štogl Co-authored-by: Bence Magyar --- joint_trajectory_controller/CMakeLists.txt | 8 + .../joint_trajectory_controller.hpp | 24 +- .../tolerances.hpp | 36 +- .../validate_jtc_parameters.hpp | 91 +++++ joint_trajectory_controller/package.xml | 1 + .../src/joint_trajectory_controller.cpp | 328 +++++++----------- ...oint_trajectory_controller_parameters.yaml | 128 +++++++ .../test/test_trajectory_controller.cpp | 174 +++++----- .../test/test_trajectory_controller_utils.hpp | 36 +- 9 files changed, 476 insertions(+), 350 deletions(-) create mode 100644 joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp create mode 100644 joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 5689e589f8..6d443c532f 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -29,11 +29,19 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +find_package(generate_parameter_library REQUIRED) + +generate_parameter_library(joint_trajectory_controller_parameters + src/joint_trajectory_controller_parameters.yaml + include/joint_trajectory_controller/validate_jtc_parameters.hpp +) + add_library(${PROJECT_NAME} SHARED src/joint_trajectory_controller.cpp src/trajectory.cpp ) target_include_directories(${PROJECT_NAME} PRIVATE include) +target_link_libraries(${PROJECT_NAME} joint_trajectory_controller_parameters) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 79fe6bccf4..a229489422 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -44,6 +44,8 @@ #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "joint_trajectory_controller_parameters.hpp" + using namespace std::chrono_literals; // NOLINT namespace rclcpp_action @@ -112,11 +114,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const rclcpp_lifecycle::State & previous_state) override; protected: - std::vector joint_names_; - std::vector command_joint_names_; - std::vector command_interface_types_; - std::vector state_interface_types_; - // To reduce number of variables and to make the code shorter the interfaces are ordered in types // as the following constants const std::vector allowed_interface_types_ = { @@ -134,20 +131,18 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Degrees of freedom size_t dof_; - // Parameters for some special cases, e.g. hydraulics powered robots - // Run the controller in open-loop, i.e., read hardware states only when starting controller. - // This is useful when robot is not exactly following the commanded trajectory. - bool open_loop_control_ = false; + // Storing command joint names for interfaces + std::vector command_joint_names_; + + // Parameters from ROS for joint_trajectory_controller + std::shared_ptr param_listener_; + Params params_; + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; - /// Allow integration in goal trajectories to accept goals without position or velocity specified - bool allow_integration_in_goal_trajectories_ = false; /// Specify interpolation method. Default to splines. interpolation_methods::InterpolationMethod interpolation_method_{ interpolation_methods::DEFAULT_INTERPOLATION}; - double state_publish_rate_; - double action_monitor_rate_; - // The interfaces are defined as the types in 'allowed_interface_types_' member. // For convenience, for each type the interfaces are ordered so that i-th position // matches i-th index in joint_names_ @@ -201,7 +196,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; rclcpp_action::Server::SharedPtr action_server_; - bool allow_partial_joints_goal_ = false; RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. rclcpp::TimerBase::SharedPtr goal_handle_timer_; rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 8f3f30d476..de8f060b1a 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -36,6 +36,7 @@ #include #include "control_msgs/action/follow_joint_trajectory.hpp" +#include "joint_trajectory_controller_parameters.hpp" #include "rclcpp/node.hpp" @@ -87,45 +88,34 @@ struct SegmentTolerances * goal: 0.01 * \endcode * - * \param node LifecycleNode where the tolerances are specified. - * \param joint_names Names of joints to look for in the parameter server for a tolerance specification. + * \param params The ROS Parameters * \return Trajectory segment tolerances. */ -SegmentTolerances get_segment_tolerances( - const rclcpp_lifecycle::LifecycleNode & node, const std::vector & joint_names) +SegmentTolerances get_segment_tolerances(Params const & params) { - const auto n_joints = joint_names.size(); + auto const & constraints = params.constraints; + auto const n_joints = params.joints.size(); + SegmentTolerances tolerances; + tolerances.goal_time_tolerance = constraints.goal_time; // State and goal state tolerances - double stopped_velocity_tolerance = 0.01; - node.get_parameter_or( - "constraints.stopped_velocity_tolerance", stopped_velocity_tolerance, - stopped_velocity_tolerance); - tolerances.state_tolerance.resize(n_joints); tolerances.goal_state_tolerance.resize(n_joints); for (size_t i = 0; i < n_joints; ++i) { - const std::string prefix = "constraints." + joint_names[i]; - - node.get_parameter_or( - prefix + ".trajectory", tolerances.state_tolerance[i].position, 0.0); - node.get_parameter_or( - prefix + ".goal", tolerances.goal_state_tolerance[i].position, 0.0); + auto const joint = params.joints[i]; + tolerances.state_tolerance[i].position = constraints.joints_map.at(joint).trajectory; + tolerances.goal_state_tolerance[i].position = constraints.joints_map.at(joint).goal; + tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance; auto logger = rclcpp::get_logger("tolerance"); RCLCPP_DEBUG( - logger, "%s %f", (prefix + ".trajectory").c_str(), tolerances.state_tolerance[i].position); + logger, "%s %f", (joint + ".trajectory").c_str(), tolerances.state_tolerance[i].position); RCLCPP_DEBUG( - logger, "%s %f", (prefix + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); - - tolerances.goal_state_tolerance[i].velocity = stopped_velocity_tolerance; + logger, "%s %f", (joint + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); } - // Goal time tolerance - node.get_parameter_or("constraints.goal_time", tolerances.goal_time_tolerance, 0.0); - return tolerances; } diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp new file mode 100644 index 0000000000..09363eebb1 --- /dev/null +++ b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp @@ -0,0 +1,91 @@ +// Copyright (c) 2022 ros2_control Development Team +// +// 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 JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_ + +#include + +#include "parameter_traits/parameter_traits.hpp" + +namespace parameter_traits +{ +Result command_interface_type_combinations(rclcpp::Parameter const & parameter) +{ + auto const & interface_types = parameter.as_string_array(); + + // Check if command interfaces combination is valid. Valid combinations are: + // 1. effort + // 2. velocity + // 2. position [velocity, [acceleration]] + + if ( + contains(interface_types, "velocity") && interface_types.size() > 1 && + !contains(interface_types, "position")) + { + return ERROR( + "'velocity' command interface can be used either alone or 'position' " + "interface has to be present"); + } + + if ( + contains(interface_types, "acceleration") && + (!contains(interface_types, "velocity") && + !contains(interface_types, "position"))) + { + return ERROR( + "'acceleration' command interface can only be used if 'velocity' and " + "'position' interfaces are present"); + } + + if (contains(interface_types, "effort") && interface_types.size() > 1) + { + return ERROR("'effort' command interface has to be used alone"); + } + + return OK; +} + +Result state_interface_type_combinations(rclcpp::Parameter const & parameter) +{ + auto const & interface_types = parameter.as_string_array(); + + // Valid combinations are + // 1. position [velocity, [acceleration]] + + if ( + contains(interface_types, "velocity") && + !contains(interface_types, "position")) + { + return ERROR( + "'velocity' state interface cannot be used if 'position' interface " + "is missing."); + } + + if ( + contains(interface_types, "acceleration") && + (!contains(interface_types, "position") || + !contains(interface_types, "velocity"))) + { + return ERROR( + "'acceleration' state interface cannot be used if 'position' and 'velocity' " + "interfaces are not present."); + } + + return OK; +} + +} // namespace parameter_traits + +#endif // JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_ diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 158b399e09..3880b28508 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -12,6 +12,7 @@ ament_cmake angles + generate_parameter_library pluginlib realtime_tools diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index f5f5190caa..96110adcb2 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -46,7 +46,7 @@ namespace joint_trajectory_controller { JointTrajectoryController::JointTrajectoryController() -: controller_interface::ControllerInterface(), joint_names_({}), dof_(0) +: controller_interface::ControllerInterface(), dof_(0) { } @@ -54,26 +54,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() { try { - // with the lifecycle node being initialized, we can declare parameters - joint_names_ = auto_declare>("joints", joint_names_); - command_joint_names_ = - auto_declare>("command_joints", command_joint_names_); - command_interface_types_ = - auto_declare>("command_interfaces", command_interface_types_); - state_interface_types_ = - auto_declare>("state_interfaces", state_interface_types_); - allow_partial_joints_goal_ = - auto_declare("allow_partial_joints_goal", allow_partial_joints_goal_); - open_loop_control_ = auto_declare("open_loop_control", open_loop_control_); - allow_integration_in_goal_trajectories_ = auto_declare( - "allow_integration_in_goal_trajectories", allow_integration_in_goal_trajectories_); - state_publish_rate_ = auto_declare("state_publish_rate", 50.0); - action_monitor_rate_ = auto_declare("action_monitor_rate", 20.0); - - const std::string interpolation_string = auto_declare( - "interpolation_method", interpolation_methods::InterpolationMethodMap.at( - interpolation_methods::DEFAULT_INTERPOLATION)); - interpolation_method_ = interpolation_methods::from_string(interpolation_string); + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + + // Set interpolation method from string parameter + interpolation_method_ = interpolation_methods::from_string(params_.interpolation_method); } catch (const std::exception & e) { @@ -98,10 +84,10 @@ JointTrajectoryController::command_interface_configuration() const dof_); std::exit(EXIT_FAILURE); } - conf.names.reserve(dof_ * command_interface_types_.size()); + conf.names.reserve(dof_ * params_.command_interfaces.size()); for (const auto & joint_name : command_joint_names_) { - for (const auto & interface_type : command_interface_types_) + for (const auto & interface_type : params_.command_interfaces) { conf.names.push_back(joint_name + "/" + interface_type); } @@ -114,10 +100,10 @@ JointTrajectoryController::state_interface_configuration() const { controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(dof_ * state_interface_types_.size()); - for (const auto & joint_name : joint_names_) + conf.names.reserve(dof_ * params_.state_interfaces.size()); + for (const auto & joint_name : params_.joints) { - for (const auto & interface_type : state_interface_types_) + for (const auto & interface_type : params_.state_interfaces) { conf.names.push_back(joint_name + "/" + interface_type); } @@ -185,7 +171,7 @@ controller_interface::return_type JointTrajectoryController::update( if (!(*traj_point_active_ptr_)->is_sampled_already()) { first_sample = true; - if (open_loop_control_) + if (params_.open_loop_control) { (*traj_point_active_ptr_)->set_point_before_trajectory_msg(time, last_commanded_state_); } @@ -305,7 +291,7 @@ controller_interface::return_type JointTrajectoryController::update( // send feedback auto feedback = std::make_shared(); feedback->header.stamp = time; - feedback->joint_names = joint_names_; + feedback->joint_names = params_.joints; feedback->actual = state_current_; feedback->desired = state_desired_; @@ -481,9 +467,20 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( { const auto logger = get_node()->get_logger(); - // update parameters - joint_names_ = get_node()->get_parameter("joints").as_string_array(); - if ((dof_ > 0) && (joint_names_.size() != dof_)) + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + + // Check if the DoF has changed + if ((dof_ > 0) && (params_.joints.size() != dof_)) { RCLCPP_ERROR( logger, @@ -493,7 +490,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( return CallbackReturn::FAILURE; } - dof_ = joint_names_.size(); + dof_ = params_.joints.size(); // TODO(destogl): why is this here? Add comment or move if (!reset()) @@ -501,34 +498,34 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( return CallbackReturn::FAILURE; } - if (joint_names_.empty()) + if (params_.joints.empty()) { // TODO(destogl): is this correct? Can we really move-on if no joint names are not provided? RCLCPP_WARN(logger, "'joints' parameter is empty."); } - command_joint_names_ = get_node()->get_parameter("command_joints").as_string_array(); + command_joint_names_ = params_.command_joints; if (command_joint_names_.empty()) { - command_joint_names_ = joint_names_; + command_joint_names_ = params_.joints; RCLCPP_INFO( logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); } - else if (command_joint_names_.size() != joint_names_.size()) + else if (command_joint_names_.size() != params_.joints.size()) { RCLCPP_ERROR( logger, "'command_joints' parameter has to have the same size as 'joints' parameter."); return CallbackReturn::FAILURE; } - // Specialized, child controllers set interfaces before calling configure function. - if (command_interface_types_.empty()) - { - command_interface_types_ = get_node()->get_parameter("command_interfaces").as_string_array(); - } + // // Specialized, child controllers set interfaces before calling configure function. + // if (command_interface_types_.empty()) + // { + // command_interface_types_ = get_node()->get_parameter("command_interfaces").as_string_array(); + // } - if (command_interface_types_.empty()) + if (params_.command_interfaces.empty()) { RCLCPP_ERROR(logger, "'command_interfaces' parameter is empty."); return CallbackReturn::FAILURE; @@ -537,70 +534,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // Check if only allowed interface types are used and initialize storage to avoid memory // allocation during activation joint_command_interface_.resize(allowed_interface_types_.size()); - for (const auto & interface : command_interface_types_) - { - auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - if (it == allowed_interface_types_.end()) - { - RCLCPP_ERROR(logger, "Command interface type '%s' not allowed!", interface.c_str()); - return CallbackReturn::FAILURE; - } - } - - // Check if command interfaces combination is valid. Valid combinations are: - // 1. effort - // 2. velocity - // 2. position [velocity, [acceleration]] has_position_command_interface_ = - contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION); + contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_POSITION); has_velocity_command_interface_ = - contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY); + contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_VELOCITY); has_acceleration_command_interface_ = - contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION); + contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_ACCELERATION); has_effort_command_interface_ = - contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT); - - if (has_velocity_command_interface_) - { - // if there is only velocity then use also PID adapter - if (command_interface_types_.size() == 1) - { - use_closed_loop_pid_adapter_ = true; - } - else if (!has_position_command_interface_) - { - RCLCPP_ERROR( - logger, - "'velocity' command interface can be used either alone or 'position' " - "interface has to be present."); - return CallbackReturn::FAILURE; - } - // invalid: acceleration is defined and no velocity - } - else if (has_acceleration_command_interface_) - { - RCLCPP_ERROR( - logger, - "'acceleration' command interface can only be used if 'velocity' and " - "'position' interfaces are present"); - return CallbackReturn::FAILURE; - } + contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_EFFORT); - // effort can't be combined with other interfaces - if (has_effort_command_interface_) - { - if (command_interface_types_.size() == 1) - { - use_closed_loop_pid_adapter_ = true; - } - else - { - RCLCPP_ERROR(logger, "'effort' command interface has to be used alone."); - return CallbackReturn::FAILURE; - } - } + // if there is only velocity or if there is effort command interface + // then use also PID adapter + use_closed_loop_pid_adapter_ = + (has_velocity_command_interface_ && params_.command_interfaces.size() == 1) || + has_effort_command_interface_; if (use_closed_loop_pid_adapter_) { @@ -608,35 +556,34 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( ff_velocity_scale_.resize(dof_); tmp_command_.resize(dof_, 0.0); - // Init PID gains from ROS parameter server - for (size_t i = 0; i < pids_.size(); ++i) + // Init PID gains from ROS parameters + for (size_t i = 0; i < dof_; ++i) { - const std::string prefix = "gains." + command_joint_names_[i]; - const auto k_p = auto_declare(prefix + ".p", 0.0); - const auto k_i = auto_declare(prefix + ".i", 0.0); - const auto k_d = auto_declare(prefix + ".d", 0.0); - const auto i_clamp = auto_declare(prefix + ".i_clamp", 0.0); - ff_velocity_scale_[i] = - auto_declare("ff_velocity_scale/" + command_joint_names_[i], 0.0); - // Initialize PID - pids_[i] = std::make_shared(k_p, k_i, k_d, i_clamp, -i_clamp); - } - } + const auto & gains = params_.gains.joints_map.at(params_.joints[i]); + pids_[i] = std::make_shared( + gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); - // Read always state interfaces from the parameter because they can be used - // independently from the controller's type. - // Specialized, child controllers should set its default value. - state_interface_types_ = get_node()->get_parameter("state_interfaces").as_string_array(); + // TODO(destogl): remove this in ROS2 Iron + // Check deprecated style for "ff_velocity_scale" parameter definition. + if (gains.ff_velocity_scale == 0.0) + { + RCLCPP_WARN( + get_node()->get_logger(), + "'ff_velocity_scale' parameters is not defined under 'gains..' structure. " + "Maybe you are using deprecated format 'ff_velocity_scale/'!"); - if (state_interface_types_.empty()) - { - RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); - return CallbackReturn::FAILURE; + ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + params_.joints[i], 0.0); + } + else + { + ff_velocity_scale_[i] = gains.ff_velocity_scale; + } + } } - if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_EFFORT)) + if (params_.state_interfaces.empty()) { - RCLCPP_ERROR(logger, "State interface type 'effort' not allowed!"); + RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); return CallbackReturn::FAILURE; } @@ -644,62 +591,38 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // allocation during activation // Note: 'effort' storage is also here, but never used. Still, for this is OK. joint_state_interface_.resize(allowed_interface_types_.size()); - for (const auto & interface : state_interface_types_) - { - auto it = - std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - if (it == allowed_interface_types_.end()) - { - RCLCPP_ERROR(logger, "State interface type '%s' not allowed!", interface.c_str()); - return CallbackReturn::FAILURE; - } - } has_position_state_interface_ = - contains_interface_type(state_interface_types_, hardware_interface::HW_IF_POSITION); + contains_interface_type(params_.state_interfaces, hardware_interface::HW_IF_POSITION); has_velocity_state_interface_ = - contains_interface_type(state_interface_types_, hardware_interface::HW_IF_VELOCITY); + contains_interface_type(params_.state_interfaces, hardware_interface::HW_IF_VELOCITY); has_acceleration_state_interface_ = - contains_interface_type(state_interface_types_, hardware_interface::HW_IF_ACCELERATION); + contains_interface_type(params_.state_interfaces, hardware_interface::HW_IF_ACCELERATION); - if (has_velocity_state_interface_) + // Validation of combinations of state and velocity together have to be done + // here because the parameter validators only deal with each parameter + // separately. + if ( + has_velocity_command_interface_ && params_.command_interfaces.size() == 1 && + (!has_velocity_state_interface_ || !has_position_state_interface_)) { - if (!has_position_state_interface_) - { - RCLCPP_ERROR( - logger, - "'velocity' state interface cannot be used if 'position' interface " - "is missing."); - return CallbackReturn::FAILURE; - } + RCLCPP_ERROR( + logger, + "'velocity' command interface can only be used alone if 'velocity' and " + "'position' state interfaces are present"); + return CallbackReturn::FAILURE; } - else + + // effort is always used alone so no need for size check + if ( + has_effort_command_interface_ && + (!has_velocity_state_interface_ || !has_position_state_interface_)) { - if (has_acceleration_state_interface_) - { - RCLCPP_ERROR( - logger, - "'acceleration' state interface cannot be used if 'position' and 'velocity' " - "interfaces are not present."); - return CallbackReturn::FAILURE; - } - if (has_velocity_command_interface_ && command_interface_types_.size() == 1) - { - RCLCPP_ERROR( - logger, - "'velocity' command interface can only be used alone if 'velocity' and " - "'position' state interfaces are present"); - return CallbackReturn::FAILURE; - } - // effort is always used alone so no need for size check - if (has_effort_command_interface_) - { - RCLCPP_ERROR( - logger, - "'effort' command interface can only be used alone if 'velocity' and " - "'position' state interfaces are present"); - return CallbackReturn::FAILURE; - } + RCLCPP_ERROR( + logger, + "'effort' command interface can only be used alone if 'velocity' and " + "'position' state interfaces are present"); + return CallbackReturn::FAILURE; } auto get_interface_list = [](const std::vector & interface_types) @@ -719,15 +642,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // Print output so users can be sure the interface setup is correct RCLCPP_INFO( logger, "Command interfaces are [%s] and state interfaces are [%s].", - get_interface_list(command_interface_types_).c_str(), - get_interface_list(state_interface_types_).c_str()); - - default_tolerances_ = get_segment_tolerances(*get_node(), command_joint_names_); + get_interface_list(params_.command_interfaces).c_str(), + get_interface_list(params_.state_interfaces).c_str()); - // Read parameters customizing controller for special cases - open_loop_control_ = get_node()->get_parameter("open_loop_control").get_value(); - allow_integration_in_goal_trajectories_ = - get_node()->get_parameter("allow_integration_in_goal_trajectories").get_value(); + default_tolerances_ = get_segment_tolerances(params_); const std::string interpolation_string = get_node()->get_parameter("interpolation_method").as_string(); @@ -742,11 +660,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1)); // State publisher - state_publish_rate_ = get_node()->get_parameter("state_publish_rate").get_value(); - RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate_); - if (state_publish_rate_ > 0.0) + RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", params_.state_publish_rate); + if (params_.state_publish_rate > 0.0) { - state_publisher_period_ = rclcpp::Duration::from_seconds(1.0 / state_publish_rate_); + state_publisher_period_ = rclcpp::Duration::from_seconds(1.0 / params_.state_publish_rate); } else { @@ -758,7 +675,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( state_publisher_ = std::make_unique(publisher_); state_publisher_->lock(); - state_publisher_->msg_.joint_names = command_joint_names_; + state_publisher_->msg_.joint_names = params_.joints; state_publisher_->msg_.desired.positions.resize(dof_); state_publisher_->msg_.desired.velocities.resize(dof_); state_publisher_->msg_.desired.accelerations.resize(dof_); @@ -779,16 +696,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( last_state_publish_time_ = get_node()->now(); // action server configuration - allow_partial_joints_goal_ = - get_node()->get_parameter("allow_partial_joints_goal").get_value(); - if (allow_partial_joints_goal_) + if (params_.allow_partial_joints_goal) { RCLCPP_INFO(logger, "Goals with partial set of joints are allowed"); } - action_monitor_rate_ = get_node()->get_parameter("action_monitor_rate").get_value(); - RCLCPP_INFO(logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate_); - action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / action_monitor_rate_); + RCLCPP_INFO( + logger, "Action status changes will be monitored at %.2f Hz.", params_.action_monitor_rate); + action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / params_.action_monitor_rate); using namespace std::placeholders; action_server_ = rclcpp_action::create_server( @@ -811,7 +726,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( const rclcpp_lifecycle::State &) { // order all joints in the storage - for (const auto & interface : command_interface_types_) + for (const auto & interface : params_.command_interfaces) { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); @@ -825,13 +740,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( return CallbackReturn::ERROR; } } - for (const auto & interface : state_interface_types_) + for (const auto & interface : params_.state_interfaces) { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); auto index = std::distance(allowed_interface_types_.begin(), it); if (!controller_interface::get_ordered_interfaces( - state_interfaces_, joint_names_, interface, joint_state_interface_[index])) + state_interfaces_, params_.joints, interface, joint_state_interface_[index])) { RCLCPP_ERROR( get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_, @@ -943,7 +858,10 @@ bool JointTrajectoryController::reset() for (const auto & pid : pids_) { - pid->reset(); + if (pid) + { + pid->reset(); + } } // iterator has no default value @@ -953,12 +871,6 @@ bool JointTrajectoryController::reset() traj_home_point_ptr_.reset(); traj_msg_home_ptr_.reset(); - // reset pids - for (const auto & pid : pids_) - { - pid->reset(); - } - return true; } @@ -1083,7 +995,7 @@ void JointTrajectoryController::goal_accepted_callback( // Update the active goal RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); - rt_goal->preallocated_feedback_->joint_names = joint_names_; + rt_goal->preallocated_feedback_->joint_names = params_.joints; rt_goal->execute(); rt_active_goal_.writeFromNonRT(rt_goal); @@ -1114,12 +1026,12 @@ void JointTrajectoryController::fill_partial_goal( if ( std::find( trajectory_msg->joint_names.begin(), trajectory_msg->joint_names.end(), - joint_names_[index]) != trajectory_msg->joint_names.end()) + params_.joints[index]) != trajectory_msg->joint_names.end()) { // joint found on msg continue; } - trajectory_msg->joint_names.push_back(joint_names_[index]); + trajectory_msg->joint_names.push_back(params_.joints[index]); for (auto & it : trajectory_msg->points) { @@ -1160,7 +1072,7 @@ void JointTrajectoryController::sort_to_local_joint_order( std::shared_ptr trajectory_msg) { // rearrange all points in the trajectory message based on mapping - std::vector mapping_vector = mapping(trajectory_msg->joint_names, joint_names_); + std::vector mapping_vector = mapping(trajectory_msg->joint_names, params_.joints); auto remap = [this]( const std::vector & to_remap, const std::vector & mapping) -> std::vector @@ -1223,7 +1135,7 @@ bool JointTrajectoryController::validate_trajectory_msg( const trajectory_msgs::msg::JointTrajectory & trajectory) const { // If partial joints goals are not allowed, goal should specify all controller joints - if (!allow_partial_joints_goal_) + if (!params_.allow_partial_joints_goal) { if (trajectory.joint_names.size() != dof_) { @@ -1265,8 +1177,8 @@ bool JointTrajectoryController::validate_trajectory_msg( { const std::string & incoming_joint_name = trajectory.joint_names[i]; - auto it = std::find(joint_names_.begin(), joint_names_.end(), incoming_joint_name); - if (it == joint_names_.end()) + auto it = std::find(params_.joints.begin(), params_.joints.end(), incoming_joint_name); + if (it == params_.joints.end()) { RCLCPP_ERROR( get_node()->get_logger(), "Incoming joint %s doesn't match the controller's joints.", @@ -1292,7 +1204,7 @@ bool JointTrajectoryController::validate_trajectory_msg( const size_t joint_count = trajectory.joint_names.size(); const auto & points = trajectory.points; // This currently supports only position, velocity and acceleration inputs - if (allow_integration_in_goal_trajectories_) + if (params_.allow_integration_in_goal_trajectories) { const bool all_empty = points[i].positions.empty() && points[i].velocities.empty() && points[i].accelerations.empty(); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml new file mode 100644 index 0000000000..80aa32585b --- /dev/null +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -0,0 +1,128 @@ +joint_trajectory_controller: + joints: { + type: string_array, + default_value: [], + description: "Names of joints used by the controller", + validation: { + unique<>: null, + } + } + command_joints: { + type: string_array, + default_value: [], + description: "Names of the commanding joints used by the controller", + validation: { + unique<>: null, + } + } + command_interfaces: { + type: string_array, + default_value: [], + description: "Names of command interfaces to claim", + validation: { + unique<>: null, + subset_of<>: [["position", "velocity", "acceleration", "effort",]], + command_interface_type_combinations: null, + } + } + state_interfaces: { + type: string_array, + default_value: [], + description: "Names of state interfaces to claim", + validation: { + unique<>: null, + subset_of<>: [["position", "velocity", "acceleration",]], + state_interface_type_combinations: null, + } + } + allow_partial_joints_goal: { + type: bool, + default_value: false, + description: "Goals with partial set of joints are allowed", + } + open_loop_control: { + type: bool, + default_value: false, + description: "Run the controller in open-loop, i.e., read hardware states only when starting controller. This is useful when robot is not exactly following the commanded trajectory.", + } + allow_integration_in_goal_trajectories: { + type: bool, + default_value: false, + description: "Allow integration in goal trajectories to accept goals without position or velocity specified", + } + state_publish_rate: { + type: double, + default_value: 50.0, + description: "Rate controller state is published", + validation: { + lower_bounds: [0.1] + } + } + action_monitor_rate: { + type: double, + default_value: 20.0, + description: "Rate status changes will be monitored", + validation: { + lower_bounds: [0.1] + } + } + interpolation_method: { + type: string, + default_value: "splines", + description: "The type of interpolation to use, if any", + validation: { + one_of<>: [["splines", "none"]], + } + } + gains: + __map_joints: + p: { + type: double, + default_value: 0.0, + description: "Proportional gain for PID" + } + i: { + type: double, + default_value: 0.0, + description: "Intigral gain for PID" + } + d: { + type: double, + default_value: 0.0, + description: "Derivative gain for PID" + } + i_clamp: { + type: double, + default_value: 0.0, + description: "Integrale clamp. Symmetrical in both positive and negative direction." + } + ff_velocity_scale: { + type: double, + default_value: 0.0, + description: "Feed-forward scaling of velocity." + } + constraints: + stopped_velocity_tolerance: { + type: double, + default_value: 0.01, + description: "Velocity tolerance for at the end of the trajectory that indicates that controlled system is stopped.", + } + goal_time: { + type: double, + default_value: 0.0, + description: "Time tolerance for achieving trajectory goal before or after commanded time.", + validation: { + lower_bounds: [0.0], + } + } + __map_joints: + trajectory: { + type: double, + default_value: 0.0, + description: "Per-joint trajectory offset tolerance during movement.", + } + goal: { + type: double, + default_value: 0.0, + description: "Per-joint trajectory offset tolerance at the goal position.", + } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index e758dc038f..6b0020c294 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -63,10 +63,9 @@ void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { - SetUpTrajectoryController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + SetUpTrajectoryController(executor); + // const auto future_handle_ = std::async(std::launch::async, spin, &executor); const auto state = traj_controller_->get_node()->configure(); @@ -97,7 +96,8 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) TEST_P(TrajectoryControllerTestParameterized, check_interface_names) { - SetUpTrajectoryController(); + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor); const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); @@ -135,7 +135,8 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names) TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints) { - SetUpTrajectoryController(); + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor); // set command_joints parameter const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_); @@ -178,10 +179,8 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command TEST_P(TrajectoryControllerTestParameterized, activate) { - SetUpTrajectoryController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + SetUpTrajectoryController(executor); traj_controller_->get_node()->configure(); ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_INACTIVE); @@ -315,11 +314,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate) TEST_P(TrajectoryControllerTestParameterized, cleanup) { - SetUpAndActivateTrajectoryController(); - - auto traj_node = traj_controller_->get_node(); rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(traj_node->get_node_base_interface()); + SetUpAndActivateTrajectoryController(executor); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); @@ -355,7 +351,8 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_parameters) { - SetUpTrajectoryController(false); + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor, false); // This call is replacing the way parameters are set via launch SetParameters(); @@ -364,8 +361,6 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); ActivateTrajectoryController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(traj_controller_->get_node()->get_node_base_interface()); state = traj_controller_->get_state(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -439,7 +434,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) { rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {}, &executor); + SetUpAndActivateTrajectoryController(executor, true, {}); subscribeToState(); updateController(); @@ -493,7 +488,7 @@ void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_cou rclcpp::Parameter state_publish_rate_param( "state_publish_rate", static_cast(target_msg_count)); rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {state_publish_rate_param}, &executor); + SetUpAndActivateTrajectoryController(executor, true, {state_publish_rate_param}); auto future_handle = std::async(std::launch::async, [&executor]() -> void { executor.spin(); }); @@ -530,10 +525,10 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) test_state_publish_rate_target(10); } -TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) -{ - test_state_publish_rate_target(0); -} +// TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) +// { +// test_state_publish_rate_target(0); +// } // /** // * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from @@ -1393,71 +1388,72 @@ INSTANTIATE_TEST_SUITE_P( // std::vector({"effort"}), // std::vector({"position", "velocity", "acceleration"})))); -TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) -{ - auto set_parameter_and_check_result = [&]() - { - EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); - SetParameters(); // This call is replacing the way parameters are set via launch - traj_controller_->get_node()->configure(); - EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); - }; - - SetUpTrajectoryController(false); - - // command interfaces: empty - command_interface_types_ = {}; - set_parameter_and_check_result(); - - // command interfaces: bad_name - command_interface_types_ = {"bad_name"}; - set_parameter_and_check_result(); - - // command interfaces: effort has to be only - command_interface_types_ = {"effort", "position"}; - set_parameter_and_check_result(); - - // command interfaces: velocity - position not present - command_interface_types_ = {"velocity", "acceleration"}; - set_parameter_and_check_result(); - - // command interfaces: acceleration without position and velocity - command_interface_types_ = {"acceleration"}; - set_parameter_and_check_result(); - - // state interfaces: empty - state_interface_types_ = {}; - set_parameter_and_check_result(); - - // state interfaces: cannot not be effort - state_interface_types_ = {"effort"}; - set_parameter_and_check_result(); - - // state interfaces: bad name - state_interface_types_ = {"bad_name"}; - set_parameter_and_check_result(); - - // state interfaces: velocity - position not present - state_interface_types_ = {"velocity"}; - set_parameter_and_check_result(); - state_interface_types_ = {"velocity", "acceleration"}; - set_parameter_and_check_result(); - - // state interfaces: acceleration without position and velocity - state_interface_types_ = {"acceleration"}; - set_parameter_and_check_result(); - - // velocity-only command interface: position - velocity not present - command_interface_types_ = {"velocity"}; - state_interface_types_ = {"position"}; - set_parameter_and_check_result(); - state_interface_types_ = {"velocity"}; - set_parameter_and_check_result(); - - // effort-only command interface: position - velocity not present - command_interface_types_ = {"effort"}; - state_interface_types_ = {"position"}; - set_parameter_and_check_result(); - state_interface_types_ = {"velocity"}; - set_parameter_and_check_result(); -} +// TODO(destogl): this tests should be changed because we are using `generate_parameters_library` +// TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) +// { +// auto set_parameter_and_check_result = [&]() +// { +// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); +// SetParameters(); // This call is replacing the way parameters are set via launch +// traj_controller_->get_node()->configure(); +// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED); +// }; +// +// SetUpTrajectoryController(false); +// +// // command interfaces: empty +// command_interface_types_ = {}; +// set_parameter_and_check_result(); +// +// // command interfaces: bad_name +// command_interface_types_ = {"bad_name"}; +// set_parameter_and_check_result(); +// +// // command interfaces: effort has to be only +// command_interface_types_ = {"effort", "position"}; +// set_parameter_and_check_result(); +// +// // command interfaces: velocity - position not present +// command_interface_types_ = {"velocity", "acceleration"}; +// set_parameter_and_check_result(); +// +// // command interfaces: acceleration without position and velocity +// command_interface_types_ = {"acceleration"}; +// set_parameter_and_check_result(); +// +// // state interfaces: empty +// state_interface_types_ = {}; +// set_parameter_and_check_result(); +// +// // state interfaces: cannot not be effort +// state_interface_types_ = {"effort"}; +// set_parameter_and_check_result(); +// +// // state interfaces: bad name +// state_interface_types_ = {"bad_name"}; +// set_parameter_and_check_result(); +// +// // state interfaces: velocity - position not present +// state_interface_types_ = {"velocity"}; +// set_parameter_and_check_result(); +// state_interface_types_ = {"velocity", "acceleration"}; +// set_parameter_and_check_result(); +// +// // state interfaces: acceleration without position and velocity +// state_interface_types_ = {"acceleration"}; +// set_parameter_and_check_result(); +// +// // velocity-only command interface: position - velocity not present +// command_interface_types_ = {"velocity"}; +// state_interface_types_ = {"position"}; +// set_parameter_and_check_result(); +// state_interface_types_ = {"velocity"}; +// set_parameter_and_check_result(); +// +// // effort-only command interface: position - velocity not present +// command_interface_types_ = {"effort"}; +// state_interface_types_ = {"position"}; +// set_parameter_and_check_result(); +// state_interface_types_ = {"velocity"}; +// set_parameter_and_check_result(); +// } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 840a624eca..0d523cc88d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -79,7 +79,10 @@ class TestableJointTrajectoryController return success; } - void set_joint_names(const std::vector & joint_names) { joint_names_ = joint_names; } + void set_joint_names(const std::vector & joint_names) + { + params_.joints = joint_names; + } void set_command_joint_names(const std::vector & command_joint_names) { @@ -88,14 +91,16 @@ class TestableJointTrajectoryController void set_command_interfaces(const std::vector & command_interfaces) { - command_interface_types_ = command_interfaces; + params_.command_interfaces = command_interfaces; } void set_state_interfaces(const std::vector & state_interfaces) { - state_interface_types_ = state_interfaces; + params_.state_interfaces = state_interfaces; } + void trigger_declare_parameters() { param_listener_->declare_params(); } + trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() { return last_commanded_state_; @@ -138,9 +143,10 @@ class TrajectoryControllerTest : public ::testing::Test controller_name_ + "/joint_trajectory", rclcpp::SystemDefaultsQoS()); } - void SetUpTrajectoryController(bool use_local_parameters = true) + void SetUpTrajectoryController(rclcpp::Executor & executor, bool use_local_parameters = true) { traj_controller_ = std::make_shared(); + if (use_local_parameters) { traj_controller_->set_joint_names(joint_names_); @@ -152,6 +158,8 @@ class TrajectoryControllerTest : public ::testing::Test { FAIL(); } + executor.add_node(traj_controller_->get_node()->get_node_base_interface()); + SetParameters(); } void SetParameters() @@ -162,8 +170,10 @@ class TrajectoryControllerTest : public ::testing::Test const rclcpp::Parameter state_interfaces_params("state_interfaces", state_interface_types_); node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params}); } + void SetPidParameters() { + traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); for (size_t i = 0; i < joint_names_.size(); ++i) @@ -173,32 +183,28 @@ class TrajectoryControllerTest : public ::testing::Test const rclcpp::Parameter k_i(prefix + ".i", 0.0); const rclcpp::Parameter k_d(prefix + ".d", 0.0); const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); - const rclcpp::Parameter ff_velocity_scale("ff_velocity_scale/" + joint_names_[i], 1.0); + const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", 1.0); node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); } } void SetUpAndActivateTrajectoryController( - bool use_local_parameters = true, const std::vector & parameters = {}, - rclcpp::Executor * executor = nullptr, bool separate_cmd_and_state_values = false) + rclcpp::Executor & executor, bool use_local_parameters = true, + const std::vector & parameters = {}, + bool separate_cmd_and_state_values = false) { - SetUpTrajectoryController(use_local_parameters); + SetUpTrajectoryController(executor, use_local_parameters); + // set pid parameters before configure + SetPidParameters(); for (const auto & param : parameters) { traj_controller_->get_node()->set_parameter(param); } - if (executor) - { - executor->add_node(traj_controller_->get_node()->get_node_base_interface()); - } - // ignore velocity tolerances for this test since they aren't committed in test_robot->write() rclcpp::Parameter stopped_velocity_parameters("constraints.stopped_velocity_tolerance", 0.0); traj_controller_->get_node()->set_parameter(stopped_velocity_parameters); - // set pid parameters before configure - SetPidParameters(); traj_controller_->get_node()->configure(); ActivateTrajectoryController(separate_cmd_and_state_values); From 133ac357b3b539b1c45c246bd56b0b99b151177b Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 5 Oct 2022 00:03:14 -0600 Subject: [PATCH 040/126] Generate parameters for Joint State Broadcaster (#401) Co-authored-by: Bence Magyar --- joint_state_broadcaster/CMakeLists.txt | 10 ++++ .../joint_state_broadcaster.hpp | 6 +-- joint_state_broadcaster/package.xml | 3 +- .../src/joint_state_broadcaster.cpp | 53 ++++++++----------- .../joint_state_broadcaster_parameters.yaml | 30 +++++++++++ 5 files changed, 68 insertions(+), 34 deletions(-) create mode 100644 joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index f8c14de1ef..0717577e15 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -14,12 +14,17 @@ endif() find_package(ament_cmake REQUIRED) find_package(control_msgs REQUIRED) find_package(controller_interface REQUIRED) +find_package(generate_parameter_library REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(rcutils REQUIRED) find_package(realtime_tools REQUIRED) find_package(sensor_msgs REQUIRED) +generate_parameter_library(joint_state_broadcaster_parameters + src/joint_state_broadcaster_parameters.yaml +) + add_library(joint_state_broadcaster SHARED src/joint_state_broadcaster.cpp @@ -29,12 +34,16 @@ ament_target_dependencies(joint_state_broadcaster builtin_interfaces control_msgs controller_interface + generate_parameter_library pluginlib rclcpp_lifecycle rcutils realtime_tools sensor_msgs ) +target_link_libraries(joint_state_broadcaster + joint_state_broadcaster_parameters +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(joint_state_broadcaster PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL") @@ -84,6 +93,7 @@ endif() ament_export_dependencies( control_msgs controller_interface + generate_parameter_library rclcpp_lifecycle sensor_msgs ) diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index f3716149a5..4761f3f250 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -23,6 +23,7 @@ #include "control_msgs/msg/dynamic_joint_state.hpp" #include "controller_interface/controller_interface.hpp" #include "joint_state_broadcaster/visibility_control.h" +#include "joint_state_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "realtime_tools/realtime_publisher.h" @@ -93,9 +94,8 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface protected: // Optional parameters - bool use_local_topics_; - std::vector joints_; - std::vector interfaces_; + std::shared_ptr param_listener_; + Params params_; std::unordered_map map_interface_to_joint_state_; // For the JointState message, diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9262419ec4..fea91eee4d 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -20,10 +20,11 @@ control_msgs controller_interface + generate_parameter_library hardware_interface rclcpp_lifecycle - sensor_msgs realtime_tools + sensor_msgs ament_cmake_gmock controller_manager diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 69f5441a6d..74a19c9ed0 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -50,16 +50,8 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_init() { try { - auto_declare("use_local_topics", false); - auto_declare>("joints", std::vector({})); - auto_declare>("interfaces", std::vector({})); - auto_declare>("extra_joints", std::vector({})); - auto_declare( - std::string("map_interface_to_joint_state.") + HW_IF_POSITION, HW_IF_POSITION); - auto_declare( - std::string("map_interface_to_joint_state.") + HW_IF_VELOCITY, HW_IF_VELOCITY); - auto_declare( - std::string("map_interface_to_joint_state.") + HW_IF_EFFORT, HW_IF_EFFORT); + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -89,9 +81,9 @@ controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interf else { state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto & joint : joints_) + for (const auto & joint : params_.joints) { - for (const auto & interface : interfaces_) + for (const auto & interface : params_.interfaces) { state_interfaces_config.names.push_back(joint + "/" + interface); } @@ -104,9 +96,12 @@ controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interf controller_interface::CallbackReturn JointStateBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - use_local_topics_ = get_node()->get_parameter("use_local_topics").as_bool(); - joints_ = get_node()->get_parameter("joints").as_string_array(); - interfaces_ = get_node()->get_parameter("interfaces").as_string_array(); + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); if (use_all_available_interfaces()) { @@ -114,8 +109,8 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( get_node()->get_logger(), "'joints' or 'interfaces' parameter is empty. " "All available state interfaces will be published"); - joints_.clear(); - interfaces_.clear(); + params_.joints.clear(); + params_.interfaces.clear(); } else { @@ -124,14 +119,12 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( "Publishing state interfaces defined in 'joints' and 'interfaces' parameters."); } - auto get_map_interface_parameter = [&](const std::string & interface) + auto get_map_interface_parameter = + [&](std::string const & interface, std::string const & interface_to_map) { - std::string interface_to_map = - get_node() - ->get_parameter(std::string("map_interface_to_joint_state.") + interface) - .as_string(); - - if (std::find(interfaces_.begin(), interfaces_.end(), interface) != interfaces_.end()) + if ( + std::find(params_.interfaces.begin(), params_.interfaces.end(), interface) != + params_.interfaces.end()) { map_interface_to_joint_state_[interface] = interface; RCLCPP_WARN( @@ -147,13 +140,13 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( }; map_interface_to_joint_state_ = {}; - get_map_interface_parameter(HW_IF_POSITION); - get_map_interface_parameter(HW_IF_VELOCITY); - get_map_interface_parameter(HW_IF_EFFORT); + get_map_interface_parameter(HW_IF_POSITION, params_.map_interface_to_joint_state.position); + get_map_interface_parameter(HW_IF_VELOCITY, params_.map_interface_to_joint_state.velocity); + get_map_interface_parameter(HW_IF_EFFORT, params_.map_interface_to_joint_state.effort); try { - const std::string topic_name_prefix = use_local_topics_ ? "~/" : ""; + const std::string topic_name_prefix = params_.use_local_topics ? "~/" : ""; joint_state_publisher_ = get_node()->create_publisher( topic_name_prefix + "joint_states", rclcpp::SystemDefaultsQoS()); @@ -194,7 +187,7 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_activate( if ( !use_all_available_interfaces() && - state_interfaces_.size() != (joints_.size() * interfaces_.size())) + state_interfaces_.size() != (params_.joints.size() * params_.interfaces.size())) { RCLCPP_WARN( get_node()->get_logger(), @@ -321,7 +314,7 @@ void JointStateBroadcaster::init_dynamic_joint_state_msg() bool JointStateBroadcaster::use_all_available_interfaces() const { - return joints_.empty() || interfaces_.empty(); + return params_.joints.empty() || params_.interfaces.empty(); } double get_value( diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml new file mode 100644 index 0000000000..ba0d4f0051 --- /dev/null +++ b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml @@ -0,0 +1,30 @@ +joint_state_broadcaster: + use_local_topics: { + type: bool, + default_value: false, + } + joints: { + type: string_array, + default_value: [], + } + extra_joints: { + type: string_array, + default_value: [], + } + interfaces: { + type: string_array, + default_value: [], + } + map_interface_to_joint_state: + position: { + type: string, + default_value: "position", + } + velocity: { + type: string, + default_value: "velocity", + } + effort: { + type: string, + default_value: "effort", + } From e0df7ddc5dd6f5800a03d994ef122315b9519c00 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Oct 2022 07:06:06 +0100 Subject: [PATCH 041/126] Update changelogs --- diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 7 +++++++ joint_state_broadcaster/CHANGELOG.rst | 9 +++++++++ joint_trajectory_controller/CHANGELOG.rst | 11 +++++++++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 5 +++++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 14 files changed, 62 insertions(+) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index b575411966..295549da24 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- * Fix formatting CI job (`#418 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6eca17389b..a99f7e090b 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index f2ce7457eb..e3bc6ca2ba 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- * Generate params for ForceTorqueSensorBroadcaster (`#395 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 5755b526e2..6ef0b4bc7d 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index bbf0f65e73..f53c2718d2 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- * Add an initialization of the gripper action command for safe startup. (`#425 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index ce5149b6c7..42408c5560 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix undeclared and wrong parameters in controllers. (`#438 `_) + * Add missing parameter declaration in the joint state broadcaster. + * Fix unsensible test in IMU Sensor Broadcaster. +* Contributors: Denis Štogl + 2.12.0 (2022-09-01) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 8fc784711b..920b585dcf 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Generate parameters for Joint State Broadcaster (`#401 `_) +* Fix undeclared and wrong parameters in controllers. (`#438 `_) + * Add missing parameter declaration in the joint state broadcaster. + * Fix unsensible test in IMU Sensor Broadcaster. +* [JointStateBroadcaster] Reset internal variables to avoid duplication of joints (`#431 `_) +* Contributors: Denis Štogl, Gilmar Correia, Tyler Weaver, Bence Magyar + 2.12.0 (2022-09-01) ------------------- * Fix formatting CI job (`#418 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 967b3f5cee..1a077eff84 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Generate Parameter Library for Joint Trajectory Controller (`#384 `_) +* Fix rates in JTC userdoc.rst (`#433 `_) +* Fix for high CPU usage by JTC in gzserver (`#428 `_) + * Change type cast wall timer period from second to nanoseconds. + create_wall_timer() expects delay in nanoseconds (duration object) however the type cast to seconds will result in 0 (if duration is less than 1s) and thus causing timer to be fired non stop resulting in very high CPU usage. + * Reset smartpointer so that create_wall_timer() call can destroy previous trajectory timer. + node->create_wall_timer() first removes timers associated with expired smartpointers before servicing current request. The JTC timer pointer gets overwrite only after the create_wall_timer() returns and thus not able to remove previous trajectory timer resulting in upto two timers running for JTC during trajectory execution. Althougth the previous timer does nothing but still get fired. +* Contributors: Arshad Mehmood, Borong Yuan, Tyler Weaver, Andy Zelenak, Bence Magyar, Denis Štogl + 2.12.0 (2022-09-01) ------------------- * Use a "steady clock" when measuring time differences (`#427 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 9bdb24a322..d270071f5e 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 4bf2fb4664..06a7f4327d 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index baa90562dc..e413d8ee5a 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Enable definition of all fields in JointTrajectory message when using test node. (`#389 `_) +* Contributors: Denis Štogl + 2.12.0 (2022-09-01) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 2da4d40515..95cc41f1d0 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- * fix: :bug: make bare exceptions more narrow (`#422 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 3875d2e729..aef811442a 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- * Fix formatting CI job (`#418 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 49ffeaee5b..b9b5d6f569 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.12.0 (2022-09-01) ------------------- From 2457680a8e0dcb22cbb48361783e456fef681c30 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Oct 2022 07:06:29 +0100 Subject: [PATCH 042/126] 2.13.0 --- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 30 files changed, 44 insertions(+), 44 deletions(-) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 295549da24..3c2714dc64 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 1d17d25b58..474ff1a2d8 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.12.0 + 2.13.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index a99f7e090b..aeec4541f4 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 692f1928f3..f9164998db 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.12.0 + 2.13.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index e3bc6ca2ba..7e285a6ac0 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 11bb64374d..bf1e01bde7 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.12.0 + 2.13.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 6ef0b4bc7d..19b4711223 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 73943f9d75..f4c137301a 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.12.0 + 2.13.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index f53c2718d2..ee226c598f 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 6403d5efa7..1f5ae7ceba 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.12.0 + 2.13.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 42408c5560..285d7e061d 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- * Fix undeclared and wrong parameters in controllers. (`#438 `_) * Add missing parameter declaration in the joint state broadcaster. * Fix unsensible test in IMU Sensor Broadcaster. diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index b6a28e765c..bc9d930830 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.12.0 + 2.13.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 920b585dcf..9dc2a70a9d 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- * Generate parameters for Joint State Broadcaster (`#401 `_) * Fix undeclared and wrong parameters in controllers. (`#438 `_) * Add missing parameter declaration in the joint state broadcaster. diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index fea91eee4d..43687c70ea 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.12.0 + 2.13.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 1a077eff84..f7244e062d 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- * Generate Parameter Library for Joint Trajectory Controller (`#384 `_) * Fix rates in JTC userdoc.rst (`#433 `_) * Fix for high CPU usage by JTC in gzserver (`#428 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 3880b28508..b3a828efe2 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.12.0 + 2.13.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index d270071f5e..d78bfc246b 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 2a4a3811d2..02d9ca7cf4 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.12.0 + 2.13.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 06a7f4327d..0c194f3db8 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index e0436c2094..125988addf 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.12.0 + 2.13.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index e413d8ee5a..b791ac2a81 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- * Enable definition of all fields in JointTrajectory message when using test node. (`#389 `_) * Contributors: Denis Štogl diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index c714c93d1f..a3810d560a 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.12.0 + 2.13.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 3c3f49a876..fdc3646ad1 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.12.0", + version="2.13.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 95cc41f1d0..130d482684 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 48fe677f28..7529c38428 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.12.0 + 2.13.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 40cc4abdd0..d1a720d311 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.12.0", + version="2.13.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index aef811442a..c71becc1ce 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index aa36f2822e..467ed03ffc 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.12.0 + 2.13.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index b9b5d6f569..cdabd8ca5e 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.13.0 (2022-10-05) +------------------- 2.12.0 (2022-09-01) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index a2cfa040bb..aecb6cf3d1 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.12.0 + 2.13.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 34da157213e64dbccbc32f3d478519371dbac54c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 8 Oct 2022 18:31:39 +0200 Subject: [PATCH 043/126] Remove deprecation warning when parameter without value is set. (#445) --- .../publisher_joint_trajectory_controller.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py index 301fe42cd4..56b56c5998 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py @@ -18,6 +18,7 @@ import rclpy from rclpy.node import Node from builtin_interfaces.msg import Duration +from rcl_interfaces.msg import ParameterDescriptor from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from sensor_msgs.msg import JointState @@ -66,7 +67,7 @@ def __init__(self): # Read all positions from parameters self.goals = [] # List of JointTrajectoryPoint for name in goal_names: - self.declare_parameter(name) + self.declare_parameter(name, descriptor=ParameterDescriptor(dynamic_typing=True)) goal = self.get_parameter(name).value # TODO(anyone): remove this "if" part in ROS Iron From 9be3bfe2614230606ce02b95aabb604e616fd089 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 10 Oct 2022 17:43:06 -0400 Subject: [PATCH 044/126] Add generic admittance controller for TCP wrenches (#370) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: AndyZe Co-authored-by: Denis Štogl --- admittance_controller/CMakeLists.txt | 122 ++++ .../admittance_controller.xml | 9 + admittance_controller/doc/userdoc.rst | 58 ++ .../admittance_controller.hpp | 187 ++++++ .../admittance_controller/admittance_rule.hpp | 216 +++++++ .../admittance_rule_impl.hpp | 405 ++++++++++++ .../visibility_control.h | 49 ++ admittance_controller/package.xml | 43 ++ .../src/admittance_controller.cpp | 592 ++++++++++++++++++ .../src/admittance_controller_parameters.yaml | 159 +++++ .../test/6d_robot_description.hpp | 313 +++++++++ .../test/test_admittance_controller.cpp | 292 +++++++++ .../test/test_admittance_controller.hpp | 463 ++++++++++++++ .../test/test_load_admittance_controller.cpp | 47 ++ admittance_controller/test/test_params.yaml | 111 ++++ ros2_controllers-not-released.humble.repos | 4 + ros2_controllers-not-released.rolling.repos | 4 + ros2_controllers.foxy.repos | 4 - ros2_controllers.galactic.repos | 4 - ros2_controllers.humble.repos | 16 +- ros2_controllers.rolling.repos | 19 +- 21 files changed, 3098 insertions(+), 19 deletions(-) create mode 100644 admittance_controller/CMakeLists.txt create mode 100644 admittance_controller/admittance_controller.xml create mode 100644 admittance_controller/doc/userdoc.rst create mode 100644 admittance_controller/include/admittance_controller/admittance_controller.hpp create mode 100644 admittance_controller/include/admittance_controller/admittance_rule.hpp create mode 100644 admittance_controller/include/admittance_controller/admittance_rule_impl.hpp create mode 100644 admittance_controller/include/admittance_controller/visibility_control.h create mode 100644 admittance_controller/package.xml create mode 100644 admittance_controller/src/admittance_controller.cpp create mode 100644 admittance_controller/src/admittance_controller_parameters.yaml create mode 100644 admittance_controller/test/6d_robot_description.hpp create mode 100644 admittance_controller/test/test_admittance_controller.cpp create mode 100644 admittance_controller/test/test_admittance_controller.hpp create mode 100644 admittance_controller/test/test_load_admittance_controller.cpp create mode 100644 admittance_controller/test/test_params.yaml diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt new file mode 100644 index 0000000000..bbfe19deda --- /dev/null +++ b/admittance_controller/CMakeLists.txt @@ -0,0 +1,122 @@ +cmake_minimum_required(VERSION 3.5) +project(admittance_controller) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +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 + angles + control_msgs + control_toolbox + controller_interface + kinematics_interface + Eigen3 + generate_parameter_library + geometry_msgs + hardware_interface + joint_trajectory_controller + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_kdl + tf2_ros + trajectory_msgs +) + +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(${PROJECT_NAME} SHARED src/admittance_controller.cpp) +target_include_directories(${PROJECT_NAME} PRIVATE include) +generate_parameter_library(${PROJECT_NAME}_parameters src/admittance_controller_parameters.yaml) +target_link_libraries(${PROJECT_NAME} admittance_controller_parameters) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "ADMITTANCE_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) + +install(DIRECTORY include/ + DESTINATION include +) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(control_msgs REQUIRED) + find_package(controller_manager REQUIRED) + find_package(controller_interface REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ## create custom test function to pass yaml file into test main + #function(add_test_with_yaml_input TARGET SOURCES YAML_FILE) + #add_executable(${TARGET} ${SOURCES}) + #_ament_cmake_gmock_find_gmock() + #target_include_directories(${TARGET} PUBLIC "${GMOCK_INCLUDE_DIRS}") + #target_link_libraries(${TARGET} ${GMOCK_LIBRARIES}) + #set(executable "$") + #set(result_file "${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.gtest.xml") + #ament_add_test( + #${TARGET} + #COMMAND ${executable} --ros-args --params-file ${YAML_FILE} + #--gtest_output=xml:${result_file} + #OUTPUT_FILE ${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.txt + #RESULT_FILE ${result_file} + #) + #endfunction() + + # test loading admittance controller + add_rostest_with_parameters_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) + target_include_directories(test_load_admittance_controller PUBLIC ${GMOCK_INCLUDE_DIRS}) + target_link_libraries(test_load_admittance_controller ${GMOCK_LIBRARIES}) + ament_target_dependencies( + test_load_admittance_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + # test admittance controller function + add_rostest_with_parameters_gmock(test_admittance_controller test/test_admittance_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) + target_include_directories(test_admittance_controller PRIVATE include) + target_link_libraries(test_admittance_controller admittance_controller) + ament_target_dependencies( + test_admittance_controller + control_msgs + controller_interface + hardware_interface + ros2_control_test_assets + ) +endif() + +ament_export_include_directories( + include +) +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_package() diff --git a/admittance_controller/admittance_controller.xml b/admittance_controller/admittance_controller.xml new file mode 100644 index 0000000000..3e4d5cb682 --- /dev/null +++ b/admittance_controller/admittance_controller.xml @@ -0,0 +1,9 @@ + + + + AdmittanceController ros2_control controller. + + + diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst new file mode 100644 index 0000000000..60837457f9 --- /dev/null +++ b/admittance_controller/doc/userdoc.rst @@ -0,0 +1,58 @@ +.. _joint_trajectory_controller_userdoc: + +Admittance Controller +====================== + +Admittance controller enables you do zero-force control from a force measured on your TCP. +The controller implements ``ChainedControllerInterface``, so it is possible to add another controllers in front of it, e.g., ``JointTrajectoryController``. + +The controller requires an external kinematics plugin to function. The `kinematics_interface `_ repository provides an interface and an implementation that the admittance controller can use. + +Parameters: + + + +ROS2 interface of the controller +--------------------------------- + +Parameters +^^^^^^^^^^^ + +The admittance controller's uses the `generate_parameter_library `_ to handle its parameters. +The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. +An example parameter file can be found in the `test folder of the controller `_ + + +Topics +^^^^^^^ + +~/joint_references (input topic) [trajectory_msgs::msg::JointTrajectoryPoint] + Target joint commands when controller is not in chained mode. + +~/state (output topic) [control_msgs::msg::AdmittanceControllerState] + Topic publishing internal states. + + +ros2_control interfaces +------------------------ + +References +^^^^^^^^^^^ +The controller has ``position`` and ``velocity`` reference interfaces exported in the format: +``//[position|velocity]`` + + +States +^^^^^^^ +The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. +Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. +If some interface is not provided, the last commanded interface will be used for calculation. + +For handling TCP wrenches `*Force Torque Sensor* semantic component (from package *controller_interface*) `_ is used. +The interfaces have prefix ``ft_sensor.name``, building the interfaces: ``/[force.x|force.y|force.z|torque.x|torque.y|torque.z]``. + + +Commands +^^^^^^^^^ +The command interfaces are defined with ``joints`` and ``command_interfaces`` parameters as follows: ``/``. +Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp new file mode 100644 index 0000000000..f776646d42 --- /dev/null +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -0,0 +1,187 @@ +// Copyright (c) 2022, PickNik, Inc. +// +// 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. +// +/// \authors: Denis Stogl, Andy Zelenak, Paul Gesel + +#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ +#define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ + +#include +#include +#include +#include + +// include generated parameter library +#include "admittance_controller_parameters.hpp" + +#include "admittance_controller/admittance_rule.hpp" +#include "admittance_controller/visibility_control.h" +#include "control_msgs/msg/admittance_controller_state.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/time.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 "semantic_components/force_torque_sensor.hpp" + +#include "trajectory_msgs/msg/joint_trajectory.hpp" + +namespace admittance_controller +{ +using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; + +class AdmittanceController : public controller_interface::ChainableControllerInterface +{ +public: + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + /// Export configuration of required state interfaces. + /** + * Allowed types of state interfaces are \ref hardware_interface::POSITION, + * \ref hardware_interface::VELOCITY, \ref hardware_interface::ACCELERATION. + */ + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + /// Export configuration of required state interfaces. + /** + * Allowed types of state interfaces are \ref hardware_interface::POSITION, + * \ref hardware_interface::VELOCITY, \ref hardware_interface::ACCELERATION. + */ + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; + + ADMITTANCE_CONTROLLER_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + std::vector on_export_reference_interfaces() override; + + controller_interface::return_type update_reference_from_subscribers() override; + + size_t num_joints_ = 0; + std::vector command_joint_names_; + + // The interfaces are defined as the types in 'allowed_interface_types_' member. + // For convenience, for each type the interfaces are ordered so that i-th position + // matches i-th index in joint_names_ + template + using InterfaceReferences = std::vector>>; + + InterfaceReferences joint_command_interface_; + InterfaceReferences joint_state_interface_; + + bool has_position_state_interface_ = false; + bool has_velocity_state_interface_ = false; + bool has_acceleration_state_interface_ = false; + bool has_position_command_interface_ = false; + bool has_velocity_command_interface_ = false; + bool has_acceleration_command_interface_ = false; + bool has_effort_command_interface_ = false; + + // To reduce number of variables and to make the code shorter the interfaces are ordered in types + // as the following constants + const std::vector allowed_interface_types_ = { + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, + hardware_interface::HW_IF_ACCELERATION}; + + // internal reference values + const std::vector allowed_reference_interfaces_types_ = { + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY}; + std::vector> position_reference_; + std::vector> velocity_reference_; + + // Admittance rule and dependent variables; + std::unique_ptr admittance_; + + // force torque sensor + std::unique_ptr force_torque_sensor_; + + // ROS subscribers + rclcpp::Subscription::SharedPtr + input_joint_command_subscriber_; + rclcpp::Publisher::SharedPtr s_publisher_; + + // admittance parameters + std::shared_ptr parameter_handler_; + + // ROS messages + std::shared_ptr joint_command_msg_; + + // real-time buffer + realtime_tools::RealtimeBuffer> + input_joint_command_; + std::unique_ptr> state_publisher_; + + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_; + trajectory_msgs::msg::JointTrajectoryPoint last_reference_; + + // control loop data + // reference_: reference value read by the controller + // joint_state_: current joint readings from the hardware + // reference_admittance_: reference value used by the controller after the admittance values are + // applied ft_values_: values read from the force torque sensor + trajectory_msgs::msg::JointTrajectoryPoint reference_, joint_state_, reference_admittance_; + geometry_msgs::msg::Wrench ft_values_; + + /** + * @brief Read values from hardware interfaces and set corresponding fields of state_current and ft_values + */ + void read_state_from_hardware( + trajectory_msgs::msg::JointTrajectoryPoint & state_current, + geometry_msgs::msg::Wrench & ft_values); + + /** + * @brief Set fields of state_reference with values from controllers exported position and velocity references + */ + void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state); + + /** +* @brief Write values from state_command to claimed hardware interfaces +*/ + void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint & state_command); +}; + +} // namespace admittance_controller + +#endif // ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp new file mode 100644 index 0000000000..0f0aabb063 --- /dev/null +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -0,0 +1,216 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// 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. +// +/// \authors: Denis Stogl, Andy Zelenak, Paul Gesel + +#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ +#define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "control_msgs/msg/admittance_controller_state.hpp" +#include "control_toolbox/filters.hpp" +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "kinematics_interface/kinematics_interface.hpp" +#include "pluginlib/class_loader.hpp" +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_kdl/tf2_kdl.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace admittance_controller +{ +struct AdmittanceTransforms +{ + // transformation from force torque sensor frame to base link frame at reference joint angles + Eigen::Isometry3d ref_base_ft_; + // transformation from force torque sensor frame to base link frame at reference + admittance offset joint angles + Eigen::Isometry3d base_ft_; + // transformation from control frame to base link frame at reference + admittance offset joint angles + Eigen::Isometry3d base_control_; + // transformation from end effector frame to base link frame at reference + admittance offset joint angles + Eigen::Isometry3d base_tip_; + // transformation from center of gravity frame to base link frame at reference + admittance offset joint angles + Eigen::Isometry3d base_cog_; + // transformation from world frame to base link frame + Eigen::Isometry3d world_base_; +}; + +struct AdmittanceState +{ + explicit AdmittanceState(size_t num_joints) + { + admittance_velocity.setZero(); + admittance_acceleration.setZero(); + damping.setZero(); + mass.setOnes(); + mass_inv.setZero(); + stiffness.setZero(); + selected_axes.setZero(); + current_joint_pos = Eigen::VectorXd::Zero(num_joints); + joint_pos = Eigen::VectorXd::Zero(num_joints); + joint_vel = Eigen::VectorXd::Zero(num_joints); + joint_acc = Eigen::VectorXd::Zero(num_joints); + } + + Eigen::VectorXd current_joint_pos; + Eigen::VectorXd joint_pos; + Eigen::VectorXd joint_vel; + Eigen::VectorXd joint_acc; + Eigen::Matrix damping; + Eigen::Matrix mass; + Eigen::Matrix mass_inv; + Eigen::Matrix selected_axes; + Eigen::Matrix stiffness; + Eigen::Matrix wrench_base; + Eigen::Matrix admittance_acceleration; + Eigen::Matrix admittance_velocity; + Eigen::Isometry3d admittance_position; + Eigen::Matrix rot_base_control; + Eigen::Isometry3d ref_trans_base_ft; + std::string ft_sensor_frame; +}; + +class AdmittanceRule +{ +public: + explicit AdmittanceRule( + const std::shared_ptr & parameter_handler) + { + parameter_handler_ = parameter_handler; + parameters_ = parameter_handler_->get_params(); + num_joints_ = parameters_.joints.size(); + admittance_state_ = AdmittanceState(num_joints_); + reset(num_joints_); + } + + /// Configure admittance rule memory using number of joints. + controller_interface::return_type configure( + const std::shared_ptr & node, const size_t num_joint); + + /// Reset all values back to default + controller_interface::return_type reset(const size_t num_joints); + + /** + * Calculate all transforms needed for admittance control using the loader kinematics plugin. If the transform does + * not exist in the kinematics model, then TF will be used for lookup. The return value is true if all transformation + * are calculated without an error + * \param[in] current_joint_state current joint state of the robot + * \param[in] reference_joint_state input joint state reference + * \param[out] success true if no calls to the kinematics interface fail + */ + bool get_all_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state); + + /** + * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent Eigen field + * members (end_effector_weight_, cog_pos_, mass_, mass_inv_ stiffness, selected_axes, damping_) are also updated + */ + void apply_parameters_update(); + + /** + * Calculate 'desired joint states' based on the 'measured force', 'reference joint state', and + * 'current_joint_state'. + * + * \param[in] current_joint_state current joint state of the robot + * \param[in] measured_wrench most recent measured wrench from force torque sensor + * \param[in] reference_joint_state input joint state reference + * \param[in] period time in seconds since last controller update + * \param[out] desired_joint_state joint state reference after the admittance offset is applied to the input reference + */ + controller_interface::return_type update( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const geometry_msgs::msg::Wrench & measured_wrench, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, + const rclcpp::Duration & period, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states); + + /** + * Set fields of `state_message` from current admittance controller state. + * + * \param[out] state_message message containing target position/vel/accel, wrench, and actual robot state, among other things + */ + const control_msgs::msg::AdmittanceControllerState & get_controller_state(); + +public: + // admittance config parameters + std::shared_ptr parameter_handler_; + admittance_controller::Params parameters_; + +protected: + /** + * Calculates the admittance rule from given the robot's current joint angles. The admittance controller state input + * is updated with the new calculated values. A boolean value is returned indicating if any of the kinematics plugin + * calls failed. + * \param[in] admittance_state contains all the information needed to calculate the admittance offset + * \param[in] dt controller period + * \param[out] success true if no calls to the kinematics interface fail + */ + bool calculate_admittance_rule(AdmittanceState & admittance_state, double dt); + + /** + * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement `measured_wrench`, + * the sensor to base frame rotation `sensor_world_rot`, and the center of gravity frame to base frame rotation `cog_world_rot`. + * The `wrench_world_` estimate includes gravity compensation + * \param[in] measured_wrench most recent measured wrench from force torque sensor + * \param[in] sensor_world_rot rotation matrix from world frame to sensor frame + * \param[in] cog_world_rot rotation matrix from world frame to center of gravity frame + */ + void process_wrench_measurements( + const geometry_msgs::msg::Wrench & measured_wrench, + const Eigen::Matrix & sensor_world_rot, + const Eigen::Matrix & cog_world_rot); + + template + void vec_to_eigen(const std::vector & data, T2 & matrix); + + // number of robot joint + size_t num_joints_; + + // Kinematics interface plugin loader + std::shared_ptr> + kinematics_loader_; + std::unique_ptr kinematics_; + + // filtered wrench in world frame + Eigen::Matrix wrench_world_; + + // admittance controllers internal state + AdmittanceState admittance_state_{0}; + + // transforms needed for admittance update + AdmittanceTransforms admittance_transforms_; + + // position of center of gravity in cog_frame + Eigen::Vector3d cog_pos_; + + // force applied to sensor due to weight of end effector + Eigen::Vector3d end_effector_weight_; + + // ROS + control_msgs::msg::AdmittanceControllerState state_message_; +}; + +} // namespace admittance_controller + +#endif // ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp new file mode 100644 index 0000000000..87c16e4787 --- /dev/null +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -0,0 +1,405 @@ +// Copyright (c) 2022, PickNik, Inc. +// +// 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. +// +/// \authors: Denis Stogl, Andy Zelenak, Paul Gesel + +#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ +#define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ + +#include "admittance_controller/admittance_rule.hpp" + +#include +#include + +#include "rclcpp/duration.hpp" +#include "rclcpp/utilities.hpp" +#include "tf2_ros/transform_listener.h" + +namespace admittance_controller +{ +/// Configure admittance rule memory for num joints and load kinematics interface +controller_interface::return_type AdmittanceRule::configure( + const std::shared_ptr & node, const size_t num_joints) +{ + num_joints_ = num_joints; + + // initialize memory and values to zero (non-realtime function) + reset(num_joints); + + // Load the differential IK plugin + if (!parameters_.kinematics.plugin_name.empty()) + { + try + { + kinematics_loader_ = + std::make_shared>( + parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsInterface"); + kinematics_ = std::unique_ptr( + kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); + if (!kinematics_->initialize( + node->get_node_parameters_interface(), parameters_.kinematics.tip)) + { + return controller_interface::return_type::ERROR; + } + } + catch (pluginlib::PluginlibException & ex) + { + RCLCPP_ERROR( + rclcpp::get_logger("AdmittanceRule"), "Exception while loading the IK plugin '%s': '%s'", + parameters_.kinematics.plugin_name.c_str(), ex.what()); + return controller_interface::return_type::ERROR; + } + } + else + { + RCLCPP_ERROR( + rclcpp::get_logger("AdmittanceRule"), + "A differential IK plugin name was not specified in the config file."); + return controller_interface::return_type::ERROR; + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceRule::reset(const size_t num_joints) +{ + // reset state message fields + state_message_.joint_state.name.assign(num_joints, ""); + state_message_.joint_state.position.assign(num_joints, 0); + state_message_.joint_state.velocity.assign(num_joints, 0); + state_message_.joint_state.effort.assign(num_joints, 0); + for (size_t i = 0; i < parameters_.joints.size(); ++i) + { + state_message_.joint_state.name = parameters_.joints; + } + state_message_.mass.data.resize(6, 0.0); + state_message_.selected_axes.data.resize(6, 0); + state_message_.damping.data.resize(6, 0); + state_message_.stiffness.data.resize(6, 0); + state_message_.wrench_base.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_velocity.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_acceleration.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_position.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_position.child_frame_id = "admittance_offset"; + + // reset admittance state + admittance_state_ = AdmittanceState(num_joints); + + // reset transforms and rotations + admittance_transforms_ = AdmittanceTransforms(); + + // reset forces + wrench_world_.setZero(); + end_effector_weight_.setZero(); + + // load/initialize Eigen types from parameters + apply_parameters_update(); + + return controller_interface::return_type::OK; +} + +void AdmittanceRule::apply_parameters_update() +{ + if (parameter_handler_->is_old(parameters_)) + { + parameters_ = parameter_handler_->get_params(); + } + // update param values + end_effector_weight_[2] = -parameters_.gravity_compensation.CoG.force; + vec_to_eigen(parameters_.gravity_compensation.CoG.pos, cog_pos_); + vec_to_eigen(parameters_.admittance.mass, admittance_state_.mass); + vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness); + vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes); + + for (size_t i = 0; i < 6; ++i) + { + admittance_state_.mass_inv[i] = 1.0 / parameters_.admittance.mass[i]; + admittance_state_.damping[i] = parameters_.admittance.damping_ratio[i] * 2 * + sqrt(admittance_state_.mass[i] * admittance_state_.stiffness[i]); + } +} + +bool AdmittanceRule::get_all_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state) +{ + // get reference transforms + bool success = kinematics_->calculate_link_transform( + reference_joint_state.positions, parameters_.ft_sensor.frame.id, + admittance_transforms_.ref_base_ft_); + + // get transforms at current configuration + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.ft_sensor.frame.id, admittance_transforms_.base_ft_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.kinematics.tip, admittance_transforms_.base_tip_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.fixed_world_frame.frame.id, + admittance_transforms_.world_base_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.gravity_compensation.frame.id, + admittance_transforms_.base_cog_); + success &= kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.control.frame.id, + admittance_transforms_.base_control_); + + return success; +} + +// Update from reference joint states +controller_interface::return_type AdmittanceRule::update( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const geometry_msgs::msg::Wrench & measured_wrench, + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, + const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state) +{ + const double dt = period.seconds(); + + if (parameters_.enable_parameter_update_without_reactivation) + { + apply_parameters_update(); + } + + bool success = get_all_transforms(current_joint_state, reference_joint_state); + + // apply filter and update wrench_world_ vector + Eigen::Matrix rot_world_sensor = + admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_ft_.rotation(); + Eigen::Matrix rot_world_cog = + admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_cog_.rotation(); + process_wrench_measurements(measured_wrench, rot_world_sensor, rot_world_cog); + + // transform wrench_world_ into base frame + admittance_state_.wrench_base.block<3, 1>(0, 0) = + admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(0, 0); + admittance_state_.wrench_base.block<3, 1>(3, 0) = + admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(3, 0); + + // Compute admittance control law + vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos); + admittance_state_.rot_base_control = admittance_transforms_.base_control_.rotation(); + admittance_state_.ref_trans_base_ft = admittance_transforms_.ref_base_ft_; + admittance_state_.ft_sensor_frame = parameters_.ft_sensor.frame.id; + success &= calculate_admittance_rule(admittance_state_, dt); + + // if a failure occurred during any kinematics interface calls, return an error and don't + // modify the desired reference + if (!success) + { + desired_joint_state = reference_joint_state; + return controller_interface::return_type::ERROR; + } + + // update joint desired joint state + for (size_t i = 0; i < num_joints_; ++i) + { + desired_joint_state.positions[i] = + reference_joint_state.positions[i] + admittance_state_.joint_pos[i]; + desired_joint_state.velocities[i] = + reference_joint_state.velocities[i] + admittance_state_.joint_vel[i]; + desired_joint_state.accelerations[i] = + reference_joint_state.accelerations[i] + admittance_state_.joint_acc[i]; + } + + return controller_interface::return_type::OK; +} + +bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_state, double dt) +{ + // Create stiffness matrix in base frame. The user-provided values of admittance_state.stiffness + // correspond to the six diagonal elements of the stiffness matrix expressed in the control frame + auto rot_base_control = admittance_state.rot_base_control; + Eigen::Matrix K = Eigen::Matrix::Zero(); + Eigen::Matrix K_pos = Eigen::Matrix::Zero(); + Eigen::Matrix K_rot = Eigen::Matrix::Zero(); + K_pos.diagonal() = admittance_state.stiffness.block<3, 1>(0, 0); + K_rot.diagonal() = admittance_state.stiffness.block<3, 1>(3, 0); + // Transform to the control frame + // A reference is here: https://users.wpi.edu/~jfu2/rbe502/files/force_control.pdf + // Force Control by Luigi Villani and Joris De Schutter + // Page 200 + K_pos = rot_base_control * K_pos * rot_base_control.transpose(); + K_rot = rot_base_control * K_rot * rot_base_control.transpose(); + K.block<3, 3>(0, 0) = K_pos; + K.block<3, 3>(3, 3) = K_rot; + + // The same for damping + Eigen::Matrix D = Eigen::Matrix::Zero(); + Eigen::Matrix D_pos = Eigen::Matrix::Zero(); + Eigen::Matrix D_rot = Eigen::Matrix::Zero(); + D_pos.diagonal() = admittance_state.damping.block<3, 1>(0, 0); + D_rot.diagonal() = admittance_state.damping.block<3, 1>(3, 0); + D_pos = rot_base_control * D_pos * rot_base_control.transpose(); + D_rot = rot_base_control * D_rot * rot_base_control.transpose(); + D.block<3, 3>(0, 0) = D_pos; + D.block<3, 3>(3, 3) = D_rot; + + // calculate admittance relative offset in base frame + Eigen::Isometry3d desired_trans_base_ft; + kinematics_->calculate_link_transform( + admittance_state.current_joint_pos, admittance_state.ft_sensor_frame, desired_trans_base_ft); + Eigen::Matrix X; + X.block<3, 1>(0, 0) = + desired_trans_base_ft.translation() - admittance_state.ref_trans_base_ft.translation(); + auto R_ref = admittance_state.ref_trans_base_ft.rotation(); + auto R_desired = desired_trans_base_ft.rotation(); + auto R = R_desired * R_ref.transpose(); + auto angle_axis = Eigen::AngleAxisd(R); + X.block<3, 1>(3, 0) = angle_axis.angle() * angle_axis.axis(); + + // get admittance relative velocity + auto X_dot = Eigen::Matrix(admittance_state.admittance_velocity.data()); + + // external force expressed in the base frame + auto F_base = admittance_state.wrench_base; + + // zero out any forces in the control frame + Eigen::Matrix F_control; + F_control.block<3, 1>(0, 0) = rot_base_control.transpose() * F_base.block<3, 1>(0, 0); + F_control.block<3, 1>(3, 0) = rot_base_control.transpose() * F_base.block<3, 1>(3, 0); + F_control = F_control.cwiseProduct(admittance_state.selected_axes); + F_base.block<3, 1>(0, 0) = rot_base_control * F_control.block<3, 1>(0, 0); + F_base.block<3, 1>(3, 0) = rot_base_control * F_control.block<3, 1>(3, 0); + + // Compute admittance control law in the base frame: F = M*x_ddot + D*x_dot + K*x + Eigen::Matrix X_ddot = + admittance_state.mass_inv.cwiseProduct(F_base - D * X_dot - K * X); + bool success = kinematics_->convert_cartesian_deltas_to_joint_deltas( + admittance_state.current_joint_pos, X_ddot, admittance_state.ft_sensor_frame, + admittance_state.joint_acc); + + // add damping if cartesian velocity falls below threshold + for (int64_t i = 0; i < admittance_state.joint_acc.size(); ++i) + { + admittance_state.joint_acc[i] -= + parameters_.admittance.joint_damping * admittance_state.joint_vel[i]; + } + + // integrate motion in joint space + admittance_state.joint_vel += (admittance_state.joint_acc) * dt; + admittance_state.joint_pos += admittance_state.joint_vel * dt; + + // calculate admittance velocity corresponding to joint velocity ("base_link" frame) + success &= kinematics_->convert_joint_deltas_to_cartesian_deltas( + admittance_state.current_joint_pos, admittance_state.joint_vel, + admittance_state.ft_sensor_frame, admittance_state.admittance_velocity); + success &= kinematics_->convert_joint_deltas_to_cartesian_deltas( + admittance_state.current_joint_pos, admittance_state.joint_acc, + admittance_state.ft_sensor_frame, admittance_state.admittance_acceleration); + + return success; +} + +void AdmittanceRule::process_wrench_measurements( + const geometry_msgs::msg::Wrench & measured_wrench, + const Eigen::Matrix & sensor_world_rot, + const Eigen::Matrix & cog_world_rot) +{ + Eigen::Matrix new_wrench; + new_wrench(0, 0) = measured_wrench.force.x; + new_wrench(1, 0) = measured_wrench.force.y; + new_wrench(2, 0) = measured_wrench.force.z; + new_wrench(0, 1) = measured_wrench.torque.x; + new_wrench(1, 1) = measured_wrench.torque.y; + new_wrench(2, 1) = measured_wrench.torque.z; + + // transform to world frame + Eigen::Matrix new_wrench_base = sensor_world_rot * new_wrench; + + // apply gravity compensation + new_wrench_base(2, 0) -= end_effector_weight_[2]; + new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_pos_).cross(end_effector_weight_); + + // apply smoothing filter + for (size_t i = 0; i < 6; ++i) + { + wrench_world_(i) = filters::exponentialSmoothing( + new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient); + } +} + +const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state() +{ + for (size_t i = 0; i < parameters_.joints.size(); ++i) + { + state_message_.joint_state.name[i] = parameters_.joints[i]; + state_message_.joint_state.position[i] = admittance_state_.joint_pos[i]; + state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[i]; + state_message_.joint_state.effort[i] = admittance_state_.joint_acc[i]; + state_message_.stiffness.data[i] = admittance_state_.stiffness[i]; + state_message_.damping.data[i] = admittance_state_.damping[i]; + state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[i]); + state_message_.mass.data[i] = admittance_state_.mass[i]; + } + + state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0]; + state_message_.wrench_base.wrench.force.y = admittance_state_.wrench_base[1]; + state_message_.wrench_base.wrench.force.z = admittance_state_.wrench_base[2]; + state_message_.wrench_base.wrench.torque.x = admittance_state_.wrench_base[3]; + state_message_.wrench_base.wrench.torque.y = admittance_state_.wrench_base[4]; + state_message_.wrench_base.wrench.torque.z = admittance_state_.wrench_base[5]; + + state_message_.admittance_velocity.twist.linear.x = admittance_state_.admittance_velocity[0]; + state_message_.admittance_velocity.twist.linear.y = admittance_state_.admittance_velocity[1]; + state_message_.admittance_velocity.twist.linear.z = admittance_state_.admittance_velocity[2]; + state_message_.admittance_velocity.twist.angular.x = admittance_state_.admittance_velocity[3]; + state_message_.admittance_velocity.twist.angular.y = admittance_state_.admittance_velocity[4]; + state_message_.admittance_velocity.twist.angular.z = admittance_state_.admittance_velocity[5]; + + state_message_.admittance_acceleration.twist.linear.x = + admittance_state_.admittance_acceleration[0]; + state_message_.admittance_acceleration.twist.linear.y = + admittance_state_.admittance_acceleration[1]; + state_message_.admittance_acceleration.twist.linear.z = + admittance_state_.admittance_acceleration[2]; + state_message_.admittance_acceleration.twist.angular.x = + admittance_state_.admittance_acceleration[3]; + state_message_.admittance_acceleration.twist.angular.y = + admittance_state_.admittance_acceleration[4]; + state_message_.admittance_acceleration.twist.angular.z = + admittance_state_.admittance_acceleration[5]; + + state_message_.admittance_position = tf2::eigenToTransform(admittance_state_.admittance_position); + + state_message_.ref_trans_base_ft.header.frame_id = parameters_.kinematics.base; + state_message_.ref_trans_base_ft.header.frame_id = "ft_reference"; + state_message_.ref_trans_base_ft = tf2::eigenToTransform(admittance_state_.ref_trans_base_ft); + + Eigen::Quaterniond quat(admittance_state_.rot_base_control); + state_message_.rot_base_control.w = quat.w(); + state_message_.rot_base_control.x = quat.x(); + state_message_.rot_base_control.y = quat.y(); + state_message_.rot_base_control.z = quat.z(); + + state_message_.ft_sensor_frame.data = + admittance_state_.ft_sensor_frame; // TODO(anyone) remove dynamic allocation here + + return state_message_; +} + +template +void AdmittanceRule::vec_to_eigen(const std::vector & data, T2 & matrix) +{ + for (auto col = 0; col < matrix.cols(); col++) + { + for (auto row = 0; row < matrix.rows(); row++) + { + matrix(row, col) = data[row + col * matrix.rows()]; + } + } +} + +} // namespace admittance_controller + +#endif // ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_IMPL_HPP_ diff --git a/admittance_controller/include/admittance_controller/visibility_control.h b/admittance_controller/include/admittance_controller/visibility_control.h new file mode 100644 index 0000000000..24f17a5c2c --- /dev/null +++ b/admittance_controller/include/admittance_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// 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 ADMITTANCE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define ADMITTANCE_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ADMITTANCE_CONTROLLER_EXPORT __attribute__((dllexport)) +#define ADMITTANCE_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define ADMITTANCE_CONTROLLER_EXPORT __declspec(dllexport) +#define ADMITTANCE_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef ADMITTANCE_CONTROLLER_BUILDING_DLL +#define ADMITTANCE_CONTROLLER_PUBLIC ADMITTANCE_CONTROLLER_EXPORT +#else +#define ADMITTANCE_CONTROLLER_PUBLIC ADMITTANCE_CONTROLLER_IMPORT +#endif +#define ADMITTANCE_CONTROLLER_PUBLIC_TYPE ADMITTANCE_CONTROLLER_PUBLIC +#define ADMITTANCE_CONTROLLER_LOCAL +#else +#define ADMITTANCE_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define ADMITTANCE_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define ADMITTANCE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define ADMITTANCE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define ADMITTANCE_CONTROLLER_PUBLIC +#define ADMITTANCE_CONTROLLER_LOCAL +#endif +#define ADMITTANCE_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // ADMITTANCE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml new file mode 100644 index 0000000000..e352dcfa91 --- /dev/null +++ b/admittance_controller/package.xml @@ -0,0 +1,43 @@ + + + + admittance_controller + 0.0.0 + Implementation of admittance controllers for different input and output interface. + Denis Štogl + Andy Zelenak + Apache License 2.0 + + ament_cmake + + control_msgs + control_toolbox + controller_interface + kinematics_interface + filters + generate_parameter_library + geometry_msgs + hardware_interface + joint_trajectory_controller + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_kdl + tf2_ros + trajectory_msgs + + + ament_cmake_gmock + control_msgs + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp new file mode 100644 index 0000000000..1af4e1317f --- /dev/null +++ b/admittance_controller/src/admittance_controller.cpp @@ -0,0 +1,592 @@ +// Copyright (c) 2022, PickNik, Inc. +// +// 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. +// +/// \authors: Denis Stogl, Andy Zelenak, Paul Gesel + +#include "admittance_controller/admittance_controller.hpp" + +#include +#include +#include +#include +#include +#include + +#include "admittance_controller/admittance_rule_impl.hpp" +#include "geometry_msgs/msg/wrench.hpp" +#include "rcutils/logging_macros.h" +#include "tf2_ros/buffer.h" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace admittance_controller +{ +controller_interface::CallbackReturn AdmittanceController::on_init() +{ + // initialize controller config + try + { + parameter_handler_ = std::make_shared(get_node()); + admittance_ = std::make_unique(parameter_handler_); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // number of joints in controllers is fixed after initialization + num_joints_ = admittance_->parameters_.joints.size(); + + // allocate dynamic memory + last_reference_.positions.assign(num_joints_, 0.0); + last_reference_.velocities.assign(num_joints_, 0.0); + last_reference_.accelerations.assign(num_joints_, 0.0); + last_commanded_ = last_reference_; + reference_ = last_reference_; + reference_admittance_ = last_reference_; + joint_state_ = last_reference_; + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration AdmittanceController::command_interface_configuration() + const +{ + std::vector command_interfaces_config_names; + for (const auto & interface : admittance_->parameters_.command_interfaces) + { + for (const auto & joint : command_joint_names_) + { + auto full_name = joint + "/" + interface; + command_interfaces_config_names.push_back(full_name); + } + } + + return { + controller_interface::interface_configuration_type::INDIVIDUAL, + command_interfaces_config_names}; +} + +controller_interface::InterfaceConfiguration AdmittanceController::state_interface_configuration() + const +{ + std::vector state_interfaces_config_names; + for (size_t i = 0; i < admittance_->parameters_.state_interfaces.size(); ++i) + { + const auto & interface = admittance_->parameters_.state_interfaces[i]; + for (const auto & joint : admittance_->parameters_.joints) + { + auto full_name = joint + "/" + interface; + state_interfaces_config_names.push_back(full_name); + } + } + + auto ft_interfaces = force_torque_sensor_->get_state_interface_names(); + state_interfaces_config_names.insert( + state_interfaces_config_names.end(), ft_interfaces.begin(), ft_interfaces.end()); + + return { + controller_interface::interface_configuration_type::INDIVIDUAL, state_interfaces_config_names}; +} + +std::vector +AdmittanceController::on_export_reference_interfaces() +{ + // create CommandInterface interfaces that other controllers will be able to chain with + if (!admittance_) + { + return {}; + } + + std::vector chainable_command_interfaces; + const auto num_chainable_interfaces = + admittance_->parameters_.chainable_command_interfaces.size() * + admittance_->parameters_.joints.size(); + + // allocate dynamic memory + chainable_command_interfaces.reserve(num_chainable_interfaces); + reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); + position_reference_ = {}; + velocity_reference_ = {}; + + // assign reference interfaces + auto index = 0ul; + for (const auto & interface : allowed_reference_interfaces_types_) + { + for (const auto & joint : admittance_->parameters_.joints) + { + if (hardware_interface::HW_IF_POSITION == interface) + position_reference_.emplace_back(reference_interfaces_[index]); + else if (hardware_interface::HW_IF_VELOCITY == interface) + { + velocity_reference_.emplace_back(reference_interfaces_[index]); + } + const auto full_name = joint + "/" + interface; + chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface( + std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index)); + + index++; + } + } + + return chainable_command_interfaces; +} + +controller_interface::CallbackReturn AdmittanceController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + try + { + parameter_handler_ = std::make_shared(get_node()); + admittance_ = std::make_unique(parameter_handler_); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + command_joint_names_ = admittance_->parameters_.command_joints; + if (command_joint_names_.empty()) + { + command_joint_names_ = admittance_->parameters_.joints; + RCLCPP_INFO( + get_node()->get_logger(), + "No specific joint names are used for command interfaces. Using 'joints' parameter."); + } + else if (command_joint_names_.size() != num_joints_) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "'command_joints' parameter has to have the same size as 'joints' parameter."); + return CallbackReturn::FAILURE; + } + + // print and validate interface types + for (const auto & tmp : admittance_->parameters_.state_interfaces) + { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("state int types are: " + tmp + "\n").c_str()); + } + for (const auto & tmp : admittance_->parameters_.command_interfaces) + { + RCLCPP_INFO(get_node()->get_logger(), "%s", ("command int types are: " + tmp + "\n").c_str()); + } + + // validate exported interfaces + for (const auto & tmp : admittance_->parameters_.chainable_command_interfaces) + { + if (tmp == hardware_interface::HW_IF_POSITION || tmp == hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_INFO( + get_node()->get_logger(), "%s", ("chainable int types are: " + tmp + "\n").c_str()); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "chainable interface type %s is not supported. Supported types are %s and %s", tmp.c_str(), + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY); + return controller_interface::CallbackReturn::ERROR; + } + } + + // Check if only allowed interface types are used and initialize storage to avoid memory + // allocation during activation + auto contains_interface_type = + [](const std::vector & interface_type_list, const std::string & interface_type) + { + return std::find(interface_type_list.begin(), interface_type_list.end(), interface_type) != + interface_type_list.end(); + }; + + joint_command_interface_.resize(allowed_interface_types_.size()); + for (const auto & interface : admittance_->parameters_.command_interfaces) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + if (it == allowed_interface_types_.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Command interface type '%s' not allowed!", interface.c_str()); + return CallbackReturn::FAILURE; + } + } + + has_position_command_interface_ = contains_interface_type( + admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_POSITION); + has_velocity_command_interface_ = contains_interface_type( + admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_VELOCITY); + has_acceleration_command_interface_ = contains_interface_type( + admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_ACCELERATION); + has_effort_command_interface_ = contains_interface_type( + admittance_->parameters_.command_interfaces, hardware_interface::HW_IF_EFFORT); + + // Check if only allowed interface types are used and initialize storage to avoid memory + // allocation during activation + joint_state_interface_.resize(allowed_interface_types_.size()); + for (const auto & interface : admittance_->parameters_.state_interfaces) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + if (it == allowed_interface_types_.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "State interface type '%s' not allowed!", interface.c_str()); + return CallbackReturn::FAILURE; + } + } + + has_position_state_interface_ = contains_interface_type( + admittance_->parameters_.state_interfaces, hardware_interface::HW_IF_POSITION); + has_velocity_state_interface_ = contains_interface_type( + admittance_->parameters_.state_interfaces, hardware_interface::HW_IF_VELOCITY); + has_acceleration_state_interface_ = contains_interface_type( + admittance_->parameters_.state_interfaces, hardware_interface::HW_IF_ACCELERATION); + + auto get_interface_list = [](const std::vector & interface_types) + { + std::stringstream ss_command_interfaces; + for (size_t index = 0; index < interface_types.size(); ++index) + { + if (index != 0) + { + ss_command_interfaces << " "; + } + ss_command_interfaces << interface_types[index]; + } + return ss_command_interfaces.str(); + }; + RCLCPP_INFO( + get_node()->get_logger(), "Command interfaces are [%s] and and state interfaces are [%s].", + get_interface_list(admittance_->parameters_.command_interfaces).c_str(), + get_interface_list(admittance_->parameters_.state_interfaces).c_str()); + + // setup subscribers and publishers + auto joint_command_callback = + [this](const std::shared_ptr msg) + { input_joint_command_.writeFromNonRT(msg); }; + input_joint_command_subscriber_ = + get_node()->create_subscription( + "~/joint_references", rclcpp::SystemDefaultsQoS(), joint_command_callback); + s_publisher_ = get_node()->create_publisher( + "~/status", rclcpp::SystemDefaultsQoS()); + state_publisher_ = + std::make_unique>(s_publisher_); + + // Initialize state message + state_publisher_->lock(); + state_publisher_->msg_ = admittance_->get_controller_state(); + state_publisher_->unlock(); + + // Initialize FTS semantic semantic_component + force_torque_sensor_ = std::make_unique( + semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name)); + + // configure admittance rule + if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR) + { + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn AdmittanceController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // on_activate is called when the lifecycle node activates. + if (!admittance_) + { + return controller_interface::CallbackReturn::ERROR; + } + + // order all joints in the storage + for (const auto & interface : admittance_->parameters_.state_interfaces) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto index = std::distance(allowed_interface_types_.begin(), it); + if (!controller_interface::get_ordered_interfaces( + state_interfaces_, admittance_->parameters_.joints, interface, + joint_state_interface_[index])) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", num_joints_, + interface.c_str(), joint_state_interface_[index].size()); + return CallbackReturn::ERROR; + } + } + for (const auto & interface : admittance_->parameters_.command_interfaces) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto index = std::distance(allowed_interface_types_.begin(), it); + if (!controller_interface::get_ordered_interfaces( + command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", num_joints_, + interface.c_str(), joint_command_interface_[index].size()); + return CallbackReturn::ERROR; + } + } + + // update parameters if any have changed + admittance_->apply_parameters_update(); + + // initialize interface of the FTS semantic component + force_torque_sensor_->assign_loaned_state_interfaces(state_interfaces_); + + // initialize states + read_state_from_hardware(joint_state_, ft_values_); + for (auto val : joint_state_.positions) + { + if (std::isnan(val)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to read joint positions from the hardware.\n"); + return controller_interface::CallbackReturn::ERROR; + } + } + + // Use current joint_state as a default reference + last_reference_ = joint_state_; + last_commanded_ = joint_state_; + reference_ = joint_state_; + reference_admittance_ = joint_state_; + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type AdmittanceController::update_reference_from_subscribers() +{ + // update input reference from ros subscriber message + if (!admittance_) + { + return controller_interface::return_type::ERROR; + } + + joint_command_msg_ = *input_joint_command_.readFromRT(); + + // if message exists, load values into references + if (joint_command_msg_.get()) + { + for (size_t i = 0; i < joint_command_msg_->positions.size(); ++i) + { + position_reference_[i].get() = joint_command_msg_->positions[i]; + } + for (size_t i = 0; i < joint_command_msg_->velocities.size(); ++i) + { + velocity_reference_[i].get() = joint_command_msg_->velocities[i]; + } + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceController::update_and_write_commands( + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +{ + // Realtime constraints are required in this function + if (!admittance_) + { + return controller_interface::return_type::ERROR; + } + + // update input reference from chainable interfaces + read_state_reference_interfaces(reference_); + + // get all controller inputs + read_state_from_hardware(joint_state_, ft_values_); + + // apply admittance control to reference to determine desired state + admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_); + + // write calculated values to joint interfaces + write_state_to_hardware(reference_admittance_); + + // Publish controller state + state_publisher_->lock(); + state_publisher_->msg_ = admittance_->get_controller_state(); + state_publisher_->unlockAndPublish(); + + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn AdmittanceController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (!admittance_) + { + return controller_interface::CallbackReturn::ERROR; + } + + // release force torque sensor interface + force_torque_sensor_->release_interfaces(); + + // reset to prevent stale references + for (size_t i = 0; i < num_joints_; i++) + { + position_reference_[i].get() = std::numeric_limits::quiet_NaN(); + velocity_reference_[i].get() = std::numeric_limits::quiet_NaN(); + } + + for (size_t index = 0; index < allowed_interface_types_.size(); ++index) + { + joint_command_interface_[index].clear(); + joint_state_interface_[index].clear(); + } + release_interfaces(); + admittance_->reset(num_joints_); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn AdmittanceController::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn AdmittanceController::on_error( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (!admittance_) + { + return controller_interface::CallbackReturn::ERROR; + } + admittance_->reset(num_joints_); + return controller_interface::CallbackReturn::SUCCESS; +} + +void AdmittanceController::read_state_from_hardware( + trajectory_msgs::msg::JointTrajectoryPoint & state_current, + geometry_msgs::msg::Wrench & ft_values) +{ + // if any interface has nan values, assume state_current is the last command state + bool nan_position = false; + bool nan_velocity = false; + bool nan_acceleration = false; + + size_t pos_ind = 0; + size_t vel_ind = pos_ind + has_velocity_command_interface_; + size_t acc_ind = vel_ind + has_acceleration_state_interface_; + for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) + { + if (has_position_state_interface_) + { + state_current.positions[joint_ind] = + state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value(); + nan_position |= std::isnan(state_current.positions[joint_ind]); + } + else if (has_velocity_state_interface_) + { + state_current.velocities[joint_ind] = + state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value(); + nan_velocity |= std::isnan(state_current.velocities[joint_ind]); + } + else if (has_acceleration_state_interface_) + { + state_current.accelerations[joint_ind] = + state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value(); + nan_acceleration |= std::isnan(state_current.accelerations[joint_ind]); + } + } + + if (nan_position) + { + state_current.positions = last_commanded_.positions; + } + if (nan_velocity) + { + state_current.velocities = last_commanded_.velocities; + } + if (nan_acceleration) + { + state_current.accelerations = last_commanded_.accelerations; + } + + // if any ft_values are nan, assume values are zero + force_torque_sensor_->get_values_as_message(ft_values); + if ( + std::isnan(ft_values.force.x) || std::isnan(ft_values.force.y) || + std::isnan(ft_values.force.z) || std::isnan(ft_values.torque.x) || + std::isnan(ft_values.torque.y) || std::isnan(ft_values.torque.z)) + { + ft_values = geometry_msgs::msg::Wrench(); + } +} + +void AdmittanceController::write_state_to_hardware( + const trajectory_msgs::msg::JointTrajectoryPoint & state_commanded) +{ + // if any interface has nan values, assume state_commanded is the last command state + size_t pos_ind = 0; + size_t vel_ind = pos_ind + has_velocity_command_interface_; + size_t acc_ind = vel_ind + has_acceleration_state_interface_; + for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) + { + if (has_position_command_interface_) + { + command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value( + state_commanded.positions[joint_ind]); + } + else if (has_velocity_command_interface_) + { + command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value( + state_commanded.positions[joint_ind]); + } + else if (has_acceleration_command_interface_) + { + command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value( + state_commanded.positions[joint_ind]); + } + } + last_commanded_ = state_commanded; +} + +void AdmittanceController::read_state_reference_interfaces( + trajectory_msgs::msg::JointTrajectoryPoint & state_reference) +{ + // TODO(destogl): check why is this here? + + // if any interface has nan values, assume state_reference is the last set reference + for (size_t i = 0; i < num_joints_; ++i) + { + // update position + if (std::isnan(position_reference_[i])) + { + position_reference_[i].get() = last_reference_.positions[i]; + } + state_reference.positions[i] = position_reference_[i]; + + // update velocity + if (std::isnan(velocity_reference_[i])) + { + velocity_reference_[i].get() = last_reference_.velocities[i]; + } + state_reference.velocities[i] = velocity_reference_[i]; + } + + last_reference_.positions = state_reference.positions; + last_reference_.velocities = state_reference.velocities; +} + +} // namespace admittance_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + admittance_controller::AdmittanceController, controller_interface::ChainableControllerInterface) diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml new file mode 100644 index 0000000000..5bdc40e398 --- /dev/null +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -0,0 +1,159 @@ +admittance_controller: + joints: { + type: string_array, + description: "Specifies which joints will be used by the controller. ", + read_only: true + } + command_joints: { + type: string_array, + default_value: [], + description: "(optional) Specifies the joints for writing into another controllers reference. This parameter is only relevant when chaining the output of this controller to the input of another controller.", + read_only: true + } + command_interfaces: + { + type: string_array, + description: "Specifies which command interfaces the controller will claim.", + read_only: true + } + + state_interfaces: + { + type: string_array, + description: "Specifies which state interfaces the controller will claim.", + read_only: true + } + + chainable_command_interfaces: + { + type: string_array, + description: "Specifies which reference interfaces the controller will export. Normally, the position and velocity are used.", + read_only: true + } + + kinematics: + plugin_name: { + type: string, + description: "Specifies the name of the kinematics plugin to load." + } + plugin_package: { + type: string, + description: "Specifies the package name that contains the kinematics plugin." + } + base: { + type: string, + description: "Specifies the base link of the robot description used by the kinematics plugin." + } + tip: { + type: string, + description: "Specifies the end effector link of the robot description used by the kinematics plugin." + } + alpha: { + type: double, + default_value: 0.01, + description: "Specifies the damping coefficient for the Jacobian pseudo inverse." + } + + ft_sensor: + name: { + type: string, + description: "Specifies the name of the force torque sensor in the robot description which will be used in the admittance calculation." + } + frame: + id: { + type: string, + description: "Specifies the frame/link name of the force torque sensor." + } + filter_coefficient: { + type: double, + default_value: 0.05, + description: "Specifies the filter coefficient for the sensor's exponential filter." + } + + control: + frame: + id: { + type: string, + description: "Specifies the control frame used for admittance calculation." + } + + fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) + frame: + id: { + type: string, + description: "Specifies the world frame use for admittance calculation. Gravity must point down in this frame." + } + + gravity_compensation: + frame: + id: { + type: string, + description: "Specifies the frame which center of gravity (CoG) is defined in. Normally, the force torque sensor frame should be used." + } + CoG: + pos: { + type: double_array, + description: "Specifies the position of the center of gravity (CoG) of the end effector in the gravity compensation frame.", + validation: { + fixed_size<>: 3 + } + } + force: { + type: double, + default_value: 0.0, + description: "Specifies the weight of the end effector, e.g mass * 9.81." + } + + admittance: + selected_axes: + { + type: bool_array, + description: "Specifies whether the axes x, y, z, rx, ry, and rz should be included in the admittance calculation.", + validation: { + fixed_size<>: 6 + } + } + mass: { + type: double_array, + description: "Specifies the mass values for x, y, z, rx, ry, and rz used in the admittance calculation.", + validation: { + fixed_size<>: 6, + element_bounds<>: [ 0.0001, 1000000.0 ] + } + } + damping_ratio: { + type: double_array, + description: "Specifies damping ratio values for x, y, z, rx, ry, and rz used in the admittance calculation. + The damping ratio is defined as: zeta = D / (2 * sqrt( M * S )).", + validation: { + fixed_size<>: 6 + } + } + stiffness: { + type: double_array, + description: "Specifies the stiffness values for x, y, z, rx, ry, and rz used in the admittance calculation.", + validation: { + fixed_size<>: 6, + element_bounds<>: [ 0.0, 100000000.0 ] + } + } + joint_damping: { + type: double, + description: "Specifies the joint damping applied used in the admittance calculation.", + default_value: 5, + validation: { + lower_bounds: [ 0.0 ] + } + } + + # general settings + robot_description: { + type: string, + description: "Contains robot description in URDF format. The description is used for forward and inverse kinematics.", + read_only: true + } + enable_parameter_update_without_reactivation: { + type: bool, + default_value: true, + description: "If enabled, the parameters will be dynamically updated while the controller is running." + } diff --git a/admittance_controller/test/6d_robot_description.hpp b/admittance_controller/test/6d_robot_description.hpp new file mode 100644 index 0000000000..f67b5bd054 --- /dev/null +++ b/admittance_controller/test/6d_robot_description.hpp @@ -0,0 +1,313 @@ +// Copyright (c) 2021, 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 ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ +#define ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ + +#include + +namespace ros2_control_test_assets +{ +const auto valid_6d_robot_urdf = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ros2_control_kuka_demo_driver/KukaSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + +)"; + +const auto valid_6d_robot_srdf = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +} // namespace ros2_control_test_assets + +#endif // ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp new file mode 100644 index 0000000000..d056e0406c --- /dev/null +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -0,0 +1,292 @@ +// Copyright (c) 2021, 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. +// +/// \author: Denis Stogl + +#include "test_admittance_controller.hpp" + +#include +#include +#include +#include + +// Test on_configure returns ERROR when a required parameter is missing +TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_parameter_is_missing) +{ + ASSERT_EQ(SetUpController(GetParam()), controller_interface::return_type::ERROR); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); +} + +INSTANTIATE_TEST_SUITE_P( + MissingMandatoryParameterDuringConfiguration, + AdmittanceControllerTestParameterizedMissingParameters, + ::testing::Values( + "admittance.mass", "admittance.selected_axes", "admittance.stiffness", + "chainable_command_interfaces", "command_interfaces", "control.frame.id", + "fixed_world_frame.frame.id", "ft_sensor.frame.id", "ft_sensor.name", + "gravity_compensation.CoG.pos", "gravity_compensation.frame.id", "joints", "kinematics.base", + "kinematics.plugin_name", "kinematics.plugin_package", "kinematics.tip", "state_interfaces")); + +INSTANTIATE_TEST_SUITE_P( + InvalidParameterDuringConfiguration, AdmittanceControllerTestParameterizedInvalidParameters, + ::testing::Values( + // wrong length COG + std::make_tuple( + std::string("gravity_compensation.CoG.pos"), + rclcpp::ParameterValue(std::vector() = {1, 2, 3, 4})), + // wrong length stiffness + std::make_tuple( + std::string("admittance.stiffness"), + rclcpp::ParameterValue(std::vector() = {1, 2, 3})), + // negative stiffness + std::make_tuple( + std::string("admittance.stiffness"), + rclcpp::ParameterValue(std::vector() = {-1, -2, 3, 4, 5, 6})), + // wrong length mass + std::make_tuple( + std::string("admittance.mass"), rclcpp::ParameterValue(std::vector() = {1, 2, 3})), + // negative mass + std::make_tuple( + std::string("admittance.mass"), + rclcpp::ParameterValue(std::vector() = {-1, -2, 3, 4, 5, 6})), + // wrong length damping ratio + std::make_tuple( + std::string("admittance.damping_ratio"), + rclcpp::ParameterValue(std::vector() = {1, 2, 3})), + // wrong length selected axes + std::make_tuple( + std::string("admittance.selected_axes"), + rclcpp::ParameterValue(std::vector() = {1, 2, 3})), + // invalid robot description + std::make_tuple( + std::string("robot_description"), rclcpp::ParameterValue(std::string() = "bad_robot")))); + +TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) +{ + auto result = SetUpController(); + + ASSERT_EQ(result, controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_TRUE(!controller_->admittance_->parameters_.joints.empty()); + ASSERT_TRUE(controller_->admittance_->parameters_.joints.size() == joint_names_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.joints.begin(), + controller_->admittance_->parameters_.joints.end(), joint_names_.begin(), joint_names_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.command_interfaces.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.command_interfaces.size() == + command_interface_types_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.command_interfaces.begin(), + controller_->admittance_->parameters_.command_interfaces.end(), + command_interface_types_.begin(), command_interface_types_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.state_interfaces.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.state_interfaces.size() == state_interface_types_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.state_interfaces.begin(), + controller_->admittance_->parameters_.state_interfaces.end(), state_interface_types_.begin(), + state_interface_types_.end())); + + ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.name, ft_sensor_name_); + ASSERT_EQ(controller_->admittance_->parameters_.kinematics.base, ik_base_frame_); + ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.frame.id, sensor_frame_); + + ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.selected_axes.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.admittance.selected_axes.size() == + admittance_selected_axes_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.admittance.selected_axes.begin(), + controller_->admittance_->parameters_.admittance.selected_axes.end(), + admittance_selected_axes_.begin(), admittance_selected_axes_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.mass.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.admittance.mass.size() == admittance_mass_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.admittance.mass.begin(), + controller_->admittance_->parameters_.admittance.mass.end(), admittance_mass_.begin(), + admittance_mass_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.damping_ratio.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.admittance.damping_ratio.size() == + admittance_damping_ratio_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.admittance.damping_ratio.begin(), + controller_->admittance_->parameters_.admittance.damping_ratio.end(), + admittance_damping_ratio_.begin(), admittance_damping_ratio_.end())); + + ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.stiffness.empty()); + ASSERT_TRUE( + controller_->admittance_->parameters_.admittance.stiffness.size() == + admittance_stiffness_.size()); + ASSERT_TRUE(std::equal( + controller_->admittance_->parameters_.admittance.stiffness.begin(), + controller_->admittance_->parameters_.admittance.stiffness.end(), admittance_stiffness_.begin(), + admittance_stiffness_.end())); +} + +TEST_F(AdmittanceControllerTest, check_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_interfaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_interfaces.names.size(), joint_command_values_.size()); + + ASSERT_EQ( + controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); + + auto state_interfaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_interfaces.names.size(), joint_state_values_.size() + fts_state_values_.size()); + + ASSERT_EQ( + controller_->state_interfaces_.size(), + state_interface_types_.size() * joint_names_.size() + fts_state_values_.size()); +} + +TEST_F(AdmittanceControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ( + controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(AdmittanceControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + broadcast_tfs(); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(AdmittanceControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(AdmittanceControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + assign_interfaces(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + broadcast_tfs(); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(AdmittanceControllerTest, publish_status_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + broadcast_tfs(); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + // // Check that wrench command are all zero since not used + // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + // ASSERT_EQ(msg.wrench_base.wrench.force.x, 0.0); + // ASSERT_EQ(msg.wrench_base.wrench.force.y, 0.0); + // ASSERT_TRUE(msg.wrench_base.wrench.force.z > 0.15); + // ASSERT_TRUE(msg.wrench_base.wrench.torque.x != 0.0); + // ASSERT_TRUE(msg.wrench_base.wrench.torque.y != 0.0); + // ASSERT_EQ(msg.wrench_base.wrench.torque.z, 0.0); + + // // Check joint command message + // for (auto i = 0ul; i < joint_names_.size(); i++) + // { + // ASSERT_EQ(joint_names_[i], msg.joint_state.name[i]); + // ASSERT_FALSE(std::isnan(msg.joint_state.position[i])); + // ASSERT_FALSE(std::isnan(msg.joint_state.velocity[i])); + // ASSERT_FALSE(std::isnan(msg.joint_state.effort[i])); + // } +} + +TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + broadcast_tfs(); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // After first update state, commanded position should be near the start state + for (auto i = 0ul; i < joint_state_values_.size(); i++) + { + ASSERT_NEAR(joint_state_values_[i], joint_command_values_[i], COMMON_THRESHOLD); + } + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + broadcast_tfs(); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR(joint_command_values_[0], joint_state_values_[0], COMMON_THRESHOLD); + + subscribe_and_get_messages(msg); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} + +// Add test, wrong interfaces diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp new file mode 100644 index 0000000000..be78f05bb9 --- /dev/null +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -0,0 +1,463 @@ +// Copyright (c) 2021, 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. +// +/// \author: Denis Stogl + +#ifndef TEST_ADMITTANCE_CONTROLLER_HPP_ +#define TEST_ADMITTANCE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" + +#include "6d_robot_description.hpp" +#include "admittance_controller/admittance_controller.hpp" +#include "control_msgs/msg/admittance_controller_state.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "semantic_components/force_torque_sensor.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "trajectory_msgs/msg/joint_trajectory.hpp" + +// TODO(anyone): replace the state and command message types +using ControllerCommandWrenchMsg = geometry_msgs::msg::WrenchStamped; +using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped; +using ControllerCommandJointMsg = trajectory_msgs::msg::JointTrajectoryPoint; +using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; + +namespace +{ +const double COMMON_THRESHOLD = 0.001; + +constexpr auto NODE_SUCCESS = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + +rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) +{ + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + const auto timeout = std::chrono::seconds(10); + return wait_set.wait(timeout).kind(); +} + +} // namespace + +// subclassing and friending so we can access member variables +class TestableAdmittanceController : public admittance_controller::AdmittanceController +{ + FRIEND_TEST(AdmittanceControllerTest, joint_names_parameter_not_set); + FRIEND_TEST(AdmittanceControllerTest, interface_parameter_not_set); + FRIEND_TEST(AdmittanceControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(AdmittanceControllerTest, check_interfaces); + FRIEND_TEST(AdmittanceControllerTest, activate_success); + FRIEND_TEST(AdmittanceControllerTest, receive_message_and_publish_updated_status); + +public: + CallbackReturn on_init() override + { + get_node()->declare_parameter("robot_description", rclcpp::ParameterType::PARAMETER_STRING); + get_node()->declare_parameter( + "robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING); + get_node()->set_parameter({"robot_description", robot_description_}); + get_node()->set_parameter({"robot_description_semantic", robot_description_semantic_}); + + return admittance_controller::AdmittanceController::on_init(); + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override + { + auto ret = admittance_controller::AdmittanceController::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) + { + input_pose_command_subscriber_wait_set_.add_subscription(input_joint_command_subscriber_); + } + return ret; + } + + /** + * @brief wait_for_commands blocks until a new ControllerCommandMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerCommandMsg msg was received, false if timeout. + */ + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = + input_pose_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + + if (success) + { + executor.spin_some(); + } + return success; + } + +private: + rclcpp::WaitSet input_wrench_command_subscriber_wait_set_; + rclcpp::WaitSet input_pose_command_subscriber_wait_set_; + const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; + const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; +}; + +class AdmittanceControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + // rclcpp::init(0, nullptr); + } + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + force_command_publisher_ = + command_publisher_node_->create_publisher( + "/test_admittance_controller/force_references", rclcpp::SystemDefaultsQoS()); + // pose_command_publisher_ =command_publisher_node_->create_publisher( + // "/test_admittance_controller/pose_commands", rclcpp::SystemDefaultsQoS()); + joint_command_publisher_ = command_publisher_node_->create_publisher( + "/test_admittance_controller/joint_references", rclcpp::SystemDefaultsQoS()); + + test_subscription_node_ = std::make_shared("test_subscription_node"); + test_broadcaster_node_ = std::make_shared("test_broadcaster_node"); + } + + static void TearDownTestCase() + { + // rclcpp::shutdown(); + } + + void TearDown() { controller_.reset(nullptr); } + +protected: + controller_interface::return_type SetUpController( + const std::string & controller_name, const std::vector & parameter_overrides) + { + auto options = rclcpp::NodeOptions() + .allow_undeclared_parameters(false) + .parameter_overrides(parameter_overrides) + .automatically_declare_parameters_from_overrides(true); + return SetUpControllerCommon(controller_name, options); + } + + controller_interface::return_type SetUpController( + const std::string & controller_name = "test_admittance_controller") + { + auto options = rclcpp::NodeOptions() + .allow_undeclared_parameters(false) + .automatically_declare_parameters_from_overrides(true); + return SetUpControllerCommon(controller_name, options); + } + + controller_interface::return_type SetUpControllerCommon( + const std::string & controller_name, const rclcpp::NodeOptions & options) + { + auto result = controller_->init(controller_name, "", options); + + controller_->export_reference_interfaces(); + assign_interfaces(); + + return result; + } + + void assign_interfaces() + { + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + for (auto i = 0u; i < joint_command_values_.size(); ++i) + { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], command_interface_types_[0], &joint_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + auto sc_fts = semantic_components::ForceTorqueSensor(ft_sensor_name_); + fts_state_names_ = sc_fts.get_state_interface_names(); + std::vector state_ifs; + + const size_t num_state_ifs = joint_state_values_.size() + fts_state_names_.size(); + state_itfs_.reserve(num_state_ifs); + state_ifs.reserve(num_state_ifs); + + for (auto i = 0u; i < joint_state_values_.size(); ++i) + { + state_itfs_.emplace_back(hardware_interface::StateInterface( + joint_names_[i], state_interface_types_[0], &joint_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + std::vector fts_itf_names = {"force.x", "force.y", "force.z", + "torque.x", "torque.y", "torque.z"}; + + for (auto i = 0u; i < fts_state_names_.size(); ++i) + { + state_itfs_.emplace_back(hardware_interface::StateInterface( + ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void broadcast_tfs() + { + static tf2_ros::TransformBroadcaster br(test_broadcaster_node_); + geometry_msgs::msg::TransformStamped transform_stamped; + + transform_stamped.header.stamp = test_broadcaster_node_->now(); + transform_stamped.header.frame_id = fixed_world_frame_; + transform_stamped.transform.translation.x = 1.3; + transform_stamped.transform.translation.y = 0.5; + transform_stamped.transform.translation.z = 0.5; + transform_stamped.transform.rotation.x = 0; + transform_stamped.transform.rotation.y = 0; + transform_stamped.transform.rotation.z = 0; + transform_stamped.transform.rotation.w = 1; + + transform_stamped.child_frame_id = ik_base_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.child_frame_id = ik_tip_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.header.frame_id = ik_tip_frame_; + transform_stamped.transform.translation.x = 0; + transform_stamped.transform.translation.y = 0; + transform_stamped.transform.translation.z = 0; + transform_stamped.transform.rotation.x = 0; + transform_stamped.transform.rotation.y = 0; + transform_stamped.transform.rotation.z = 0; + transform_stamped.transform.rotation.w = 1; + + transform_stamped.child_frame_id = control_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.transform.translation.z = 0.05; + transform_stamped.child_frame_id = sensor_frame_; + br.sendTransform(transform_stamped); + + transform_stamped.transform.translation.z = 0.2; + transform_stamped.child_frame_id = endeffector_frame_; + br.sendTransform(transform_stamped); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node_->create_subscription( + "/test_admittance_controller/status", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // wait for message to be passed + ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands() + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + // TODO(destogl): comment in when using unified mode + // if (controller_->admittance_->unified_mode_) { + // wait_for_topic(force_command_publisher_->get_topic_name()); + // } + + // wait_for_topic(pose_command_publisher_->get_topic_name()); + + ControllerCommandWrenchMsg force_msg; + force_msg.header.frame_id = sensor_frame_; + force_msg.wrench.force.x = 0.1; + force_msg.wrench.force.y = 0.2; + force_msg.wrench.force.z = 0.3; + force_msg.wrench.torque.x = 3.1; + force_msg.wrench.torque.y = 3.2; + force_msg.wrench.torque.z = 3.3; + + ControllerCommandPoseMsg pose_msg; + pose_msg.header.frame_id = endeffector_frame_; + pose_msg.pose.position.x = 0.1; + pose_msg.pose.position.y = 0.2; + pose_msg.pose.position.z = 0.3; + pose_msg.pose.orientation.x = 0; + pose_msg.pose.orientation.y = 0; + pose_msg.pose.orientation.z = 0; + pose_msg.pose.orientation.w = 1; + + // TODO(destogl): comment in when using unified mode + // if (controller_->admittance_->unified_mode_) { + // force_command_publisher_->publish(force_msg); + // } + // pose_command_publisher_->publish(pose_msg); + + wait_for_topic(joint_command_publisher_->get_topic_name()); + + ControllerCommandJointMsg joint_msg; + // joint_msg.joint_names = joint_names_; + trajectory_msgs::msg::JointTrajectoryPoint trajectory_point; + auto num_joints = joint_names_.size(); + trajectory_point.positions.reserve(num_joints); + trajectory_point.velocities.resize(num_joints, 0.0); + for (auto index = 0u; index < num_joints; ++index) + { + trajectory_point.positions.emplace_back(joint_state_values_[index]); + } + joint_msg = trajectory_point; + + joint_command_publisher_->publish(joint_msg); + } + +protected: + // TODO(anyone): adjust the members as needed + + // Controller-related parameters + const std::vector joint_names_ = {"joint1", "joint2", "joint3", + "joint4", "joint5", "joint6"}; + const std::vector command_interface_types_ = {"position"}; + const std::vector state_interface_types_ = {"position"}; + const std::string ft_sensor_name_ = "ft_sensor_name"; + + bool hardware_state_has_offset_ = false; + + const std::string ik_base_frame_ = "base_link"; + const std::string ik_tip_frame_ = "tool0"; + const std::string ik_group_name_ = "arm"; + // const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; + // const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; + + const std::string control_frame_ = "tool0"; + const std::string endeffector_frame_ = "endeffector_frame"; + const std::string fixed_world_frame_ = "fixed_world_frame"; + const std::string sensor_frame_ = "link_6"; + + std::array admittance_selected_axes_ = {true, true, true, true, true, true}; + std::array admittance_mass_ = {5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; + std::array admittance_damping_ratio_ = {2.828427, 2.828427, 2.828427, + 2.828427, 2.828427, 2.828427}; + std::array admittance_stiffness_ = {214.1, 214.2, 214.3, 214.4, 214.5, 214.6}; + + std::array joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + std::array joint_state_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; + std::array fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + std::vector fts_state_names_; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr force_command_publisher_; + // rclcpp::Publisher::SharedPtr pose_command_publisher_; + rclcpp::Publisher::SharedPtr joint_command_publisher_; + rclcpp::Node::SharedPtr test_subscription_node_; + rclcpp::Node::SharedPtr test_broadcaster_node_; +}; + +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class AdmittanceControllerTestParameterizedMissingParameters +: public AdmittanceControllerTest, + public ::testing::WithParamInterface +{ +public: + virtual void SetUp() + { + AdmittanceControllerTest::SetUp(); + auto node = std::make_shared("test_admittance_controller"); + overrides_ = node->get_node_parameters_interface()->get_parameter_overrides(); + } + + static void TearDownTestCase() { AdmittanceControllerTest::TearDownTestCase(); } + +protected: + controller_interface::return_type SetUpController(const std::string & remove_name) + { + std::vector parameter_overrides; + for (const auto & override : overrides_) + { + if (override.first != remove_name) + { + rclcpp::Parameter param(override.first, override.second); + parameter_overrides.push_back(param); + } + } + + return AdmittanceControllerTest::SetUpController( + "test_admittance_controller_no_overrides", parameter_overrides); + } + + std::map overrides_; +}; + +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class AdmittanceControllerTestParameterizedInvalidParameters +: public AdmittanceControllerTest, + public ::testing::WithParamInterface> +{ +public: + virtual void SetUp() { AdmittanceControllerTest::SetUp(); } + + static void TearDownTestCase() { AdmittanceControllerTest::TearDownTestCase(); } + +protected: + void SetUpController() + { + AdmittanceControllerTest::SetUpController("test_admittance_controller"); + } +}; + +#endif // TEST_ADMITTANCE_CONTROLLER_HPP_ diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp new file mode 100644 index 0000000000..1f290aeff6 --- /dev/null +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2021, PickNik, Inc. +// +// 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. + +#include + +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadAdmittanceController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW( + cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController")); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml new file mode 100644 index 0000000000..22cbda2df9 --- /dev/null +++ b/admittance_controller/test/test_params.yaml @@ -0,0 +1,111 @@ +load_admittance_controller: + # contains minimal parameters that need to be set to load controller + ros__parameters: + joints: + - joint1 + - joint2 + + command_interfaces: + - velocity + + state_interfaces: + - position + - velocity + + chainable_command_interfaces: + - position + - velocity + + +test_admittance_controller: + # contains minimal needed parameters for kuka_kr6 + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + + command_interfaces: + - position + + state_interfaces: + - position + + chainable_command_interfaces: + - position + - velocity + + kinematics: + plugin_name: kinematics_interface_kdl/KinematicsInterfaceKDL + plugin_package: kinematics_interface + base: base_link # Assumed to be stationary + tip: tool0 + group_name: arm + alpha: 0.0005 + + ft_sensor: + name: ft_sensor_name + frame: + id: link_6 # tool0 Wrench measurements are in this frame + external: false # force torque frame exists within URDF kinematic chain + filter_coefficient: 0.005 + + control: + frame: + id: tool0 # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + external: false # control frame exists within URDF kinematic chain + + fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) + frame: + id: base_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + external: false # control frame exists within URDF kinematic chain + + gravity_compensation: + frame: + id: tool0 + external: false + + CoG: # specifies the center of gravity of the end effector + pos: + - 0.1 # x + - 0.0 # y + - 0.0 # z + force: 23.0 # mass * 9.81 + + admittance: + selected_axes: + - true # x + - true # y + - true # z + - true # rx + - true # ry + - true # rz + + # Having ".0" at the end is MUST, otherwise there is a loading error + # F = M*a + D*v + S*(x - x_d) + mass: + - 5.5 + - 6.6 + - 7.7 + - 8.8 + - 9.9 + - 10.10 + + damping_ratio: # damping can be used instead: zeta = D / (2 * sqrt( M * S )) + - 2.828427 # x + - 2.828427 # y + - 2.828427 # z + - 2.828427 # rx + - 2.828427 # ry + - 2.828427 # rz + + stiffness: + - 214.1 + - 214.2 + - 214.3 + - 214.4 + - 214.5 + - 214.6 diff --git a/ros2_controllers-not-released.humble.repos b/ros2_controllers-not-released.humble.repos index 1b3910e7e7..b9cdead153 100644 --- a/ros2_controllers-not-released.humble.repos +++ b/ros2_controllers-not-released.humble.repos @@ -4,3 +4,7 @@ repositories: # type: git # url: git@github.com:/.git # version: master + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: humble diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index 1b3910e7e7..b9cdead153 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -4,3 +4,7 @@ repositories: # type: git # url: git@github.com:/.git # version: master + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: humble diff --git a/ros2_controllers.foxy.repos b/ros2_controllers.foxy.repos index 09ec6e5387..d9a1c841a5 100644 --- a/ros2_controllers.foxy.repos +++ b/ros2_controllers.foxy.repos @@ -11,7 +11,3 @@ repositories: type: git url: https://github.com/ros-controls/realtime_tools.git version: foxy-devel - angles: - type: git - url: https://github.com/ros/angles.git - version: ros2 diff --git a/ros2_controllers.galactic.repos b/ros2_controllers.galactic.repos index 2011f0c31a..b9b8674fc9 100644 --- a/ros2_controllers.galactic.repos +++ b/ros2_controllers.galactic.repos @@ -11,7 +11,3 @@ repositories: type: git url: https://github.com/ros-controls/realtime_tools.git version: foxy-devel - angles: - type: git - url: https://github.com/ros/angles.git - version: ros2 diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index 3a667e7d9a..8c0ba1ff8a 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -3,11 +3,19 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: master - control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: galactic-devel realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git version: master + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: humble + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index 6f9dc84a3f..f62ba87bf3 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -3,15 +3,20 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: master - control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: galactic-devel realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git version: master - generate_parameter_library: + control_msgs: type: git - url: https://github.com/picknikrobotics/generate_parameter_library.git - version: main + url: https://github.com/ros-controls/control_msgs.git + version: humble + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master + + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master From eecb7f7af8cd750b9b4116947ae31fb5df52dcfd Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Tue, 11 Oct 2022 15:42:50 -0600 Subject: [PATCH 045/126] Fix parameter library export (#448) --- admittance_controller/CMakeLists.txt | 39 +++++-------------- .../CMakeLists.txt | 13 +++---- force_torque_sensor_broadcaster/package.xml | 3 +- joint_trajectory_controller/CMakeLists.txt | 17 ++++---- joint_trajectory_controller/package.xml | 2 +- 5 files changed, 24 insertions(+), 50 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index bbfe19deda..a6dcf35dce 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -38,15 +38,15 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library(${PROJECT_NAME} SHARED src/admittance_controller.cpp) -target_include_directories(${PROJECT_NAME} PRIVATE include) -generate_parameter_library(${PROJECT_NAME}_parameters src/admittance_controller_parameters.yaml) -target_link_libraries(${PROJECT_NAME} admittance_controller_parameters) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +add_library(admittance_controller SHARED src/admittance_controller.cpp) +target_include_directories(admittance_controller PRIVATE include) +generate_parameter_library(admittance_controller_parameters src/admittance_controller_parameters.yaml) +target_link_libraries(admittance_controller admittance_controller_parameters) +ament_target_dependencies(admittance_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "ADMITTANCE_CONTROLLER_BUILDING_DLL") +target_compile_definitions(admittance_controller PRIVATE "ADMITTANCE_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) @@ -54,7 +54,8 @@ install(DIRECTORY include/ DESTINATION include ) -install(TARGETS ${PROJECT_NAME} +install(TARGETS admittance_controller admittance_controller_parameters + EXPORT export_admittance_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -68,23 +69,6 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ## create custom test function to pass yaml file into test main - #function(add_test_with_yaml_input TARGET SOURCES YAML_FILE) - #add_executable(${TARGET} ${SOURCES}) - #_ament_cmake_gmock_find_gmock() - #target_include_directories(${TARGET} PUBLIC "${GMOCK_INCLUDE_DIRS}") - #target_link_libraries(${TARGET} ${GMOCK_LIBRARIES}) - #set(executable "$") - #set(result_file "${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.gtest.xml") - #ament_add_test( - #${TARGET} - #COMMAND ${executable} --ros-args --params-file ${YAML_FILE} - #--gtest_output=xml:${result_file} - #OUTPUT_FILE ${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.txt - #RESULT_FILE ${result_file} - #) - #endfunction() - # test loading admittance controller add_rostest_with_parameters_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) @@ -110,13 +94,10 @@ if(BUILD_TESTING) ) endif() -ament_export_include_directories( - include +ament_export_targets( + export_admittance_controller HAS_LIBRARY_TARGET ) ament_export_dependencies( ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -ament_export_libraries( - ${PROJECT_NAME} -) ament_package() diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index cdb1fe2b35..3aa57d4aad 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -13,6 +13,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface + generate_parameter_library geometry_msgs hardware_interface pluginlib @@ -26,7 +27,6 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -find_package(generate_parameter_library REQUIRED) generate_parameter_library(force_torque_sensor_broadcaster_parameters src/force_torque_sensor_broadcaster_parameters.yaml ) @@ -54,8 +54,8 @@ pluginlib_export_plugin_description_file( controller_interface force_torque_sensor_broadcaster.xml) install( - TARGETS - ${PROJECT_NAME} + TARGETS force_torque_sensor_broadcaster force_torque_sensor_broadcaster_parameters + EXPORT export_force_torque_sensor_broadcaster RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -101,11 +101,8 @@ if(BUILD_TESTING) ) endif() -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} +ament_export_targets( + export_force_torque_sensor_broadcaster HAS_LIBRARY_TARGET ) ament_export_dependencies( ${THIS_PACKAGE_INCLUDE_DEPENDS} diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index bf1e01bde7..0e105ceb2c 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -11,8 +11,6 @@ ament_cmake - generate_parameter_library - controller_interface geometry_msgs hardware_interface @@ -20,6 +18,7 @@ rclcpp rclcpp_lifecycle realtime_tools + generate_parameter_library ament_cmake_gmock controller_manager diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 6d443c532f..32461617ac 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -13,9 +13,10 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS angles - controller_interface control_msgs control_toolbox + controller_interface + generate_parameter_library hardware_interface pluginlib rclcpp @@ -29,8 +30,6 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -find_package(generate_parameter_library REQUIRED) - generate_parameter_library(joint_trajectory_controller_parameters src/joint_trajectory_controller_parameters.yaml include/joint_trajectory_controller/validate_jtc_parameters.hpp @@ -52,7 +51,8 @@ install(DIRECTORY include/ DESTINATION include ) -install(TARGETS ${PROJECT_NAME} +install(TARGETS joint_trajectory_controller joint_trajectory_controller_parameters + EXPORT export_joint_trajectory_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -105,13 +105,10 @@ if(BUILD_TESTING) # ) endif() +ament_export_targets( + export_joint_trajectory_controller HAS_LIBRARY_TARGET +) ament_export_dependencies( ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} -) ament_package() diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index b3a828efe2..f768c82a8f 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -12,7 +12,6 @@ ament_cmake angles - generate_parameter_library pluginlib realtime_tools @@ -27,6 +26,7 @@ rclcpp rclcpp_lifecycle trajectory_msgs + generate_parameter_library ament_cmake_gmock controller_manager From eccea9cef5f5dbebe1f5fda68d1af0e371d000d9 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 13 Oct 2022 10:24:10 -0600 Subject: [PATCH 046/126] Generate params for ForwardCommandController (#396) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Generate params for ForwardCommandController Signed-off-by: Tyler Weaver * Clean up CMake Signed-off-by: Tyler Weaver Co-authored-by: Denis Štogl Co-authored-by: Bence Magyar --- forward_command_controller/CMakeLists.txt | 39 ++++++++++++------- .../forward_command_controller.hpp | 6 ++- ...i_interface_forward_command_controller.hpp | 9 ++++- forward_command_controller/package.xml | 1 + .../src/forward_command_controller.cpp | 22 +++++------ ...forward_command_controller_parameters.yaml | 11 ++++++ ...i_interface_forward_command_controller.cpp | 20 ++++++---- ...forward_command_controller_parameters.yaml | 11 ++++++ ros2_controllers-not-released.rolling.repos | 11 ++---- ros2_controllers.rolling.repos | 8 ++++ 10 files changed, 93 insertions(+), 45 deletions(-) create mode 100644 forward_command_controller/src/forward_command_controller_parameters.yaml create mode 100644 forward_command_controller/src/multi_interface_forward_command_controller_parameters.yaml diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index f1d7a18328..ef8581d195 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -13,6 +13,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface + generate_parameter_library hardware_interface pluginlib rclcpp @@ -26,17 +27,30 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library(${PROJECT_NAME} +generate_parameter_library( + forward_command_controller_parameters + src/forward_command_controller_parameters.yaml +) +generate_parameter_library( + multi_interface_forward_command_controller_parameters + src/multi_interface_forward_command_controller_parameters.yaml +) + +add_library(forward_command_controller SHARED src/forward_controllers_base.cpp src/forward_command_controller.cpp src/multi_interface_forward_command_controller.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE include) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_include_directories(forward_command_controller PRIVATE include) +ament_target_dependencies(forward_command_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(forward_command_controller + forward_command_controller_parameters + multi_interface_forward_command_controller_parameters +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "FORWARD_COMMAND_CONTROLLER_BUILDING_DLL") +target_compile_definitions(forward_command_controller PRIVATE "FORWARD_COMMAND_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface forward_command_plugin.xml) install( @@ -46,10 +60,14 @@ install( install( TARGETS - ${PROJECT_NAME} + forward_command_controller + forward_command_controller_parameters + multi_interface_forward_command_controller_parameters + EXPORT export_forward_command_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + INCLUDES DESTINATION include ) if(BUILD_TESTING) @@ -75,7 +93,7 @@ if(BUILD_TESTING) ) target_include_directories(test_forward_command_controller PRIVATE include) target_link_libraries(test_forward_command_controller - ${PROJECT_NAME} + forward_command_controller ) ament_add_gmock( @@ -96,15 +114,10 @@ if(BUILD_TESTING) ) target_include_directories(test_multi_interface_forward_command_controller PRIVATE include) target_link_libraries(test_multi_interface_forward_command_controller - ${PROJECT_NAME} + forward_command_controller ) endif() +ament_export_targets(export_forward_command_controller HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} -) ament_package() diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index f50af328d8..8a697847cd 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -15,11 +15,13 @@ #ifndef FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ #define FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ +#include #include #include #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" +#include "forward_command_controller_parameters.hpp" namespace forward_command_controller { @@ -44,8 +46,8 @@ class ForwardCommandController : public ForwardControllersBase void declare_parameters() override; controller_interface::CallbackReturn read_parameters() override; - std::vector joint_names_; - std::string interface_name_; + std::shared_ptr param_listener_; + Params params_; }; } // namespace forward_command_controller diff --git a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp index 8acec36c5c..fd7c0d480e 100644 --- a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp @@ -15,11 +15,13 @@ #ifndef FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ #define FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ +#include #include #include #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" +#include "multi_interface_forward_command_controller_parameters.hpp" namespace forward_command_controller { @@ -45,8 +47,11 @@ class MultiInterfaceForwardCommandController void declare_parameters() override; controller_interface::CallbackReturn read_parameters() override; - std::string joint_name_; - std::vector interface_names_; + using Params = multi_interface_forward_command_controller::Params; + using ParamListener = multi_interface_forward_command_controller::ParamListener; + + std::shared_ptr param_listener_; + Params params_; }; } // namespace forward_command_controller diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f4c137301a..47e12892ca 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -11,6 +11,7 @@ ament_cmake controller_interface + generate_parameter_library hardware_interface rclcpp rclcpp_lifecycle diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 06c6b15120..0305a37e4e 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -29,35 +29,33 @@ ForwardCommandController::ForwardCommandController() : ForwardControllersBase() void ForwardCommandController::declare_parameters() { - auto_declare("joints", std::vector()); - auto_declare("interface_name", std::string()); + param_listener_ = std::make_shared(get_node()); } controller_interface::CallbackReturn ForwardCommandController::read_parameters() { - joint_names_ = get_node()->get_parameter("joints").as_string_array(); - - if (joint_names_.empty()) + if (!param_listener_) { - RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); return controller_interface::CallbackReturn::ERROR; } + params_ = param_listener_->get_params(); - // Specialized, child controllers set interfaces before calling configure function. - if (interface_name_.empty()) + if (params_.joints.empty()) { - interface_name_ = get_node()->get_parameter("interface_name").as_string(); + RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); + return controller_interface::CallbackReturn::ERROR; } - if (interface_name_.empty()) + if (params_.interface_name.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty"); return controller_interface::CallbackReturn::ERROR; } - for (const auto & joint : joint_names_) + for (const auto & joint : params_.joints) { - command_interface_types_.push_back(joint + "/" + interface_name_); + command_interface_types_.push_back(joint + "/" + params_.interface_name); } return controller_interface::CallbackReturn::SUCCESS; diff --git a/forward_command_controller/src/forward_command_controller_parameters.yaml b/forward_command_controller/src/forward_command_controller_parameters.yaml new file mode 100644 index 0000000000..c633fab268 --- /dev/null +++ b/forward_command_controller/src/forward_command_controller_parameters.yaml @@ -0,0 +1,11 @@ +forward_command_controller: + joints: { + type: string_array, + default_value: [], + description: "Name of the joints to control", + } + interface_name: { + type: string, + default_value: "", + description: "Name of the interface to command", + } diff --git a/forward_command_controller/src/multi_interface_forward_command_controller.cpp b/forward_command_controller/src/multi_interface_forward_command_controller.cpp index 8faacd0776..06fe395cb4 100644 --- a/forward_command_controller/src/multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/src/multi_interface_forward_command_controller.cpp @@ -14,6 +14,7 @@ #include "forward_command_controller/multi_interface_forward_command_controller.hpp" +#include #include #include @@ -26,30 +27,33 @@ MultiInterfaceForwardCommandController::MultiInterfaceForwardCommandController() void MultiInterfaceForwardCommandController::declare_parameters() { - auto_declare("joint", joint_name_); - auto_declare("interface_names", interface_names_); + param_listener_ = std::make_shared(get_node()); } controller_interface::CallbackReturn MultiInterfaceForwardCommandController::read_parameters() { - joint_name_ = get_node()->get_parameter("joint").as_string(); - interface_names_ = get_node()->get_parameter("interface_names").as_string_array(); + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); - if (joint_name_.empty()) + if (params_.joint.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "'joint' parameter is empty"); return controller_interface::CallbackReturn::ERROR; } - if (interface_names_.empty()) + if (params_.interface_names.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "'interfaces' parameter is empty"); return controller_interface::CallbackReturn::ERROR; } - for (const auto & interface : interface_names_) + for (const auto & interface : params_.interface_names) { - command_interface_types_.push_back(joint_name_ + "/" + interface); + command_interface_types_.push_back(params_.joint + "/" + interface); } return controller_interface::CallbackReturn::SUCCESS; diff --git a/forward_command_controller/src/multi_interface_forward_command_controller_parameters.yaml b/forward_command_controller/src/multi_interface_forward_command_controller_parameters.yaml new file mode 100644 index 0000000000..d1eb1cacc9 --- /dev/null +++ b/forward_command_controller/src/multi_interface_forward_command_controller_parameters.yaml @@ -0,0 +1,11 @@ +multi_interface_forward_command_controller: + joint: { + type: string, + default_value: "", + description: "Name of the joint to control", + } + interface_names: { + type: string_array, + default_value: [], + description: "Names of the interfaces to command", + } diff --git a/ros2_controllers-not-released.rolling.repos b/ros2_controllers-not-released.rolling.repos index b9cdead153..66352f4960 100644 --- a/ros2_controllers-not-released.rolling.repos +++ b/ros2_controllers-not-released.rolling.repos @@ -1,10 +1,5 @@ repositories: - ## EXAMPLE DEPENDENCY -# : -# type: git -# url: git@github.com:/.git -# version: master - control_msgs: + generate_parameter_library: type: git - url: https://github.com/ros-controls/control_msgs.git - version: humble + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index f62ba87bf3..dfed17059b 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -20,3 +20,11 @@ repositories: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master + angles: + type: git + url: https://github.com/ros/angles.git + version: ros2 + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main From 0cfa8046a808bd16b54b171f0f694209eb919544 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Thu, 20 Oct 2022 23:22:29 -0600 Subject: [PATCH 047/126] Generate parameters for IMU Sensor Broadcaster (#399) --- imu_sensor_broadcaster/CMakeLists.txt | 61 ++++++++++--------- .../imu_sensor_broadcaster.hpp | 5 +- imu_sensor_broadcaster/package.xml | 1 + .../src/imu_sensor_broadcaster.cpp | 18 +++--- .../imu_sensor_broadcaster_parameters.yaml | 11 ++++ 5 files changed, 55 insertions(+), 41 deletions(-) create mode 100644 imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 5febe1c048..e49fb65ad3 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -13,12 +13,13 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface - sensor_msgs + generate_parameter_library hardware_interface pluginlib rclcpp rclcpp_lifecycle realtime_tools + sensor_msgs ) find_package(ament_cmake REQUIRED) @@ -26,37 +27,29 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(imu_sensor_broadcaster_parameters + src/imu_sensor_broadcaster_parameters.yaml +) + add_library( - ${PROJECT_NAME} - SHARED + imu_sensor_broadcaster SHARED src/imu_sensor_broadcaster.cpp ) -target_include_directories( - ${PROJECT_NAME} - PUBLIC - include +target_include_directories(imu_sensor_broadcaster + PRIVATE $ + PRIVATE $ ) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(imu_sensor_broadcaster ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "IMU_SENSOR_BROADCASTER_BUILDING_DLL") +target_compile_definitions(imu_sensor_broadcaster PRIVATE "IMU_SENSOR_BROADCASTER_BUILDING_DLL") +target_link_libraries(imu_sensor_broadcaster + imu_sensor_broadcaster_parameters +) pluginlib_export_plugin_description_file( controller_interface imu_sensor_broadcaster.xml) -install( - TARGETS - ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install( - DIRECTORY include/ - DESTINATION include -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) @@ -80,7 +73,7 @@ if(BUILD_TESTING) ) target_include_directories(test_imu_sensor_broadcaster PRIVATE include) target_link_libraries(test_imu_sensor_broadcaster - ${PROJECT_NAME} + imu_sensor_broadcaster ) ament_target_dependencies(test_imu_sensor_broadcaster hardware_interface @@ -92,14 +85,22 @@ if(BUILD_TESTING) ) endif() -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} +install( + DIRECTORY include/ + DESTINATION include ) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} + +install( + TARGETS + imu_sensor_broadcaster + imu_sensor_broadcaster_parameters + EXPORT export_imu_sensor_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include ) +ament_export_targets(export_imu_sensor_broadcaster HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index 11310b5b70..d78e28780e 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -25,6 +25,7 @@ #include "controller_interface/controller_interface.hpp" #include "imu_sensor_broadcaster/visibility_control.h" +#include "imu_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" @@ -61,8 +62,8 @@ class IMUSensorBroadcaster : public controller_interface::ControllerInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: - std::string sensor_name_; - std::string frame_id_; + std::shared_ptr param_listener_; + Params params_; std::unique_ptr imu_sensor_; diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index bc9d930830..855d3fe58e 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -12,6 +12,7 @@ ament_cmake controller_interface + generate_parameter_library hardware_interface pluginlib rclcpp diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index b609bca8c7..e488d13638 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -27,8 +27,8 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_init() { try { - auto_declare("sensor_name", ""); - auto_declare("frame_id", ""); + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -43,22 +43,22 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_init() controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - sensor_name_ = get_node()->get_parameter("sensor_name").as_string(); - if (sensor_name_.empty()) + + params_ = param_listener_->get_params(); + if (params_.sensor_name.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "'sensor_name' parameter has to be specified."); return CallbackReturn::ERROR; } - frame_id_ = get_node()->get_parameter("frame_id").as_string(); - if (frame_id_.empty()) + if (params_.frame_id.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); return CallbackReturn::ERROR; } - imu_sensor_ = - std::make_unique(semantic_components::IMUSensor(sensor_name_)); + imu_sensor_ = std::make_unique( + semantic_components::IMUSensor(params_.sensor_name)); try { // register ft sensor data publisher @@ -75,7 +75,7 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( } realtime_publisher_->lock(); - realtime_publisher_->msg_.header.frame_id = frame_id_; + realtime_publisher_->msg_.header.frame_id = params_.frame_id; realtime_publisher_->unlock(); RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml new file mode 100644 index 0000000000..613d42c02d --- /dev/null +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml @@ -0,0 +1,11 @@ +imu_sensor_broadcaster: + sensor_name: { + type: string, + default_value: "", + description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined.", + } + frame_id: { + type: string, + default_value: "", + description: "Sensor's frame_id in which values are published.", + } From 3f9f55038c0125dcc4a1fc831fc34e2fc20e383f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 21 Oct 2022 07:44:44 +0200 Subject: [PATCH 048/126] [IMU Broadcaster] Added parameters for definition of static covariances. (#455) --- .../src/imu_sensor_broadcaster.cpp | 10 ++++++- .../imu_sensor_broadcaster_parameters.yaml | 24 +++++++++++++++ .../test/test_imu_sensor_broadcaster.cpp | 29 ++++++++++++------- 3 files changed, 51 insertions(+), 12 deletions(-) diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index e488d13638..3e2d574f0f 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -43,7 +43,6 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_init() controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - params_ = param_listener_->get_params(); if (params_.sensor_name.empty()) { @@ -76,6 +75,15 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( realtime_publisher_->lock(); realtime_publisher_->msg_.header.frame_id = params_.frame_id; + // convert double vector to fixed-size array in the message + for (size_t i = 0; i < 9; ++i) + { + realtime_publisher_->msg_.orientation_covariance[i] = params_.static_covariance_orientation[i]; + realtime_publisher_->msg_.angular_velocity_covariance[i] = + params_.static_covariance_angular_velocity[i]; + realtime_publisher_->msg_.linear_acceleration_covariance[i] = + params_.static_covariance_linear_acceleration[i]; + } realtime_publisher_->unlock(); RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml index 613d42c02d..c0daba9f11 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml @@ -9,3 +9,27 @@ imu_sensor_broadcaster: default_value: "", description: "Sensor's frame_id in which values are published.", } + static_covariance_orientation: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Static orientation covariance. Row major about x, y, z axes", + validation: { + fixed_size<>: [9], + } + } + static_covariance_angular_velocity: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Static angular velocity covariance. Row major about x, y, z axes", + validation: { + fixed_size<>: [9], + } + } + static_covariance_linear_acceleration: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Static linear acceleration covariance. Row major about x, y, z axes", + validation: { + fixed_size<>: [9], + } + } diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index cc1398a9b8..31c8d9e2f0 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -189,15 +189,22 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success) sensor_msgs::msg::Imu imu_msg; subscribe_and_get_message(imu_msg); - ASSERT_EQ(imu_msg.header.frame_id, frame_id_); - ASSERT_EQ(imu_msg.orientation.x, sensor_values_[0]); - ASSERT_EQ(imu_msg.orientation.y, sensor_values_[1]); - ASSERT_EQ(imu_msg.orientation.z, sensor_values_[2]); - ASSERT_EQ(imu_msg.orientation.w, sensor_values_[3]); - ASSERT_EQ(imu_msg.angular_velocity.x, sensor_values_[4]); - ASSERT_EQ(imu_msg.angular_velocity.y, sensor_values_[5]); - ASSERT_EQ(imu_msg.angular_velocity.z, sensor_values_[6]); - ASSERT_EQ(imu_msg.linear_acceleration.x, sensor_values_[7]); - ASSERT_EQ(imu_msg.linear_acceleration.y, sensor_values_[8]); - ASSERT_EQ(imu_msg.linear_acceleration.z, sensor_values_[9]); + EXPECT_EQ(imu_msg.header.frame_id, frame_id_); + EXPECT_EQ(imu_msg.orientation.x, sensor_values_[0]); + EXPECT_EQ(imu_msg.orientation.y, sensor_values_[1]); + EXPECT_EQ(imu_msg.orientation.z, sensor_values_[2]); + EXPECT_EQ(imu_msg.orientation.w, sensor_values_[3]); + EXPECT_EQ(imu_msg.angular_velocity.x, sensor_values_[4]); + EXPECT_EQ(imu_msg.angular_velocity.y, sensor_values_[5]); + EXPECT_EQ(imu_msg.angular_velocity.z, sensor_values_[6]); + EXPECT_EQ(imu_msg.linear_acceleration.x, sensor_values_[7]); + EXPECT_EQ(imu_msg.linear_acceleration.y, sensor_values_[8]); + EXPECT_EQ(imu_msg.linear_acceleration.z, sensor_values_[9]); + + for (size_t i = 0; i < 9; ++i) + { + EXPECT_EQ(imu_msg.orientation_covariance[i], 0.0); + EXPECT_EQ(imu_msg.angular_velocity_covariance[i], 0.0); + EXPECT_EQ(imu_msg.linear_acceleration_covariance[i], 0.0); + } } From 908b7cc7ed5413d3fbb09203fd5edc4fc24abdd0 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Sat, 22 Oct 2022 06:54:18 +0900 Subject: [PATCH 049/126] [doc] Remove Controllers which is redundant with Available Controllers (#456) --- doc/controllers_index.rst | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 23feac42d7..dee9d31abe 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -20,18 +20,10 @@ The controllers' namespaces are commanding the following command interface types - ``effort_controllers``: ``hardware_interface::HW_IF_EFFORT`` - ... - -Controllers -*********** - -The following standard controllers are implemented: - - - `Joint Trajectory Controller `_ - provided a list of waypoints or target point defined with position, velocity and acceleration, the controller interpolates joint trajectories through it. - - ... ... - .. _common hardware interface definitions: https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp + Guidelines and Best Practices ***************************** @@ -43,7 +35,7 @@ Guidelines and Best Practices Available Controllers -===================== +********************* .. toctree:: :titlesonly: @@ -57,7 +49,7 @@ Available Controllers Available Broadcasters -====================== +********************** .. toctree:: :titlesonly: From 5c0cb9210b2230347af12bbe2690032f6942435d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 24 Oct 2022 09:48:11 +0200 Subject: [PATCH 050/126] Write detailed Diff-Drive-Controller documentation to make all the interfaces understandable. (#371) --- diff_drive_controller/doc/userdoc.rst | 132 ++++++++++++++++++++++++++ 1 file changed, 132 insertions(+) diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index fee26db2cd..dc158d7ff4 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -24,3 +24,135 @@ Other features Odometry publishing Task-space velocity, acceleration and jerk limits Automatic stop after command time-out + + +ros2_control Interfaces +------------------------ + +References +,,,,,,,,,,, + + +States +,,,,,,, + + +Commands +,,,,,,,,, + + +ROS2 Interfaces +---------------- + +Subscribers +,,,,,,,,,,,, +~/cmd_vel [geometry_msgs/msg/TwistStamped] + Velocity command for the controller. + +~/cmd_vel_unstamped [geometry_msgs::msg::Twist] + +~/cmd_vel_out [] + + + + +Publishers +,,,,,,,,,,, +~/odom [] + +/tf + + +Services +,,,,,,,,, + + +Parameters +------------ + +Wheels +,,,,,,, +left_wheel_names [array] + Link names of the left side wheels. + +right_wheel_names [array] + Link names of the right side wheels. + +wheel_separation [double] + Shortest distance between the left and right wheels. + If this parameter is wrong, the robot will not behave correctly in curves. + +wheels_per_side [integer] + Number of wheels on each wide of the robot. + This is important to take the wheels slip into account when multiple wheels on each side are present. + If there are more wheels then control signals for each side, you should enter number or control signals. + For example, Husky has two wheels on each side, but they use one control signal, in this case "1" is the correct value of the parameter. + +wheel_radius [double] + Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. + If this parameter is wrong the robot will move faster or slower then expected. + +wheel_separation_multiplier [double] + Correction factor for wheel separation (TODO(destogl): Please help me describe this correctly) + +left_wheel_radius_multiplier [double] + Correction factor when radius of left wheels differs from the nominal value in ``wheel_radius`` parameter. + +right_wheel_radius_multiplier [double] + Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter. + +Odometry +,,,,,,,,, +base_frame_id [string] (default: "base_link") + Name of the frame for odometry. + This frame is parent of ``base_frame_id`` when controller publishes odometry. + +odom_frame_id [string] (default: "odom") + Name of the robot's base frame that is child of the odometry frame. + +pose_covariance_diagonal [array[6]] (default: "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]") + Odometry covariance for the encoder output of the robot for the pose. + These values should be tuned to your robot's sample odometry data, but these values are a good place to start: + ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``. + +twist_covariance_diagonal [array[6]] (default: "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]") + Odometry covariance for the encoder output of the robot for the speed. + These values should be tuned to your robot's sample odometry data, but these values are a good place to start: + ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``. + +open_loop [bool] (default: "false") + If set to false the odometry of the robot will be calculated from the commanded values and not from feedback. + +position_feedback [bool] (default: "true") + Is there position feedback from hardware. + +enable_odom_tf [bool] (default: "true") + Publish transformation between ``odom_frame_id`` and ``base_frame_id``. + +velocity_rolling_window_size [int] (default: "10") + (TODO(destogl): Please help me describe this correctly) + +Commands +``````````` +cmd_vel_timeout [double] (default: "0.5s") + Timeout after which input command on ``cmd_vel`` topic is considered staled. + +publish_limited_velocity [double] (default: "false") + Publish limited velocity value. + + +use_stamped_vel [bool] (default: "true") + Use stamp from input velocity message to calculate how old the command actually is. + +linear.x [JointLimits structure] + Joint limits structure for the linear X-axis. + The limiter ignores position limits. + For details see ``joint_limits`` package from ros2_control repository. + +angular.z [JointLimits structure] + Joint limits structure for the rotation about Z-axis. + The limiter ignores position limits. + For details see ``joint_limits`` package from ros2_control repository. + +publish_rate [double] (default: "50.0") + Publishing rate (Hz) of the odometry and TF messages. From a238100114f626ee17411673a87e8ab41863d6c9 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Tue, 1 Nov 2022 01:46:35 -0600 Subject: [PATCH 051/126] Generate parameters for Gripper Action (#398) --- gripper_controllers/CMakeLists.txt | 41 +++++++---- .../gripper_action_controller.hpp | 10 +-- .../gripper_action_controller_impl.hpp | 69 ++++++++----------- gripper_controllers/package.xml | 1 + .../gripper_action_controller_parameters.yaml | 43 ++++++++++++ 5 files changed, 102 insertions(+), 62 deletions(-) create mode 100644 gripper_controllers/src/gripper_action_controller_parameters.yaml diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index 40ca2fdd32..aa203709a0 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -17,14 +17,15 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp_action control_msgs control_toolbox controller_interface + generate_parameter_library hardware_interface pluginlib - realtime_tools rclcpp + rclcpp_action + realtime_tools ) find_package(ament_cmake REQUIRED) @@ -32,6 +33,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(gripper_action_controller_parameters + src/gripper_action_controller_parameters.yaml +) + add_library(gripper_action_controller SHARED src/gripper_action_controller.cpp ) @@ -40,18 +45,10 @@ target_include_directories(gripper_action_controller PRIVATE $ PRIVATE $ ) -pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) -############# -## Install ## -############# - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include -) - -install(TARGETS gripper_action_controller - LIBRARY DESTINATION lib +target_link_libraries(gripper_action_controller + gripper_action_controller_parameters ) +pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) @@ -68,4 +65,22 @@ if(BUILD_TESTING) ) endif() +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + gripper_action_controller + gripper_action_controller_parameters + EXPORT export_gripper_action_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + +ament_export_targets(export_gripper_action_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index d5999002f4..78120b0cf5 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -43,6 +43,7 @@ #include "realtime_tools/realtime_server_goal_handle.h" // Project +#include "gripper_action_controller_parameters.hpp" #include "gripper_controllers/hardware_interface_adapter.hpp" namespace gripper_action_controller @@ -130,7 +131,8 @@ class GripperActionController : public controller_interface::ControllerInterface std::experimental::optional> joint_velocity_state_interface_; - std::string joint_name_; ///< Controlled joint names. + std::shared_ptr param_listener_; + Params params_; HwIfaceAdapter hw_iface_adapter_; ///< Adapts desired goal state to HW interface. @@ -158,12 +160,6 @@ class GripperActionController : public controller_interface::ControllerInterface rclcpp::Time last_movement_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); ///< Store stall time double computed_command_; ///< Computed command - double stall_timeout_, - stall_velocity_threshold_; ///< Stall related parameters - double default_max_effort_; ///< Max allowed effort - double goal_tolerance_; - bool allow_stalling_; ///< If gripper stalls when moving to goal is considered successful - /** * \brief Check for success and publish appropriate result and feedback. **/ diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 6fb9d61359..85f28d102b 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -41,14 +41,8 @@ controller_interface::CallbackReturn GripperActionController: { try { - // with the lifecycle node being initialized, we can declare parameters - auto_declare("action_monitor_rate", 20.0); - joint_name_ = auto_declare("joint", joint_name_); - auto_declare("goal_tolerance", 0.01); - auto_declare("max_effort", 0.0); - auto_declare("allow_stalling", false); - auto_declare("stall_velocity_threshold", 0.001); - auto_declare("stall_timeout", 1.0); + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -145,7 +139,7 @@ template void GripperActionController::set_hold_position() { command_struct_.position_ = joint_position_state_interface_->get().get_value(); - command_struct_.max_effort_ = default_max_effort_; + command_struct_.max_effort_ = params_.max_effort; command_.writeFromNonRT(command_struct_); } @@ -159,7 +153,7 @@ void GripperActionController::check_for_success( return; } - if (fabs(error_position) < goal_tolerance_) + if (fabs(error_position) < params_.goal_tolerance) { pre_alloc_result_->effort = computed_command_; pre_alloc_result_->position = current_position; @@ -171,17 +165,18 @@ void GripperActionController::check_for_success( } else { - if (fabs(current_velocity) > stall_velocity_threshold_) + if (fabs(current_velocity) > params_.stall_velocity_threshold) { last_movement_time_ = time; } - else if ((time - last_movement_time_).seconds() > stall_timeout_) + else if ((time - last_movement_time_).seconds() > params_.stall_timeout) { pre_alloc_result_->effort = computed_command_; pre_alloc_result_->position = current_position; pre_alloc_result_->reached_goal = false; pre_alloc_result_->stalled = true; - if (allow_stalling_) + + if (params_.allow_stalling) { RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Returning success."); rt_active_goal_->setSucceeded(pre_alloc_result_); @@ -201,35 +196,25 @@ controller_interface::CallbackReturn GripperActionController: const rclcpp_lifecycle::State &) { const auto logger = get_node()->get_logger(); + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); // Action status checking update rate - const auto action_monitor_rate = get_node()->get_parameter("action_monitor_rate").as_double(); - action_monitor_period_ = rclcpp::Duration::from_seconds( - 1.0 / get_node()->get_parameter("action_monitor_rate").as_double()); + action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / params_.action_monitor_rate); RCLCPP_INFO_STREAM( - logger, "Action status changes will be monitored at " << action_monitor_rate << "Hz."); + logger, "Action status changes will be monitored at " << params_.action_monitor_rate << "Hz."); // Controlled joint - joint_name_ = get_node()->get_parameter("joint").as_string(); - if (joint_name_.empty()) + if (params_.joint.empty()) { RCLCPP_ERROR(logger, "Could not find joint name on param server"); return controller_interface::CallbackReturn::ERROR; } - // Default tolerances - goal_tolerance_ = get_node()->get_parameter("goal_tolerance").as_double(); - goal_tolerance_ = fabs(goal_tolerance_); - // Max allowable effort - default_max_effort_ = get_node()->get_parameter("max_effort").as_double(); - default_max_effort_ = fabs(default_max_effort_); - // Allow stalling will make the action server return success if the - // gripper stalls when moving to the goal - allow_stalling_ = get_node()->get_parameter("allow_stalling").as_bool(); - // Stall - stall velocity threshold, stall timeout - stall_velocity_threshold_ = get_node()->get_parameter("stall_velocity_threshold").as_double(); - stall_timeout_ = get_node()->get_parameter("stall_timeout").as_double(); - return controller_interface::CallbackReturn::SUCCESS; } template @@ -245,12 +230,12 @@ controller_interface::CallbackReturn GripperActionController: RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position command interface"); return controller_interface::CallbackReturn::ERROR; } - if (position_command_interface_it->get_prefix_name() != joint_name_) + if (position_command_interface_it->get_prefix_name() != params_.joint) { RCLCPP_ERROR_STREAM( get_node()->get_logger(), "Position command interface is different than joint name `" << position_command_interface_it->get_prefix_name() << "` != `" - << joint_name_ << "`"); + << params_.joint << "`"); return controller_interface::CallbackReturn::ERROR; } const auto position_state_interface_it = std::find_if( @@ -262,12 +247,12 @@ controller_interface::CallbackReturn GripperActionController: RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position state interface"); return controller_interface::CallbackReturn::ERROR; } - if (position_state_interface_it->get_prefix_name() != joint_name_) + if (position_state_interface_it->get_prefix_name() != params_.joint) { RCLCPP_ERROR_STREAM( get_node()->get_logger(), "Position state interface is different than joint name `" << position_state_interface_it->get_prefix_name() << "` != `" - << joint_name_ << "`"); + << params_.joint << "`"); return controller_interface::CallbackReturn::ERROR; } const auto velocity_state_interface_it = std::find_if( @@ -279,12 +264,12 @@ controller_interface::CallbackReturn GripperActionController: RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 velocity state interface"); return controller_interface::CallbackReturn::ERROR; } - if (velocity_state_interface_it->get_prefix_name() != joint_name_) + if (velocity_state_interface_it->get_prefix_name() != params_.joint) { RCLCPP_ERROR_STREAM( get_node()->get_logger(), "Velocity command interface is different than joint name `" << velocity_state_interface_it->get_prefix_name() << "` != `" - << joint_name_ << "`"); + << params_.joint << "`"); return controller_interface::CallbackReturn::ERROR; } @@ -297,7 +282,7 @@ controller_interface::CallbackReturn GripperActionController: // Command - non RT version command_struct_.position_ = joint_position_state_interface_->get().get_value(); - command_struct_.max_effort_ = default_max_effort_; + command_struct_.max_effort_ = params_.max_effort; command_.initRT(command_struct_); // Result @@ -334,7 +319,7 @@ GripperActionController::command_interface_configuration() co { return { controller_interface::interface_configuration_type::INDIVIDUAL, - {joint_name_ + "/" + hardware_interface::HW_IF_POSITION}}; + {params_.joint + "/" + hardware_interface::HW_IF_POSITION}}; } template @@ -343,8 +328,8 @@ GripperActionController::state_interface_configuration() cons { return { controller_interface::interface_configuration_type::INDIVIDUAL, - {joint_name_ + "/" + hardware_interface::HW_IF_POSITION, - joint_name_ + "/" + hardware_interface::HW_IF_VELOCITY}}; + {params_.joint + "/" + hardware_interface::HW_IF_POSITION, + params_.joint + "/" + hardware_interface::HW_IF_VELOCITY}}; } template diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 1f5ae7ceba..46916ba584 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -17,6 +17,7 @@ control_msgs control_toolbox controller_interface + generate_parameter_library hardware_interface rclcpp rclcpp_action diff --git a/gripper_controllers/src/gripper_action_controller_parameters.yaml b/gripper_controllers/src/gripper_action_controller_parameters.yaml new file mode 100644 index 0000000000..9027e5b730 --- /dev/null +++ b/gripper_controllers/src/gripper_action_controller_parameters.yaml @@ -0,0 +1,43 @@ +gripper_action_controller: + action_monitor_rate: { + type: double, + default_value: 20.0, + description: "Hz", + validation: { + lower_bounds: [0.1] + }, + } + joint: { + type: string, + default_value: "", + } + goal_tolerance: { + type: double, + default_value: 0.01, + validation: { + lower_bounds: [0.0] + }, + } + max_effort: { + type: double, + default_value: 0.0, + description: "Max allowable effort", + validation: { + lower_bounds: [0.0] + }, + } + allow_stalling: { + type: bool, + description: "Allow stalling will make the action server return success if the gripper stalls when moving to the goal", + default_value: false, + } + stall_velocity_threshold: { + type: double, + description: "stall velocity threshold", + default_value: 0.001, + } + stall_timeout: { + type: double, + description: "stall timeout", + default_value: 1.0, + } From 12160de6d9c132b99b6c6f1cf975f6296006db7c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 2 Nov 2022 18:18:33 +0000 Subject: [PATCH 052/126] Use optional from C++17 (#460) --- .../gripper_action_controller.hpp | 8 +++----- .../gripper_action_controller_impl.hpp | 6 +++--- .../hardware_interface_adapter.hpp | 16 +++++----------- 3 files changed, 11 insertions(+), 19 deletions(-) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 78120b0cf5..c219c7bc09 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -22,8 +22,6 @@ #include #include #include -// TODO(JafarAbdi): Remove experimental once the default standard is C++17 -#include "experimental/optional" // ROS #include "rclcpp/rclcpp.hpp" @@ -124,11 +122,11 @@ class GripperActionController : public controller_interface::ControllerInterface bool verbose_ = false; ///< Hard coded verbose flag to help in debugging std::string name_; ///< Controller name. - std::experimental::optional> + std::optional> joint_position_command_interface_; - std::experimental::optional> + std::optional> joint_position_state_interface_; - std::experimental::optional> + std::optional> joint_velocity_state_interface_; std::shared_ptr param_listener_; diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 85f28d102b..423cf0861a 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -306,9 +306,9 @@ template controller_interface::CallbackReturn GripperActionController::on_deactivate( const rclcpp_lifecycle::State &) { - joint_position_command_interface_ = std::experimental::nullopt; - joint_position_state_interface_ = std::experimental::nullopt; - joint_velocity_state_interface_ = std::experimental::nullopt; + joint_position_command_interface_ = std::nullopt; + joint_position_state_interface_ = std::nullopt; + joint_velocity_state_interface_ = std::nullopt; release_interfaces(); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index 40fde4442c..97ebbb1f4b 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -23,8 +23,6 @@ #include #include #include -// TODO(JafarAbdi): Remove experimental once the default standard is C++17 -#include "experimental/optional" #include "control_toolbox/pid.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -44,7 +42,7 @@ class HardwareInterfaceAdapter { public: bool init( - std::experimental::optional< + std::optional< std::reference_wrapper> /* joint_handle */, std::shared_ptr & /* node */) { @@ -71,8 +69,7 @@ class HardwareInterfaceAdapter { public: bool init( - std::experimental::optional> - joint_handle, + std::optional> joint_handle, const rclcpp_lifecycle::LifecycleNode::SharedPtr & /* node */) { joint_handle_ = joint_handle; @@ -92,8 +89,7 @@ class HardwareInterfaceAdapter } private: - std::experimental::optional> - joint_handle_; + std::optional> joint_handle_; }; /** @@ -130,8 +126,7 @@ class HardwareInterfaceAdapter } bool init( - std::experimental::optional> - joint_handle, + std::optional> joint_handle, const std::shared_ptr & node) { joint_handle_ = joint_handle; @@ -182,8 +177,7 @@ class HardwareInterfaceAdapter private: using PidPtr = std::shared_ptr; PidPtr pid_; - std::experimental::optional> - joint_handle_; + std::optional> joint_handle_; std::chrono::steady_clock::time_point last_update_time_; }; From 39fce8974df43c398754b93f986363e92e3e8b59 Mon Sep 17 00:00:00 2001 From: sp-sophia-labs <111747728+sp-sophia-labs@users.noreply.github.com> Date: Sat, 12 Nov 2022 11:34:12 +0100 Subject: [PATCH 053/126] Odom Topic & Frame Namespaces (#461) --- .../src/diff_drive_controller.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index bfb5baf3c3..c4fec1430a 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -471,6 +471,20 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( std::make_shared>( odometry_publisher_); + std::string controller_namespace = std::string(get_node()->get_namespace()); + + if (controller_namespace == "/") + { + controller_namespace = ""; + } + else + { + controller_namespace = controller_namespace + "/"; + } + + odom_params_.odom_frame_id = controller_namespace + odom_params_.odom_frame_id; + odom_params_.base_frame_id = controller_namespace + odom_params_.base_frame_id; + auto & odometry_message = realtime_odometry_publisher_->msg_; odometry_message.header.frame_id = odom_params_.odom_frame_id; odometry_message.child_frame_id = odom_params_.base_frame_id; From 950fb26b41a899d64a11b3f310ad0c3e181afcf4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 16 Nov 2022 20:21:23 +0100 Subject: [PATCH 054/126] [AdmittanceController] Add missing dependecies for the tests (#465) We need a concrete implementation of `kinematics_interface` for tests to work. We use `kinematics_interface_kdl` implementation in the tests. --- admittance_controller/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index e352dcfa91..a1d1904c21 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -30,11 +30,11 @@ tf2_ros trajectory_msgs - ament_cmake_gmock control_msgs controller_manager hardware_interface + kinematics_interface_kdl ros2_control_test_assets From bc1ed47b50df3317a511a4539e7b7cae24ae937d Mon Sep 17 00:00:00 2001 From: light-tech Date: Fri, 18 Nov 2022 05:06:24 +0700 Subject: [PATCH 055/126] Include to fix compilation error on macOS (#467) --- tricycle_controller/src/steering_limiter.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/tricycle_controller/src/steering_limiter.cpp b/tricycle_controller/src/steering_limiter.cpp index c1649f12dc..b76cc0f602 100644 --- a/tricycle_controller/src/steering_limiter.cpp +++ b/tricycle_controller/src/steering_limiter.cpp @@ -18,6 +18,7 @@ #include #include +#include #include "rcppmath/clamp.hpp" #include "tricycle_controller/steering_limiter.hpp" From 14ea6ba3a3cd6bc1697c2d3e3054c77dca927809 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 17 Nov 2022 23:41:15 +0000 Subject: [PATCH 056/126] Bring admittance_controller version up to speed --- admittance_controller/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index a1d1904c21..3859520ae6 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,9 +2,10 @@ admittance_controller - 0.0.0 + 2.13.0 Implementation of admittance controllers for different input and output interface. Denis Štogl + Bence Magyar Andy Zelenak Apache License 2.0 From 933da0f19b9b86256209abb0e790f45100c31657 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 17 Nov 2022 23:46:07 +0000 Subject: [PATCH 057/126] Add admittance_controller to ros2_controllers metapackage --- ros2_controllers/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 125988addf..e81ed2b798 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -5,12 +5,12 @@ Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios - Karsten Knese Apache License 2.0 ament_cmake + admittance_controller diff_drive_controller effort_controllers force_torque_sensor_broadcaster From a0fc29ef138d882acba943cfc6aae25c9341d140 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 18 Nov 2022 00:36:33 +0000 Subject: [PATCH 058/126] Update changelogs --- admittance_controller/CHANGELOG.rst | 110 ++++++++++++++++++ diff_drive_controller/CHANGELOG.rst | 6 + effort_controllers/CHANGELOG.rst | 3 + force_torque_sensor_broadcaster/CHANGELOG.rst | 5 + forward_command_controller/CHANGELOG.rst | 5 + gripper_controllers/CHANGELOG.rst | 6 + imu_sensor_broadcaster/CHANGELOG.rst | 6 + joint_state_broadcaster/CHANGELOG.rst | 3 + joint_trajectory_controller/CHANGELOG.rst | 5 + position_controllers/CHANGELOG.rst | 3 + ros2_controllers/CHANGELOG.rst | 3 + ros2_controllers_test_nodes/CHANGELOG.rst | 5 + rqt_joint_trajectory_controller/CHANGELOG.rst | 3 + tricycle_controller/CHANGELOG.rst | 5 + velocity_controllers/CHANGELOG.rst | 3 + 15 files changed, 171 insertions(+) create mode 100644 admittance_controller/CHANGELOG.rst diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst new file mode 100644 index 0000000000..d8571f6303 --- /dev/null +++ b/admittance_controller/CHANGELOG.rst @@ -0,0 +1,110 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package admittance_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Bring admittance_controller version up to speed +* [AdmittanceController] Add missing dependecies for the tests (`#465 `_) + We need a concrete implementation of `kinematics_interface` for tests to work. We use `kinematics_interface_kdl` implementation in the tests. +* Fix parameter library export (`#448 `_) +* Add generic admittance controller for TCP wrenches (`#370 `_) + Co-authored-by: AndyZe + Co-authored-by: Denis Štogl +* Contributors: Bence Magyar, Denis Štogl, Paul Gesel, Tyler Weaver + +* Bring admittance_controller version up to speed +* [AdmittanceController] Add missing dependecies for the tests (`#465 `_) + We need a concrete implementation of `kinematics_interface` for tests to work. We use `kinematics_interface_kdl` implementation in the tests. +* Fix parameter library export (`#448 `_) +* Add generic admittance controller for TCP wrenches (`#370 `_) + Co-authored-by: AndyZe + Co-authored-by: Denis Štogl +* Contributors: Bence Magyar, Denis Štogl, Paul Gesel, Tyler Weaver + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 3c2714dc64..eb98ef30ee 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Odom Topic & Frame Namespaces (`#461 `_) +* Write detailed Diff-Drive-Controller documentation to make all the interfaces understandable. (`#371 `_) +* Contributors: Denis Štogl, sp-sophia-labs + 2.13.0 (2022-10-05) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index aeec4541f4..0778813310 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.13.0 (2022-10-05) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 7e285a6ac0..dee110d588 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix parameter library export (`#448 `_) +* Contributors: Tyler Weaver + 2.13.0 (2022-10-05) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 19b4711223..7b429662ca 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Generate params for ForwardCommandController (`#396 `_) +* Contributors: Tyler Weaver + 2.13.0 (2022-10-05) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index ee226c598f..dc6f15552f 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use optional from C++17 (`#460 `_) +* Generate parameters for Gripper Action (`#398 `_) +* Contributors: Bence Magyar, Tyler Weaver + 2.13.0 (2022-10-05) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 285d7e061d..eabe61732d 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [IMU Broadcaster] Added parameters for definition of static covariances. (`#455 `_) +* Generate parameters for IMU Sensor Broadcaster (`#399 `_) +* Contributors: Denis Štogl, Tyler Weaver + 2.13.0 (2022-10-05) ------------------- * Fix undeclared and wrong parameters in controllers. (`#438 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 9dc2a70a9d..1c95a97006 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.13.0 (2022-10-05) ------------------- * Generate parameters for Joint State Broadcaster (`#401 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index f7244e062d..0bec81752e 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix parameter library export (`#448 `_) +* Contributors: Tyler Weaver + 2.13.0 (2022-10-05) ------------------- * Generate Parameter Library for Joint Trajectory Controller (`#384 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index d78bfc246b..218646366f 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.13.0 (2022-10-05) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 0c194f3db8..38bd425c35 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.13.0 (2022-10-05) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index b791ac2a81..26196e3652 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove deprecation warning when parameter without value is set. (`#445 `_) +* Contributors: Denis Štogl + 2.13.0 (2022-10-05) ------------------- * Enable definition of all fields in JointTrajectory message when using test node. (`#389 `_) diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 130d482684..43faf36e42 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.13.0 (2022-10-05) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index c71becc1ce..e1195f4ebb 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Include to fix compilation error on macOS (`#467 `_) +* Contributors: light-tech + 2.13.0 (2022-10-05) ------------------- diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index cdabd8ca5e..45f0ce49fb 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.13.0 (2022-10-05) ------------------- From 755fec9580cdb74f95b443216eb35a1381226397 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 18 Nov 2022 00:37:18 +0000 Subject: [PATCH 059/126] 2.14.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index d8571f6303..1b09c945b6 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * Bring admittance_controller version up to speed * [AdmittanceController] Add missing dependecies for the tests (`#465 `_) We need a concrete implementation of `kinematics_interface` for tests to work. We use `kinematics_interface_kdl` implementation in the tests. diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 3859520ae6..34f7ba23b2 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 2.13.0 + 2.14.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index eb98ef30ee..f631cb18c6 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * Odom Topic & Frame Namespaces (`#461 `_) * Write detailed Diff-Drive-Controller documentation to make all the interfaces understandable. (`#371 `_) * Contributors: Denis Štogl, sp-sophia-labs diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 474ff1a2d8..764f52c66d 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.13.0 + 2.14.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 0778813310..95d292e041 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- 2.13.0 (2022-10-05) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index f9164998db..b1c174f2c1 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.13.0 + 2.14.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index dee110d588..d31b8cc6a8 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * Fix parameter library export (`#448 `_) * Contributors: Tyler Weaver diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 0e105ceb2c..75f2cc70dc 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.13.0 + 2.14.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 7b429662ca..04b740e499 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * Generate params for ForwardCommandController (`#396 `_) * Contributors: Tyler Weaver diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 47e12892ca..0510e8bb3a 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.13.0 + 2.14.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index dc6f15552f..c4ff69decf 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * Use optional from C++17 (`#460 `_) * Generate parameters for Gripper Action (`#398 `_) * Contributors: Bence Magyar, Tyler Weaver diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 46916ba584..2f46a5c970 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.13.0 + 2.14.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index eabe61732d..c79e3bc3fc 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * [IMU Broadcaster] Added parameters for definition of static covariances. (`#455 `_) * Generate parameters for IMU Sensor Broadcaster (`#399 `_) * Contributors: Denis Štogl, Tyler Weaver diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 855d3fe58e..6eba494c33 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.13.0 + 2.14.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 1c95a97006..44600171f6 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- 2.13.0 (2022-10-05) ------------------- diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 43687c70ea..a4523a9dc0 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.13.0 + 2.14.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 0bec81752e..deb9d68c6e 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * Fix parameter library export (`#448 `_) * Contributors: Tyler Weaver diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index f768c82a8f..f6c8984a66 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.13.0 + 2.14.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 218646366f..c199ae0ed0 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- 2.13.0 (2022-10-05) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 02d9ca7cf4..4a435fe93c 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.13.0 + 2.14.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 38bd425c35..fcfbacc0e6 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- 2.13.0 (2022-10-05) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index e81ed2b798..5836915aa1 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.13.0 + 2.14.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 26196e3652..a4feb47161 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * Remove deprecation warning when parameter without value is set. (`#445 `_) * Contributors: Denis Štogl diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index a3810d560a..3367d44c2d 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.13.0 + 2.14.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index fdc3646ad1..1f24996371 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.13.0", + version="2.14.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 43faf36e42..5a01e489c2 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- 2.13.0 (2022-10-05) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 7529c38428..faff00589e 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.13.0 + 2.14.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index d1a720d311..d635d515b0 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.13.0", + version="2.14.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index e1195f4ebb..65cc206bda 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- * Include to fix compilation error on macOS (`#467 `_) * Contributors: light-tech diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 467ed03ffc..bd5204c357 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.13.0 + 2.14.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 45f0ce49fb..beb72450a1 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.14.0 (2022-11-18) +------------------- 2.13.0 (2022-10-05) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index aecb6cf3d1..fe5dbdbb56 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.13.0 + 2.14.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 09e50139882b31a21e2c54ec1c4bf691bf58ef3d Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 27 Nov 2022 18:58:32 +0100 Subject: [PATCH 060/126] Add basic gripper controller tests (#459) --- gripper_controllers/CMakeLists.txt | 9 + .../gripper_action_controller.hpp | 2 +- .../gripper_action_controller_impl.hpp | 2 +- .../test/test_gripper_controllers.cpp | 154 ++++++++++++++++++ .../test/test_gripper_controllers.hpp | 65 ++++++++ 5 files changed, 230 insertions(+), 2 deletions(-) create mode 100644 gripper_controllers/test/test_gripper_controllers.cpp create mode 100644 gripper_controllers/test/test_gripper_controllers.hpp diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index aa203709a0..97ccd2788d 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -63,6 +63,15 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) + + ament_add_gmock( + test_gripper_controllers + test/test_gripper_controllers.cpp + ) + target_include_directories(test_gripper_controllers PRIVATE include) + target_link_libraries(test_gripper_controllers + gripper_action_controller + ) endif() install( diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index c219c7bc09..830de7b514 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -107,7 +107,7 @@ class GripperActionController : public controller_interface::ControllerInterface // pre-allocated memory that is re-used to set the realtime buffer Commands command_struct_, command_struct_rt_; -private: +protected: using GripperCommandAction = control_msgs::action::GripperCommand; using ActionServer = rclcpp_action::Server; using ActionServerPtr = ActionServer::SharedPtr; diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 423cf0861a..e28495fd1e 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -211,7 +211,7 @@ controller_interface::CallbackReturn GripperActionController: // Controlled joint if (params_.joint.empty()) { - RCLCPP_ERROR(logger, "Could not find joint name on param server"); + RCLCPP_ERROR(logger, "Joint name cannot be empty"); return controller_interface::CallbackReturn::ERROR; } diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp new file mode 100644 index 0000000000..4c82eb6961 --- /dev/null +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -0,0 +1,154 @@ +// Copyright 2020 PAL Robotics SL. +// +// 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. + +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" + +#include "test_gripper_controllers.hpp" + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using GripperCommandAction = control_msgs::action::GripperCommand; +using GoalHandle = rclcpp_action::ServerGoalHandle; + +void GripperControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } + +void GripperControllerTest::TearDownTestCase() { rclcpp::shutdown(); } + +void GripperControllerTest::SetUp() +{ + // initialize controller + controller_ = std::make_unique(); +} + +void GripperControllerTest::TearDown() { controller_.reset(nullptr); } + +void GripperControllerTest::SetUpController() +{ + const auto result = controller_->init("gripper_controller"); + ASSERT_EQ(result, controller_interface::return_type::OK); + + std::vector command_ifs; + command_ifs.emplace_back(joint_1_pos_cmd_); + std::vector state_ifs; + state_ifs.emplace_back(joint_1_pos_state_); + state_ifs.emplace_back(joint_1_vel_state_); + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); +} + +TEST_F(GripperControllerTest, ParametersNotSet) +{ + SetUpController(); + + // configure failed, 'joints' parameter not set + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, JointParameterIsEmpty) +{ + SetUpController(); + + controller_->get_node()->set_parameter({"joint", ""}); + + // configure failed, 'joints' is empty + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, ConfigureParamsSuccess) +{ + SetUpController(); + + controller_->get_node()->set_parameter({"joint", "joint_1"}); + + // configure successful + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(GripperControllerTest, ActivateWithWrongJointsNamesFails) +{ + SetUpController(); + + controller_->get_node()->set_parameter({"joint", "unicorn_joint"}); + + // activate failed, 'joint4' is not a valid joint name for the hardware + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, ActivateSuccess) +{ + SetUpController(); + + controller_->get_node()->set_parameter({"joint", "joint1"}); + + // activate successful + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(GripperControllerTest, ActivateDeactivateActivateSuccess) +{ + SetUpController(); + + controller_->get_node()->set_parameter({"joint", "joint1"}); + + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_deactivate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // re-assign interfaces + std::vector command_ifs; + command_ifs.emplace_back(joint_1_pos_cmd_); + std::vector state_ifs; + state_ifs.emplace_back(joint_1_pos_state_); + state_ifs.emplace_back(joint_1_vel_state_); + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} diff --git a/gripper_controllers/test/test_gripper_controllers.hpp b/gripper_controllers/test/test_gripper_controllers.hpp new file mode 100644 index 0000000000..3e9750a603 --- /dev/null +++ b/gripper_controllers/test/test_gripper_controllers.hpp @@ -0,0 +1,65 @@ +// Copyright 2022 ros2_control development team +// +// 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 TEST_GRIPPER_CONTROLLERS_HPP_ +#define TEST_GRIPPER_CONTROLLERS_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "gripper_controllers/gripper_action_controller.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using hardware_interface::CommandInterface; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::StateInterface; + +// subclassing and friending so we can access member variables +class FriendGripperController +: public gripper_action_controller::GripperActionController +{ + FRIEND_TEST(GripperControllerTest, CommandSuccessTest); +}; + +class GripperControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase(); + static void TearDownTestCase(); + + void SetUp(); + void TearDown(); + + void SetUpController(); + void SetUpHandles(); + +protected: + std::unique_ptr controller_; + + // dummy joint state values used for tests + const std::string joint_name_ = "joint1"; + std::vector joint_states_ = {1.1, 2.1}; + std::vector joint_commands_ = {3.1}; + + StateInterface joint_1_pos_state_{joint_name_, HW_IF_POSITION, &joint_states_[0]}; + StateInterface joint_1_vel_state_{joint_name_, HW_IF_VELOCITY, &joint_states_[1]}; + CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &joint_commands_[0]}; +}; + +#endif // TEST_GRIPPER_CONTROLLERS_HPP_ From bc7be0ee888c71662bca67eb5b57e2c5d9699a68 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 3 Dec 2022 08:07:13 +0000 Subject: [PATCH 061/126] Fix format step (#475) --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index fba9f85911..dd78cca0ed 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -14,7 +14,7 @@ jobs: - uses: actions/checkout@v3 - uses: actions/setup-python@v2 with: - python-version: 3.9.7 + python-version: 3.9 - name: Install system hooks run: sudo apt install -qq clang-format-12 cppcheck - uses: pre-commit/action@v2.0.3 From 5a367e25a310db6adbd7657539a7464af79e7d62 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Maciej=20St=C4=99pie=C5=84?= Date: Sat, 3 Dec 2022 13:11:45 +0100 Subject: [PATCH 062/126] [DiffDriveController] Change units of velocity feedback (#452) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds multiplying feedback by wheel radius so that it can be in rad/s (which should be expected unit) Co-authored-by: Maciej Stępień Co-authored-by: Bence Magyar Co-authored-by: Denis Štogl --- diff_drive_controller/src/diff_drive_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index c4fec1430a..e873aeb4f0 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -220,7 +220,8 @@ controller_interface::return_type DiffDriveController::update( else { odometry_.updateFromVelocity( - left_feedback_mean * period.seconds(), right_feedback_mean * period.seconds(), time); + left_feedback_mean * left_wheel_radius * period.seconds(), + right_feedback_mean * right_wheel_radius * period.seconds(), time); } } From bc88acbf8a2bc15030a00ab17aa4065a3bce304a Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Sun, 4 Dec 2022 04:02:52 -0500 Subject: [PATCH 063/126] [DiffDriveController] Use generate parameter library (#386) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl Co-authored-by: Bence Magyar --- diff_drive_controller/CMakeLists.txt | 9 + diff_drive_controller/doc/userdoc.rst | 78 +------ .../diff_drive_controller.hpp | 34 +-- diff_drive_controller/package.xml | 1 + .../src/diff_drive_controller.cpp | 198 +++++------------- .../src/diff_drive_controller_parameter.yaml | 177 ++++++++++++++++ 6 files changed, 250 insertions(+), 247 deletions(-) create mode 100644 diff_drive_controller/src/diff_drive_controller_parameter.yaml diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 09ca27e1d2..83f18a9931 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -30,6 +30,12 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +find_package(generate_parameter_library REQUIRED) + +generate_parameter_library(diff_drive_controller_parameters + src/diff_drive_controller_parameter.yaml +) + add_library(${PROJECT_NAME} SHARED src/diff_drive_controller.cpp src/odometry.cpp @@ -39,6 +45,9 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(${PROJECT_NAME} + diff_drive_controller_parameters +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "DIFF_DRIVE_CONTROLLER_BUILDING_DLL") diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index dc158d7ff4..1ef47bd975 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -70,79 +70,10 @@ Services Parameters ------------ -Wheels -,,,,,,, -left_wheel_names [array] - Link names of the left side wheels. - -right_wheel_names [array] - Link names of the right side wheels. - -wheel_separation [double] - Shortest distance between the left and right wheels. - If this parameter is wrong, the robot will not behave correctly in curves. - -wheels_per_side [integer] - Number of wheels on each wide of the robot. - This is important to take the wheels slip into account when multiple wheels on each side are present. - If there are more wheels then control signals for each side, you should enter number or control signals. - For example, Husky has two wheels on each side, but they use one control signal, in this case "1" is the correct value of the parameter. - -wheel_radius [double] - Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. - If this parameter is wrong the robot will move faster or slower then expected. - -wheel_separation_multiplier [double] - Correction factor for wheel separation (TODO(destogl): Please help me describe this correctly) - -left_wheel_radius_multiplier [double] - Correction factor when radius of left wheels differs from the nominal value in ``wheel_radius`` parameter. - -right_wheel_radius_multiplier [double] - Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter. - -Odometry -,,,,,,,,, -base_frame_id [string] (default: "base_link") - Name of the frame for odometry. - This frame is parent of ``base_frame_id`` when controller publishes odometry. - -odom_frame_id [string] (default: "odom") - Name of the robot's base frame that is child of the odometry frame. +Check `parameter definition file for details `_. -pose_covariance_diagonal [array[6]] (default: "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]") - Odometry covariance for the encoder output of the robot for the pose. - These values should be tuned to your robot's sample odometry data, but these values are a good place to start: - ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``. - -twist_covariance_diagonal [array[6]] (default: "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]") - Odometry covariance for the encoder output of the robot for the speed. - These values should be tuned to your robot's sample odometry data, but these values are a good place to start: - ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``. - -open_loop [bool] (default: "false") - If set to false the odometry of the robot will be calculated from the commanded values and not from feedback. - -position_feedback [bool] (default: "true") - Is there position feedback from hardware. - -enable_odom_tf [bool] (default: "true") - Publish transformation between ``odom_frame_id`` and ``base_frame_id``. - -velocity_rolling_window_size [int] (default: "10") - (TODO(destogl): Please help me describe this correctly) - -Commands -``````````` -cmd_vel_timeout [double] (default: "0.5s") - Timeout after which input command on ``cmd_vel`` topic is considered staled. - -publish_limited_velocity [double] (default: "false") - Publish limited velocity value. - - -use_stamped_vel [bool] (default: "true") - Use stamp from input velocity message to calculate how old the command actually is. +Note that the documentation on parameters for joint limits can be found in `their header file `_. +Those parameters are: linear.x [JointLimits structure] Joint limits structure for the linear X-axis. @@ -153,6 +84,3 @@ angular.z [JointLimits structure] Joint limits structure for the rotation about Z-axis. The limiter ignores position limits. For details see ``joint_limits`` package from ros2_control repository. - -publish_rate [double] (default: "50.0") - Publishing rate (Hz) of the odometry and TF messages. diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index ce357e4116..f229667fb9 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -42,6 +42,8 @@ #include "realtime_tools/realtime_publisher.h" #include "tf2_msgs/msg/tf_message.hpp" +#include "diff_drive_controller_parameters.hpp" + namespace diff_drive_controller { class DiffDriveController : public controller_interface::ControllerInterface @@ -101,35 +103,18 @@ class DiffDriveController : public controller_interface::ControllerInterface const std::string & side, const std::vector & wheel_names, std::vector & registered_handles); - std::vector left_wheel_names_; - std::vector right_wheel_names_; - std::vector registered_left_wheel_handles_; std::vector registered_right_wheel_handles_; - struct WheelParams - { - size_t wheels_per_side = 0; - double separation = 0.0; // w.r.t. the midpoint of the wheel width - double radius = 0.0; // Assumed to be the same for both wheels - double separation_multiplier = 1.0; - double left_radius_multiplier = 1.0; - double right_radius_multiplier = 1.0; - } wheel_params_; - - struct OdometryParams - { - bool open_loop = false; - bool position_feedback = true; - bool enable_odom_tf = true; - std::string base_frame_id = "base_link"; - std::string odom_frame_id = "odom"; - std::array pose_covariance_diagonal; - std::array twist_covariance_diagonal; - } odom_params_; + // Parameters from ROS for diff_drive_controller + std::shared_ptr param_listener_; + Params params_; Odometry odometry_; + // Timeout to consider cmd_vel commands old + std::chrono::milliseconds cmd_vel_timeout_{500}; + std::shared_ptr> odometry_publisher_ = nullptr; std::shared_ptr> realtime_odometry_publisher_ = nullptr; @@ -139,9 +124,6 @@ class DiffDriveController : public controller_interface::ControllerInterface std::shared_ptr> realtime_odometry_transform_publisher_ = nullptr; - // Timeout to consider cmd_vel commands old - std::chrono::milliseconds cmd_vel_timeout_{500}; - bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; rclcpp::Subscription::SharedPtr diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 764f52c66d..6b675ccc10 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -9,6 +9,7 @@ Apache License 2.0 ament_cmake + generate_parameter_library controller_interface geometry_msgs diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index e873aeb4f0..75d8b1fa35 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -50,58 +50,16 @@ DiffDriveController::DiffDriveController() : controller_interface::ControllerInt const char * DiffDriveController::feedback_type() const { - return odom_params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY; + return params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY; } controller_interface::CallbackReturn DiffDriveController::on_init() { try { - // with the lifecycle node being initialized, we can declare parameters - auto_declare>("left_wheel_names", std::vector()); - auto_declare>("right_wheel_names", std::vector()); - - auto_declare("wheel_separation", wheel_params_.separation); - auto_declare("wheels_per_side", wheel_params_.wheels_per_side); - auto_declare("wheel_radius", wheel_params_.radius); - auto_declare("wheel_separation_multiplier", wheel_params_.separation_multiplier); - auto_declare("left_wheel_radius_multiplier", wheel_params_.left_radius_multiplier); - auto_declare("right_wheel_radius_multiplier", wheel_params_.right_radius_multiplier); - - auto_declare("odom_frame_id", odom_params_.odom_frame_id); - auto_declare("base_frame_id", odom_params_.base_frame_id); - auto_declare>("pose_covariance_diagonal", std::vector()); - auto_declare>("twist_covariance_diagonal", std::vector()); - auto_declare("open_loop", odom_params_.open_loop); - auto_declare("position_feedback", odom_params_.position_feedback); - auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); - - auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); - publish_limited_velocity_ = - auto_declare("publish_limited_velocity", publish_limited_velocity_); - auto_declare("velocity_rolling_window_size", 10); - use_stamped_vel_ = auto_declare("use_stamped_vel", use_stamped_vel_); - - auto_declare("linear.x.has_velocity_limits", false); - auto_declare("linear.x.has_acceleration_limits", false); - auto_declare("linear.x.has_jerk_limits", false); - auto_declare("linear.x.max_velocity", NAN); - auto_declare("linear.x.min_velocity", NAN); - auto_declare("linear.x.max_acceleration", NAN); - auto_declare("linear.x.min_acceleration", NAN); - auto_declare("linear.x.max_jerk", NAN); - auto_declare("linear.x.min_jerk", NAN); - - auto_declare("angular.z.has_velocity_limits", false); - auto_declare("angular.z.has_acceleration_limits", false); - auto_declare("angular.z.has_jerk_limits", false); - auto_declare("angular.z.max_velocity", NAN); - auto_declare("angular.z.min_velocity", NAN); - auto_declare("angular.z.max_acceleration", NAN); - auto_declare("angular.z.min_acceleration", NAN); - auto_declare("angular.z.max_jerk", NAN); - auto_declare("angular.z.min_jerk", NAN); - publish_rate_ = auto_declare("publish_rate", publish_rate_); + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -115,11 +73,11 @@ controller_interface::CallbackReturn DiffDriveController::on_init() InterfaceConfiguration DiffDriveController::command_interface_configuration() const { std::vector conf_names; - for (const auto & joint_name : left_wheel_names_) + for (const auto & joint_name : params_.left_wheel_names) { conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); } - for (const auto & joint_name : right_wheel_names_) + for (const auto & joint_name : params_.right_wheel_names) { conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); } @@ -129,11 +87,11 @@ InterfaceConfiguration DiffDriveController::command_interface_configuration() co InterfaceConfiguration DiffDriveController::state_interface_configuration() const { std::vector conf_names; - for (const auto & joint_name : left_wheel_names_) + for (const auto & joint_name : params_.left_wheel_names) { conf_names.push_back(joint_name + "/" + feedback_type()); } - for (const auto & joint_name : right_wheel_names_) + for (const auto & joint_name : params_.right_wheel_names) { conf_names.push_back(joint_name + "/" + feedback_type()); } @@ -180,12 +138,11 @@ controller_interface::return_type DiffDriveController::update( previous_update_timestamp_ = time; // Apply (possibly new) multipliers: - const auto wheels = wheel_params_; - const double wheel_separation = wheels.separation_multiplier * wheels.separation; - const double left_wheel_radius = wheels.left_radius_multiplier * wheels.radius; - const double right_wheel_radius = wheels.right_radius_multiplier * wheels.radius; + const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; + const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; - if (odom_params_.open_loop) + if (params_.open_loop) { odometry_.updateOpenLoop(linear_command, angular_command, time); } @@ -193,7 +150,7 @@ controller_interface::return_type DiffDriveController::update( { double left_feedback_mean = 0.0; double right_feedback_mean = 0.0; - for (size_t index = 0; index < wheels.wheels_per_side; ++index) + for (size_t index = 0; index < params_.wheels_per_side; ++index) { const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value(); const double right_feedback = @@ -210,10 +167,10 @@ controller_interface::return_type DiffDriveController::update( left_feedback_mean += left_feedback; right_feedback_mean += right_feedback; } - left_feedback_mean /= wheels.wheels_per_side; - right_feedback_mean /= wheels.wheels_per_side; + left_feedback_mean /= params_.wheels_per_side; + right_feedback_mean /= params_.wheels_per_side; - if (odom_params_.position_feedback) + if (params_.position_feedback) { odometry_.update(left_feedback_mean, right_feedback_mean, time); } @@ -247,7 +204,7 @@ controller_interface::return_type DiffDriveController::update( realtime_odometry_publisher_->unlockAndPublish(); } - if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) { auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); transform.header.stamp = time; @@ -287,7 +244,7 @@ controller_interface::return_type DiffDriveController::update( (linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius; // Set wheels velocities: - for (size_t index = 0; index < wheels.wheels_per_side; ++index) + for (size_t index = 0; index < params_.wheels_per_side; ++index) { registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left); registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right); @@ -301,100 +258,50 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { auto logger = get_node()->get_logger(); - // update parameters - left_wheel_names_ = get_node()->get_parameter("left_wheel_names").as_string_array(); - right_wheel_names_ = get_node()->get_parameter("right_wheel_names").as_string_array(); + // update parameters if they have changed + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + RCLCPP_INFO(logger, "Parameters were updated"); + } - if (left_wheel_names_.size() != right_wheel_names_.size()) + if (params_.left_wheel_names.size() != params_.right_wheel_names.size()) { RCLCPP_ERROR( logger, "The number of left wheels [%zu] and the number of right wheels [%zu] are different", - left_wheel_names_.size(), right_wheel_names_.size()); + params_.left_wheel_names.size(), params_.right_wheel_names.size()); return controller_interface::CallbackReturn::ERROR; } - if (left_wheel_names_.empty()) + if (params_.left_wheel_names.empty()) { RCLCPP_ERROR(logger, "Wheel names parameters are empty!"); return controller_interface::CallbackReturn::ERROR; } - wheel_params_.separation = get_node()->get_parameter("wheel_separation").as_double(); - wheel_params_.wheels_per_side = - static_cast(get_node()->get_parameter("wheels_per_side").as_int()); - wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); - wheel_params_.separation_multiplier = - get_node()->get_parameter("wheel_separation_multiplier").as_double(); - wheel_params_.left_radius_multiplier = - get_node()->get_parameter("left_wheel_radius_multiplier").as_double(); - wheel_params_.right_radius_multiplier = - get_node()->get_parameter("right_wheel_radius_multiplier").as_double(); - - const auto wheels = wheel_params_; - - const double wheel_separation = wheels.separation_multiplier * wheels.separation; - const double left_wheel_radius = wheels.left_radius_multiplier * wheels.radius; - const double right_wheel_radius = wheels.right_radius_multiplier * wheels.radius; + const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; + const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); - odometry_.setVelocityRollingWindowSize( - get_node()->get_parameter("velocity_rolling_window_size").as_int()); - - odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string(); - odom_params_.base_frame_id = get_node()->get_parameter("base_frame_id").as_string(); - - auto pose_diagonal = get_node()->get_parameter("pose_covariance_diagonal").as_double_array(); - std::copy( - pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); - - auto twist_diagonal = get_node()->get_parameter("twist_covariance_diagonal").as_double_array(); - std::copy( - twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); - - odom_params_.open_loop = get_node()->get_parameter("open_loop").as_bool(); - odom_params_.position_feedback = get_node()->get_parameter("position_feedback").as_bool(); - odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); + odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); cmd_vel_timeout_ = std::chrono::milliseconds{ static_cast(get_node()->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; publish_limited_velocity_ = get_node()->get_parameter("publish_limited_velocity").as_bool(); use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); - try - { - limiter_linear_ = SpeedLimiter( - get_node()->get_parameter("linear.x.has_velocity_limits").as_bool(), - get_node()->get_parameter("linear.x.has_acceleration_limits").as_bool(), - get_node()->get_parameter("linear.x.has_jerk_limits").as_bool(), - get_node()->get_parameter("linear.x.min_velocity").as_double(), - get_node()->get_parameter("linear.x.max_velocity").as_double(), - get_node()->get_parameter("linear.x.min_acceleration").as_double(), - get_node()->get_parameter("linear.x.max_acceleration").as_double(), - get_node()->get_parameter("linear.x.min_jerk").as_double(), - get_node()->get_parameter("linear.x.max_jerk").as_double()); - } - catch (const std::runtime_error & e) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error configuring linear speed limiter: %s", e.what()); - } + limiter_linear_ = SpeedLimiter( + params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits, + params_.linear.x.has_jerk_limits, params_.linear.x.min_velocity, params_.linear.x.max_velocity, + params_.linear.x.min_acceleration, params_.linear.x.max_acceleration, params_.linear.x.min_jerk, + params_.linear.x.max_jerk); - try - { - limiter_angular_ = SpeedLimiter( - get_node()->get_parameter("angular.z.has_velocity_limits").as_bool(), - get_node()->get_parameter("angular.z.has_acceleration_limits").as_bool(), - get_node()->get_parameter("angular.z.has_jerk_limits").as_bool(), - get_node()->get_parameter("angular.z.min_velocity").as_double(), - get_node()->get_parameter("angular.z.max_velocity").as_double(), - get_node()->get_parameter("angular.z.min_acceleration").as_double(), - get_node()->get_parameter("angular.z.max_acceleration").as_double(), - get_node()->get_parameter("angular.z.min_jerk").as_double(), - get_node()->get_parameter("angular.z.max_jerk").as_double()); - } - catch (const std::runtime_error & e) - { - RCLCPP_ERROR(get_node()->get_logger(), "Error configuring angular speed limiter: %s", e.what()); - } + limiter_angular_ = SpeedLimiter( + params_.angular.z.has_velocity_limits, params_.angular.z.has_acceleration_limits, + params_.angular.z.has_jerk_limits, params_.angular.z.min_velocity, + params_.angular.z.max_velocity, params_.angular.z.min_acceleration, + params_.angular.z.max_acceleration, params_.angular.z.min_jerk, params_.angular.z.max_jerk); if (!reset()) { @@ -402,7 +309,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( } // left and right sides are both equal at this point - wheel_params_.wheels_per_side = left_wheel_names_.size(); + params_.wheels_per_side = params_.left_wheel_names.size(); if (publish_limited_velocity_) { @@ -483,12 +390,12 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( controller_namespace = controller_namespace + "/"; } - odom_params_.odom_frame_id = controller_namespace + odom_params_.odom_frame_id; - odom_params_.base_frame_id = controller_namespace + odom_params_.base_frame_id; + const auto odom_frame_id = controller_namespace + params_.odom_frame_id; + const auto base_frame_id = controller_namespace + params_.base_frame_id; auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.frame_id = odom_params_.odom_frame_id; - odometry_message.child_frame_id = odom_params_.base_frame_id; + odometry_message.header.frame_id = controller_namespace + odom_frame_id; + odometry_message.child_frame_id = controller_namespace + base_frame_id; // limit the publication on the topics /odom and /tf publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); @@ -504,9 +411,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { // 0, 7, 14, 21, 28, 35 const size_t diagonal_index = NUM_DIMENSIONS * index + index; - odometry_message.pose.covariance[diagonal_index] = odom_params_.pose_covariance_diagonal[index]; - odometry_message.twist.covariance[diagonal_index] = - odom_params_.twist_covariance_diagonal[index]; + odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + odometry_message.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; } // initialize transform publisher and message @@ -519,8 +425,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( // keeping track of odom and base_link transforms only auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; odometry_transform_message.transforms.resize(1); - odometry_transform_message.transforms.front().header.frame_id = odom_params_.odom_frame_id; - odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id; + odometry_transform_message.transforms.front().header.frame_id = odom_frame_id; + odometry_transform_message.transforms.front().child_frame_id = base_frame_id; previous_update_timestamp_ = get_node()->get_clock()->now(); return controller_interface::CallbackReturn::SUCCESS; @@ -530,9 +436,9 @@ controller_interface::CallbackReturn DiffDriveController::on_activate( const rclcpp_lifecycle::State &) { const auto left_result = - configure_side("left", left_wheel_names_, registered_left_wheel_handles_); + configure_side("left", params_.left_wheel_names, registered_left_wheel_handles_); const auto right_result = - configure_side("right", right_wheel_names_, registered_right_wheel_handles_); + configure_side("right", params_.right_wheel_names, registered_right_wheel_handles_); if ( left_result == controller_interface::CallbackReturn::ERROR || diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml new file mode 100644 index 0000000000..82ba210113 --- /dev/null +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -0,0 +1,177 @@ +diff_drive_controller: + left_wheel_names: { + type: string_array, + default_value: [], + description: "Link names of the left side wheels", + } + right_wheel_names: { + type: string_array, + default_value: [], + description: "Link names of the right side wheels", + } + wheel_separation: { + type: double, + default_value: 0.0, + description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.", + } + wheels_per_side: { + type: int, + default_value: 0, + description: "Number of wheels on each wide of the robot. This is important to take the wheels slip into account when multiple wheels on each side are present. If there are more wheels then control signals for each side, you should enter number or control signals. For example, Husky has two wheels on each side, but they use one control signal, in this case '1' is the correct value of the parameter.", + } + wheel_radius: { + type: double, + default_value: 0.0, + description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", + } + wheel_separation_multiplier: { + type: double, + default_value: 1.0, + description: "Correction factor for wheel separation (TODO(destogl): Please help me describe this correctly)", + } + left_wheel_radius_multiplier: { + type: double, + default_value: 1.0, + description: "Correction factor when radius of left wheels differs from the nominal value in ``wheel_radius`` parameter.", + } + right_wheel_radius_multiplier: { + type: double, + default_value: 1.0, + description: "Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter.", + } + odom_frame_id: { + type: string, + default_value: "odom", + description: "Name of the frame for odometry. This frame is parent of ``base_frame_id`` when controller publishes odometry.", + } + base_frame_id: { + type: string, + default_value: "odom", + description: "Name of the robot's base frame that is child of the odometry frame.", + } + pose_covariance_diagonal: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the pose. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + twist_covariance_diagonal: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the speed. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + open_loop: { + type: bool, + default_value: false, + description: "If set to false the odometry of the robot will be calculated from the commanded values and not from feedback.", + } + position_feedback: { + type: bool, + default_value: true, + description: "Is there position feedback from hardware.", + } + enable_odom_tf: { + type: bool, + default_value: true, + description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.", + } + cmd_vel_timeout: { + type: double, + default_value: 0.5, # seconds + description: "Timeout after which input command on ``cmd_vel`` topic is considered staled.", + } + publish_limited_velocity: { + type: bool, + default_value: false, + description: "Publish limited velocity value.", + } + velocity_rolling_window_size: { + type: int, + default_value: 10, + description: "Size of the rolling window for calculation of mean velocity use in odometry.", + } + use_stamped_vel: { + type: bool, + default_value: true, + description: "Use stamp from input velocity message to calculate how old the command actually is.", + } + publish_rate: { + type: double, + default_value: 50.0, # Hz + description: "Publishing rate (Hz) of the odometry and TF messages.", + } + linear: + x: + has_velocity_limits: { + type: bool, + default_value: false, + } + has_acceleration_limits: { + type: bool, + default_value: false, + } + has_jerk_limits: { + type: bool, + default_value: false, + } + max_velocity: { + type: double, + default_value: .NAN, + } + min_velocity: { + type: double, + default_value: .NAN, + } + max_acceleration: { + type: double, + default_value: .NAN, + } + min_acceleration: { + type: double, + default_value: .NAN, + } + max_jerk: { + type: double, + default_value: .NAN, + } + min_jerk: { + type: double, + default_value: .NAN, + } + angular: + z: + has_velocity_limits: { + type: bool, + default_value: false, + } + has_acceleration_limits: { + type: bool, + default_value: false, + } + has_jerk_limits: { + type: bool, + default_value: false, + } + max_velocity: { + type: double, + default_value: .NAN, + } + min_velocity: { + type: double, + default_value: .NAN, + } + max_acceleration: { + type: double, + default_value: .NAN, + } + min_acceleration: { + type: double, + default_value: .NAN, + } + max_jerk: { + type: double, + default_value: .NAN, + } + min_jerk: { + type: double, + default_value: .NAN, + } From 4c0d58e459eb2352c14c9eeffad04d4d8a4100e8 Mon Sep 17 00:00:00 2001 From: Robotgir <47585672+Robotgir@users.noreply.github.com> Date: Sun, 4 Dec 2022 20:49:07 +0100 Subject: [PATCH 064/126] =?UTF-8?q?[TricycleController]=20Removed=20?= =?UTF-8?q?=E2=80=9Cpublish=20period=E2=80=9D=20functionality=20=E2=8F=B1?= =?UTF-8?q?=20#abi-break=20#behavior-break=20(#468)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../tricycle_controller.hpp | 5 -- .../src/tricycle_controller.cpp | 61 ++++++++----------- 2 files changed, 25 insertions(+), 41 deletions(-) diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 2178008725..cef90d026a 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -167,11 +167,6 @@ class TricycleController : public controller_interface::ControllerInterface TractionLimiter limiter_traction_; SteeringLimiter limiter_steering_; - // publish rate limiter - double publish_rate_ = 50.0; - rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); - rclcpp::Time previous_publish_timestamp_{0}; - bool is_halted = false; bool use_stamped_vel_ = true; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 114731242c..4f4e22ec9d 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -71,7 +71,6 @@ CallbackReturn TricycleController::on_init() auto_declare("publish_ackermann_command", publish_ackermann_command_); auto_declare("velocity_rolling_window_size", 10); auto_declare("use_stamped_vel", use_stamped_vel_); - auto_declare("publish_rate", publish_rate_); auto_declare("traction.max_velocity", NAN); auto_declare("traction.min_velocity", NAN); @@ -169,40 +168,35 @@ controller_interface::return_type TricycleController::update( tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - if (previous_publish_timestamp_ + publish_period_ < time) + if (realtime_odometry_publisher_->trylock()) { - previous_publish_timestamp_ += publish_period_; - - if (realtime_odometry_publisher_->trylock()) + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.stamp = time; + if (!odom_params_.odom_only_twist) { - auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.stamp = time; - if (!odom_params_.odom_only_twist) - { - odometry_message.pose.pose.position.x = odometry_.getX(); - odometry_message.pose.pose.position.y = odometry_.getY(); - odometry_message.pose.pose.orientation.x = orientation.x(); - odometry_message.pose.pose.orientation.y = orientation.y(); - odometry_message.pose.pose.orientation.z = orientation.z(); - odometry_message.pose.pose.orientation.w = orientation.w(); - } - odometry_message.twist.twist.linear.x = odometry_.getLinear(); - odometry_message.twist.twist.angular.z = odometry_.getAngular(); - realtime_odometry_publisher_->unlockAndPublish(); + odometry_message.pose.pose.position.x = odometry_.getX(); + odometry_message.pose.pose.position.y = odometry_.getY(); + odometry_message.pose.pose.orientation.x = orientation.x(); + odometry_message.pose.pose.orientation.y = orientation.y(); + odometry_message.pose.pose.orientation.z = orientation.z(); + odometry_message.pose.pose.orientation.w = orientation.w(); } + odometry_message.twist.twist.linear.x = odometry_.getLinear(); + odometry_message.twist.twist.angular.z = odometry_.getAngular(); + realtime_odometry_publisher_->unlockAndPublish(); + } - if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) - { - auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); - transform.header.stamp = time; - transform.transform.translation.x = odometry_.getX(); - transform.transform.translation.y = odometry_.getY(); - transform.transform.rotation.x = orientation.x(); - transform.transform.rotation.y = orientation.y(); - transform.transform.rotation.z = orientation.z(); - transform.transform.rotation.w = orientation.w(); - realtime_odometry_transform_publisher_->unlockAndPublish(); - } + if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + { + auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + transform.header.stamp = time; + transform.transform.translation.x = odometry_.getX(); + transform.transform.translation.y = odometry_.getY(); + transform.transform.rotation.x = orientation.x(); + transform.transform.rotation.y = orientation.y(); + transform.transform.rotation.z = orientation.z(); + transform.transform.rotation.w = orientation.w(); + realtime_odometry_transform_publisher_->unlockAndPublish(); } // Compute wheel velocity and angle @@ -305,9 +299,6 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); - publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); - publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); - try { limiter_traction_ = TractionLimiter( @@ -421,8 +412,6 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & odometry_message.header.frame_id = odom_params_.odom_frame_id; odometry_message.child_frame_id = odom_params_.base_frame_id; - previous_publish_timestamp_ = get_node()->get_clock()->now(); - // initialize odom values zeros odometry_message.twist = geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); From 091ab8d1c350c7fc879d59e310ca9cf197c35891 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 6 Dec 2022 12:22:15 +0000 Subject: [PATCH 065/126] Update changelogs --- admittance_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 6 ++++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 3 +++ 15 files changed, 52 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 1b09c945b6..a69b0412d9 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- * Bring admittance_controller version up to speed diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index f631cb18c6..2c1a4d510d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [DiffDriveController] Use generate parameter library (`#386 `_) +* [DiffDriveController] Change units of velocity feedback (`#452 `_) +* Contributors: Maciej Stępień, Paul Gesel, Denis Štogl, Bence Magyar + 2.14.0 (2022-11-18) ------------------- * Odom Topic & Frame Namespaces (`#461 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 95d292e041..cacea16444 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index d31b8cc6a8..a371221642 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- * Fix parameter library export (`#448 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 04b740e499..3ca94cb835 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- * Generate params for ForwardCommandController (`#396 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index c4ff69decf..ee20bdc4f5 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add basic gripper controller tests (`#459 `_) +* Contributors: Bence Magyar + 2.14.0 (2022-11-18) ------------------- * Use optional from C++17 (`#460 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index c79e3bc3fc..9b5d205275 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- * [IMU Broadcaster] Added parameters for definition of static covariances. (`#455 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 44600171f6..16c4735717 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index deb9d68c6e..1e2d50a3b8 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- * Fix parameter library export (`#448 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index c199ae0ed0..b3a5fcdd0d 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index fcfbacc0e6..05756ee7b2 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index a4feb47161..0b4d93973b 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- * Remove deprecation warning when parameter without value is set. (`#445 `_) diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 5a01e489c2..3d1869a9d9 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 65cc206bda..b235abcb52 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [TricycleController] Removed “publish period” functionality ⏱ #abi-break #behavior-break (`#468 `_) +* Contributors: Robotgir, Denis Štogl + 2.14.0 (2022-11-18) ------------------- * Include to fix compilation error on macOS (`#467 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index beb72450a1..00101fc622 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.14.0 (2022-11-18) ------------------- From f08c3789e81b72f64858eccfb4a1553fd9ed801f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 6 Dec 2022 12:23:30 +0000 Subject: [PATCH 066/126] 2.15.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index a69b0412d9..cc7fa5f2bf 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 34f7ba23b2..876ed424b0 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 2.14.0 + 2.15.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 2c1a4d510d..3546af9bba 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- * [DiffDriveController] Use generate parameter library (`#386 `_) * [DiffDriveController] Change units of velocity feedback (`#452 `_) * Contributors: Maciej Stępień, Paul Gesel, Denis Štogl, Bence Magyar diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 6b675ccc10..8b2e368824 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.14.0 + 2.15.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index cacea16444..6954b1f7e3 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index b1c174f2c1..7476889b73 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.14.0 + 2.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index a371221642..3a37a17174 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 75f2cc70dc..8cee3ef6a3 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.14.0 + 2.15.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 3ca94cb835..0dc1db853b 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 0510e8bb3a..7853d78542 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.14.0 + 2.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index ee20bdc4f5..e11321aefe 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- * Add basic gripper controller tests (`#459 `_) * Contributors: Bence Magyar diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 2f46a5c970..b033e99b12 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.14.0 + 2.15.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 9b5d205275..0f26b4cbf2 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 6eba494c33..79eb5e14a7 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.14.0 + 2.15.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 16c4735717..68d25c05aa 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index a4523a9dc0..c24d274a93 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.14.0 + 2.15.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 1e2d50a3b8..bc59ab4960 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index f6c8984a66..3e806ce59e 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.14.0 + 2.15.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index b3a5fcdd0d..115fac4e37 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 4a435fe93c..21cdd59b86 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.14.0 + 2.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 05756ee7b2..0799e154c7 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 5836915aa1..e91679802c 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.14.0 + 2.15.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 0b4d93973b..f60cb9d72a 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 3367d44c2d..269d1da466 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.14.0 + 2.15.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 1f24996371..ed3ae63656 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.14.0", + version="2.15.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 3d1869a9d9..d4b07df08b 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index faff00589e..81e69f88fd 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.14.0 + 2.15.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index d635d515b0..afb93527f1 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.14.0", + version="2.15.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index b235abcb52..0739bf6446 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- * [TricycleController] Removed “publish period” functionality ⏱ #abi-break #behavior-break (`#468 `_) * Contributors: Robotgir, Denis Štogl diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index bd5204c357..1681367c25 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.14.0 + 2.15.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 00101fc622..e47e52cdac 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.15.0 (2022-12-06) +------------------- 2.14.0 (2022-11-18) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index fe5dbdbb56..131bcd11f0 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.14.0 + 2.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 54f2c598e7305f06080d941ffe52e4ce5a2d67ab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 6 Dec 2022 15:51:14 +0100 Subject: [PATCH 067/126] Remove compilation warnings from DiffDriveController (#477) --- .../include/diff_drive_controller/odometry.hpp | 4 ++-- diff_drive_controller/src/speed_limiter.cpp | 7 +++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index 2adc512f88..83ab270f72 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -25,7 +25,7 @@ #include #include "rclcpp/time.hpp" -#include "rcppmath/rolling_mean_accumulator.hpp" +#include "rcpputils/rolling_mean_accumulator.hpp" namespace diff_drive_controller { @@ -50,7 +50,7 @@ class Odometry void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); private: - using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; + using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator; void integrateRungeKutta2(double linear, double angular); void integrateExact(double linear, double angular); diff --git a/diff_drive_controller/src/speed_limiter.cpp b/diff_drive_controller/src/speed_limiter.cpp index e25a6f47ed..0f82c2cc3b 100644 --- a/diff_drive_controller/src/speed_limiter.cpp +++ b/diff_drive_controller/src/speed_limiter.cpp @@ -20,7 +20,6 @@ #include #include "diff_drive_controller/speed_limiter.hpp" -#include "rcppmath/clamp.hpp" namespace diff_drive_controller { @@ -92,7 +91,7 @@ double SpeedLimiter::limit_velocity(double & v) if (has_velocity_limits_) { - v = rcppmath::clamp(v, min_velocity_, max_velocity_); + v = std::clamp(v, min_velocity_, max_velocity_); } return tmp != 0.0 ? v / tmp : 1.0; @@ -107,7 +106,7 @@ double SpeedLimiter::limit_acceleration(double & v, double v0, double dt) const double dv_min = min_acceleration_ * dt; const double dv_max = max_acceleration_ * dt; - const double dv = rcppmath::clamp(v - v0, dv_min, dv_max); + const double dv = std::clamp(v - v0, dv_min, dv_max); v = v0 + dv; } @@ -129,7 +128,7 @@ double SpeedLimiter::limit_jerk(double & v, double v0, double v1, double dt) const double da_min = min_jerk_ * dt2; const double da_max = max_jerk_ * dt2; - const double da = rcppmath::clamp(dv - dv0, da_min, da_max); + const double da = std::clamp(dv - dv0, da_min, da_max); v = v0 + dv0 + da; } From 4b9c06a0ef6c21b0cd3bb3e1b0d25c915a8f9e7f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 6 Dec 2022 15:52:55 +0100 Subject: [PATCH 068/126] Fix deprecation warnings when compiling (#478) --- tricycle_controller/include/tricycle_controller/odometry.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tricycle_controller/include/tricycle_controller/odometry.hpp b/tricycle_controller/include/tricycle_controller/odometry.hpp index 4a958a93c6..62eb51d6cc 100644 --- a/tricycle_controller/include/tricycle_controller/odometry.hpp +++ b/tricycle_controller/include/tricycle_controller/odometry.hpp @@ -22,7 +22,7 @@ #include #include "rclcpp/time.hpp" -#include "rcppmath/rolling_mean_accumulator.hpp" +#include "rcpputils/rolling_mean_accumulator.hpp" namespace tricycle_controller { @@ -45,7 +45,7 @@ class Odometry void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); private: - using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; + using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator; void integrateRungeKutta2(double linear, double angular); void integrateExact(double linear, double angular); From b72d0bbc97ea6cf4a1580c4bb734d7879350d2f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 6 Dec 2022 20:41:44 +0100 Subject: [PATCH 069/126] [JTC] Remove deprecation from parameters validation file. (#476) * [JTC] Remove deprecation from parameters validation file. * Add new dependencies. * Add missing dependencies * Update package.xml --- joint_trajectory_controller/CMakeLists.txt | 2 + .../validate_jtc_parameters.hpp | 49 +++++++++++-------- joint_trajectory_controller/package.xml | 4 +- 3 files changed, 34 insertions(+), 21 deletions(-) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 32461617ac..f3f8c2ab7b 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -22,6 +22,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle realtime_tools + rsl + tl_expected trajectory_msgs ) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp index 09363eebb1..fc6319fabf 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp @@ -16,12 +16,17 @@ #define JOINT_TRAJECTORY_CONTROLLER__VALIDATE_JTC_PARAMETERS_HPP_ #include +#include #include "parameter_traits/parameter_traits.hpp" +#include "rclcpp/parameter.hpp" +#include "rsl/algorithm.hpp" +#include "tl_expected/expected.hpp" namespace parameter_traits { -Result command_interface_type_combinations(rclcpp::Parameter const & parameter) +tl::expected command_interface_type_combinations( + rclcpp::Parameter const & parameter) { auto const & interface_types = parameter.as_string_array(); @@ -31,33 +36,37 @@ Result command_interface_type_combinations(rclcpp::Parameter const & parameter) // 2. position [velocity, [acceleration]] if ( - contains(interface_types, "velocity") && interface_types.size() > 1 && - !contains(interface_types, "position")) + rsl::contains>(interface_types, "velocity") && + interface_types.size() > 1 && + !rsl::contains>(interface_types, "position")) { - return ERROR( + return tl::make_unexpected( "'velocity' command interface can be used either alone or 'position' " "interface has to be present"); } if ( - contains(interface_types, "acceleration") && - (!contains(interface_types, "velocity") && - !contains(interface_types, "position"))) + rsl::contains>(interface_types, "acceleration") && + (!rsl::contains>(interface_types, "velocity") && + !rsl::contains>(interface_types, "position"))) { - return ERROR( + return tl::make_unexpected( "'acceleration' command interface can only be used if 'velocity' and " "'position' interfaces are present"); } - if (contains(interface_types, "effort") && interface_types.size() > 1) + if ( + rsl::contains>(interface_types, "effort") && + interface_types.size() > 1) { - return ERROR("'effort' command interface has to be used alone"); + return tl::make_unexpected("'effort' command interface has to be used alone"); } - return OK; + return {}; } -Result state_interface_type_combinations(rclcpp::Parameter const & parameter) +tl::expected state_interface_type_combinations( + rclcpp::Parameter const & parameter) { auto const & interface_types = parameter.as_string_array(); @@ -65,25 +74,25 @@ Result state_interface_type_combinations(rclcpp::Parameter const & parameter) // 1. position [velocity, [acceleration]] if ( - contains(interface_types, "velocity") && - !contains(interface_types, "position")) + rsl::contains>(interface_types, "velocity") && + !rsl::contains>(interface_types, "position")) { - return ERROR( + return tl::make_unexpected( "'velocity' state interface cannot be used if 'position' interface " "is missing."); } if ( - contains(interface_types, "acceleration") && - (!contains(interface_types, "position") || - !contains(interface_types, "velocity"))) + rsl::contains>(interface_types, "acceleration") && + (!rsl::contains>(interface_types, "position") || + !rsl::contains>(interface_types, "velocity"))) { - return ERROR( + return tl::make_unexpected( "'acceleration' state interface cannot be used if 'position' and 'velocity' " "interfaces are not present."); } - return OK; + return {}; } } // namespace parameter_traits diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 3e806ce59e..9750adb1b3 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -22,11 +22,13 @@ controller_interface control_msgs control_toolbox + generate_parameter_library hardware_interface rclcpp rclcpp_lifecycle + rsl + tl_expected trajectory_msgs - generate_parameter_library ament_cmake_gmock controller_manager From d18a10cd47ea08f3cff626224ad55f6c0c3adc40 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 12 Dec 2022 18:21:10 +0000 Subject: [PATCH 070/126] Don't run humble workflows from master branch (#479) * Remove workflows for distros not supported on master * Revert "Remove workflows for distros not supported on master" This reverts commit e916af3b19ba8642312692328ebc71a2f92ac93c. * Don't run humble workflows on master --- .github/workflows/humble-abi-compatibility.yml | 4 ++-- .github/workflows/humble-binary-build-main.yml | 8 ++++---- .github/workflows/humble-binary-build-testing.yml | 8 ++++---- .github/workflows/humble-rhel-binary-build.yml | 6 +++--- .github/workflows/humble-semi-binary-build-main.yml | 8 ++++---- .github/workflows/humble-semi-binary-build-testing.yml | 8 ++++---- .github/workflows/humble-source-build.yml | 6 +++--- 7 files changed, 24 insertions(+), 24 deletions(-) diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 1be00cfc76..3f38d5b6ce 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -2,10 +2,10 @@ name: Humble - ABI Compatibility Check on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble jobs: abi_check: diff --git a/.github/workflows/humble-binary-build-main.yml b/.github/workflows/humble-binary-build-main.yml index 01708cf864..64d78f281a 100644 --- a/.github/workflows/humble-binary-build-main.yml +++ b/.github/workflows/humble-binary-build-main.yml @@ -5,13 +5,13 @@ name: Humble Binary Build - main on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' @@ -23,4 +23,4 @@ jobs: ros_distro: humble ros_repo: main upstream_workspace: ros2_controllers-not-released.humble.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: humble diff --git a/.github/workflows/humble-binary-build-testing.yml b/.github/workflows/humble-binary-build-testing.yml index 73ed0a4859..524cacd685 100644 --- a/.github/workflows/humble-binary-build-testing.yml +++ b/.github/workflows/humble-binary-build-testing.yml @@ -5,13 +5,13 @@ name: Humble Binary Build - testing on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' @@ -23,4 +23,4 @@ jobs: ros_distro: humble ros_repo: testing upstream_workspace: ros2_controllers-not-released.humble.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: humble diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index 8f4a65a9b5..2a4b627d5e 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -2,13 +2,13 @@ name: Humble RHEL Binary Build on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/humble-semi-binary-build-main.yml b/.github/workflows/humble-semi-binary-build-main.yml index 10b1186b79..863df79a22 100644 --- a/.github/workflows/humble-semi-binary-build-main.yml +++ b/.github/workflows/humble-semi-binary-build-main.yml @@ -4,13 +4,13 @@ name: Humble Semi-Binary Build - main on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' @@ -22,4 +22,4 @@ jobs: ros_distro: humble ros_repo: main upstream_workspace: ros2_controllers.humble.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: humble diff --git a/.github/workflows/humble-semi-binary-build-testing.yml b/.github/workflows/humble-semi-binary-build-testing.yml index ec73cc6b98..6286636e1f 100644 --- a/.github/workflows/humble-semi-binary-build-testing.yml +++ b/.github/workflows/humble-semi-binary-build-testing.yml @@ -4,13 +4,13 @@ name: Humble Semi-Binary Build - testing on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' @@ -22,4 +22,4 @@ jobs: ros_distro: humble ros_repo: testing upstream_workspace: ros2_controllers.humble.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: humble diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index 18c33b6d52..ff0fd62e05 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -2,10 +2,10 @@ name: Humble Source Build on: workflow_dispatch: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' @@ -15,5 +15,5 @@ jobs: uses: ./.github/workflows/reusable-ros-tooling-source-build.yml with: ros_distro: humble - ref: master + ref: humble ros2_repo_branch: humble From 42f6c1470508ca2480b954eead58a87e7e827434 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 13 Dec 2022 15:36:57 +0100 Subject: [PATCH 071/126] Fix markup in userdoc.rst (#480) --- joint_trajectory_controller/doc/userdoc.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index d225624096..8230d7febb 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -42,8 +42,9 @@ References States ^^^^^^^ The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. -Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the [hardware_interface/hardware_interface_type_values.hpp](https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp). +Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. Legal combinations of state interfaces are: + - ``position`` - ``position`` and ``velocity`` - ``position``, ``velocity`` and ``acceleration`` From f6c4769eae0389cad62596291f88ec041558f02c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 6 Jan 2023 16:53:25 +0000 Subject: [PATCH 072/126] Add backward_ros to all controllers (#489) --- admittance_controller/CMakeLists.txt | 1 + admittance_controller/package.xml | 1 + diff_drive_controller/CMakeLists.txt | 1 + diff_drive_controller/package.xml | 1 + effort_controllers/CMakeLists.txt | 1 + effort_controllers/package.xml | 1 + force_torque_sensor_broadcaster/CMakeLists.txt | 1 + force_torque_sensor_broadcaster/package.xml | 1 + forward_command_controller/CMakeLists.txt | 1 + forward_command_controller/package.xml | 1 + gripper_controllers/CMakeLists.txt | 1 + gripper_controllers/package.xml | 1 + imu_sensor_broadcaster/CMakeLists.txt | 1 + imu_sensor_broadcaster/package.xml | 1 + joint_state_broadcaster/CMakeLists.txt | 1 + joint_state_broadcaster/package.xml | 1 + joint_trajectory_controller/CMakeLists.txt | 1 + joint_trajectory_controller/package.xml | 1 + position_controllers/CMakeLists.txt | 1 + position_controllers/package.xml | 1 + tricycle_controller/CMakeLists.txt | 1 + tricycle_controller/package.xml | 1 + velocity_controllers/CMakeLists.txt | 1 + velocity_controllers/package.xml | 1 + 24 files changed, 24 insertions(+) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index a6dcf35dce..3f121d9c3c 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -34,6 +34,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 876ed424b0..69180a290f 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -11,6 +11,7 @@ ament_cmake + backward_ros control_msgs control_toolbox controller_interface diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 83f18a9931..7188f09f18 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -26,6 +26,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 8b2e368824..4feec7fe37 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -11,6 +11,7 @@ ament_cmake generate_parameter_library + backward_ros controller_interface geometry_msgs hardware_interface diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index 91632606bf..d0e92dda9a 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -18,6 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 7476889b73..0ca0d8dffb 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -10,6 +10,7 @@ ament_cmake + backward_ros forward_command_controller rclcpp diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 3aa57d4aad..eac3fe9d8b 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 8cee3ef6a3..174499c573 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -11,6 +11,7 @@ ament_cmake + backward_ros controller_interface geometry_msgs hardware_interface diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index ef8581d195..5dacdf66c1 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 7853d78542..f6ded4ff93 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -10,6 +10,7 @@ ament_cmake + backward_ros controller_interface generate_parameter_library hardware_interface diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index 97ccd2788d..e1b32ac2b3 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -29,6 +29,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index b033e99b12..2734c54933 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -14,6 +14,7 @@ ament_cmake + backward_ros control_msgs control_toolbox controller_interface diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index e49fb65ad3..ef9d5e14e4 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 79eb5e14a7..40836967c3 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -11,6 +11,7 @@ ament_cmake + backward_ros controller_interface generate_parameter_library hardware_interface diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index 0717577e15..ee46d8eaab 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -12,6 +12,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(control_msgs REQUIRED) find_package(controller_interface REQUIRED) find_package(generate_parameter_library REQUIRED) diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index c24d274a93..9fd5942d5d 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -18,6 +18,7 @@ pluginlib rcutils + backward_ros control_msgs controller_interface generate_parameter_library diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index f3f8c2ab7b..af0bdb5109 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -28,6 +28,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 9750adb1b3..f64bdf635e 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -19,6 +19,7 @@ pluginlib realtime_tools + backward_ros controller_interface control_msgs control_toolbox diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index f1f57c6180..faf32a0e0b 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -18,6 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 21cdd59b86..086afccc1f 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -10,6 +10,7 @@ ament_cmake + backward_ros forward_command_controller rclcpp diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index c499bca983..e9342e8696 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -7,6 +7,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(controller_interface REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 1681367c25..3c853872e9 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -11,6 +11,7 @@ ament_cmake + backward_ros controller_interface geometry_msgs hardware_interface diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index 5f0560659a..4d7ad07dbd 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -18,6 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 131bcd11f0..03f1bed657 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -10,6 +10,7 @@ ament_cmake + backward_ros forward_command_controller rclcpp From a49a87da1df40851a80507d61931d12c969cb015 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 10 Jan 2023 18:52:18 +0100 Subject: [PATCH 073/126] [JTC] Allow ff_velocity_scale=0 without deprecated warning (#490) * Allow ff_velocity_scale=0 without deprecated warning * Remove deprecated warning for old ff_velocity_scale param --- .../src/joint_trajectory_controller.cpp | 16 +--------------- 1 file changed, 1 insertion(+), 15 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 96110adcb2..8df4193361 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -563,21 +563,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( pids_[i] = std::make_shared( gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); - // TODO(destogl): remove this in ROS2 Iron - // Check deprecated style for "ff_velocity_scale" parameter definition. - if (gains.ff_velocity_scale == 0.0) - { - RCLCPP_WARN( - get_node()->get_logger(), - "'ff_velocity_scale' parameters is not defined under 'gains..' structure. " - "Maybe you are using deprecated format 'ff_velocity_scale/'!"); - - ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + params_.joints[i], 0.0); - } - else - { - ff_velocity_scale_[i] = gains.ff_velocity_scale; - } + ff_velocity_scale_[i] = gains.ff_velocity_scale; } } From 3829d33f236932890548b25df2812c3eb5a589a6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 10 Jan 2023 22:00:35 +0100 Subject: [PATCH 074/126] =?UTF-8?q?=F0=9F=94=A7=20Fixes=20and=20updated=20?= =?UTF-8?q?on=20pre-commit=20hooks=20and=20their=20action=20(#492)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Use pydocstyle instead of pep257 because it replaces it. Bump versions of hooks. * Remove fixed python version from formatting action. Fix action warnings about Node.js 12 by updating them. Bump version of clang-format. * Fixup formatting. * Bump clang-format version. * Update .pre-commit-config.yaml --- .github/workflows/ci-format.yml | 8 +++---- .pre-commit-config.yaml | 22 +++++++++---------- .../resource/joint_trajectory_controller.ui | 2 +- .../update_combo.py | 3 ++- 4 files changed, 18 insertions(+), 17 deletions(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index dd78cca0ed..b54c76d3e5 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,11 +12,11 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: actions/setup-python@v2 + - uses: actions/setup-python@v4.4.0 with: - python-version: 3.9 + python-version: '3.10' - name: Install system hooks - run: sudo apt install -qq clang-format-12 cppcheck - - uses: pre-commit/action@v2.0.3 + run: sudo apt install -qq clang-format-14 cppcheck + - uses: pre-commit/action@v3.0.0 with: extra_args: --all-files --hook-stage manual diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 58fe46dd90..b927aeac78 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.3.0 + rev: v4.4.0 hooks: - id: check-added-large-files - id: check-ast @@ -33,26 +33,26 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v2.37.3 + rev: v3.3.1 hooks: - id: pyupgrade args: [--py36-plus] - repo: https://github.com/psf/black - rev: 22.6.0 + rev: 22.12.0 hooks: - id: black args: ["--line-length=99"] - # PEP 257 - - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257 - rev: v0.3.3 + # PyDocStyle + - repo: https://github.com/PyCQA/pydocstyle + rev: 6.2.2 hooks: - - id: pep257 + - id: pydocstyle args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/pycqa/flake8 - rev: 5.0.4 + rev: 6.0.0 hooks: - id: flake8 args: ["--ignore=E501"] @@ -63,7 +63,7 @@ repos: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format-12 + entry: clang-format-14 language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ['-fallback-style=none', '-i'] @@ -119,7 +119,7 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: v1.0.0 + rev: v1.1.1 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] @@ -136,7 +136,7 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.1.0 + rev: v2.2.2 hooks: - id: codespell args: ['--write-changes', '--uri-ignore-words-list=ist'] diff --git a/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui index 3f3b6c1186..bc43f202e8 100644 --- a/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui +++ b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui @@ -31,7 +31,7 @@ - <html><head/><body><p><span style=" font-weight:600;">controller manager</span> namespace. It is assumed that the <span style=" font-weight:600;">robot_description</span> parameter also lives in the same namesapce.</p></body></html> + <html><head/><body><p><span style=" font-weight:600;">controller manager</span> namespace. It is assumed that the <span style=" font-weight:600;">robot_description</span> parameter also lives in the same namespace.</p></body></html> controller manager ns diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py index 4702d7fbc5..f0b632322f 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py @@ -48,7 +48,8 @@ def update_combo(combo, new_vals): def _is_permutation(a, b): - """Check is arrays are permutation of each other. + """ + Check is arrays are permutation of each other. @type a [] first array with values to compare with the second one. @type b [] second array with values to compare with the first one. From 97748d796bfe32123444b5cb73e97c60356b9f92 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 10 Jan 2023 23:00:07 +0100 Subject: [PATCH 075/126] [JTC] Activate test for only velocity controller (#487) --- .../test/test_trajectory_controller.cpp | 29 ++++++++++--------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 6b0020c294..216c574228 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -421,9 +421,13 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param traj_controller_->update( rclcpp::Time(static_cast(0.35 * 1e9)), rclcpp::Duration::from_seconds(0.1)); - EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); - EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); - EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); + if (traj_controller_->has_position_command_interface()) + { + // otherwise this won't be updated + EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); + EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); + EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); + } // state = traj_controller_->get_node()->configure(); // ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); @@ -1367,16 +1371,15 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"position", "velocity", "acceleration"}), std::vector({"position", "velocity", "acceleration"})))); -// // only velocity controller -// INSTANTIATE_TEST_SUITE_P( -// OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, -// ::testing::Values( -// std::make_tuple( -// std::vector({"velocity"}), -// std::vector({"position", "velocity"})), -// std::make_tuple( -// std::vector({"velocity"}), -// std::vector({"position", "velocity", "acceleration"})))); +// only velocity controller +INSTANTIATE_TEST_SUITE_P( + OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"velocity"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"velocity"}), + std::vector({"position", "velocity", "acceleration"})))); // // only effort controller // INSTANTIATE_TEST_SUITE_P( From e0a373ab09cdf8c268bc3c8d14c4a3c0db59b724 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 10 Jan 2023 23:01:59 +0100 Subject: [PATCH 076/126] [JTC] Add pid gain structure to documentation (#485) * Add pid gain structure to documentation * Remove trailing whitespace * Remove trailing colons --- joint_trajectory_controller/doc/userdoc.rst | 55 +++++++++++++++++---- 1 file changed, 45 insertions(+), 10 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 8230d7febb..5cfbb40852 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -105,37 +105,37 @@ A yaml file for using it could be: Details about parameters ^^^^^^^^^^^^^^^^^^^^^^^^ -joints (list(string)): +joints (list(string)) Joint names to control and listen to. -command_joints (list(string)): +command_joints (list(string)) Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names. -command_interface (list(string)): +command_interface (list(string)) Command interfaces provided by the hardware interface for all joints. Values: [position | velocity | acceleration] (multiple allowed) -state_interfaces (list(string)): +state_interfaces (list(string)) State interfaces provided by the hardware for all joints. Values: position (mandatory) [velocity, [acceleration]]. Acceleration interface can only be used in combination with position and velocity. -state_publish_rate (double): +state_publish_rate (double) Publish-rate of the controller's "state" topic. Default: 50.0 -action_monitor_rate (double): +action_monitor_rate (double) Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). Default: 20.0 -allow_partial_joints_goal (boolean): +allow_partial_joints_goal (boolean) Allow joint goals defining trajectory for only some joints. -open_loop_control (boolean): +open_loop_control (boolean) Use controller in open-loop control mode using ignoring the states provided by hardware interface and using last commands as states in the next control step. This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). If this flag is set, the controller tries to read the values from the command interfaces on starting. If they have real numeric values, those will be used instead of state interfaces. Therefore it is important set command interfaces to NaN (std::numeric_limits::quiet_NaN()) or state values when the hardware is started. @@ -153,16 +153,51 @@ constraints.goal_time (double) Default: 0.0 (not checked) -constraints..trajectory +constraints..trajectory (double) Maximally allowed deviation from the target trajectory for a given joint. Default: 0.0 (tolerance is not enforced) -constraints..goal +constraints..goal (double) Maximally allowed deviation from the goal (end of the trajectory) for a given joint. Default: 0.0 (tolerance is not enforced) +gains (structure) + If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. This structure contains the controller gains for every joint with the control law + + .. math:: + + u = k_ff v_d + k_p (s_d - s) + k_i \sum(s_d - s) dt + k_d (v_d - v) + + with the desired velocity :math:`v_d` and position :math:`s_d`, + the measured velocity :math:`v` and position :math:`s`, the controller period :math:`dt`, + and the ``velocity`` or ``effort`` setpoint :math:`u`, respectively. + +gains..p (double) + Proportional gain :math:`k_p` for PID + + Default: 0.0 + +gains..i (double) + Integral gain :math:`k_i` for PID + + Default: 0.0 + +gains..d (double) + Derivative gain :math:`k_d` for PID + + Default: 0.0 + +gains..i_clamp (double) + Integral clamp. Symmetrical in both positive and negative direction. + + Default: 0.0 + +gains..ff_velocity_scale (double) + Feed-forward scaling :math:`k_ff` of velocity + + Default: 0.0 ROS2 interface of the controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ From acaf91837838635fc1cde8da1a23498a14f03c12 Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Wed, 11 Jan 2023 18:59:32 +0100 Subject: [PATCH 077/126] diff_drive base_frame_id param (#495) changed default value from `odom` -> `base_link` --- diff_drive_controller/src/diff_drive_controller_parameter.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 82ba210113..fb50f2fb50 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -46,7 +46,7 @@ diff_drive_controller: } base_frame_id: { type: string, - default_value: "odom", + default_value: "base_link", description: "Name of the robot's base frame that is child of the odometry frame.", } pose_covariance_diagonal: { From 648b1351f220d34febbd1fcf1311cb2dd61c16b7 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 12 Jan 2023 09:12:17 +0000 Subject: [PATCH 078/126] Update changelogs --- admittance_controller/CHANGELOG.rst | 5 +++++ diff_drive_controller/CHANGELOG.rst | 8 ++++++++ effort_controllers/CHANGELOG.rst | 5 +++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 10 ++++++++++ position_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 5 +++++ tricycle_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 5 +++++ 15 files changed, 80 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index cc7fa5f2bf..8d22d15d43 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 3546af9bba..bcfc7d3573 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* diff_drive base_frame_id param (`#495 `_) + changed default value from `odom` -> `base_link` +* Add backward_ros to all controllers (`#489 `_) +* Remove compilation warnings from DiffDriveController (`#477 `_) +* Contributors: Bence Magyar, Denis Štogl, Jakub Delicat + 2.15.0 (2022-12-06) ------------------- * [DiffDriveController] Use generate parameter library (`#386 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6954b1f7e3..d156da8f65 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 3a37a17174..18c0aafdf6 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 0dc1db853b..e76e1c41ce 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index e11321aefe..a39b783eed 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- * Add basic gripper controller tests (`#459 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 0f26b4cbf2..5457affdd1 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 68d25c05aa..5da825bc04 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index bc59ab4960..0bbd1f3eab 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Add pid gain structure to documentation (`#485 `_) +* [JTC] Activate test for only velocity controller (`#487 `_) +* [JTC] Allow ff_velocity_scale=0 without deprecated warning (`#490 `_) +* Add backward_ros to all controllers (`#489 `_) +* Fix markup in userdoc.rst (`#480 `_) +* [JTC] Remove deprecation from parameters validation file. (`#476 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Denis Štogl + 2.15.0 (2022-12-06) ------------------- diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 115fac4e37..079f474cd9 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 0799e154c7..a44b6f10b0 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index f60cb9d72a..6195129137 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.15.0 (2022-12-06) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index d4b07df08b..c1c59d44cf 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 🔧 Fixes and updated on pre-commit hooks and their action (`#492 `_) +* Contributors: Denis Štogl + 2.15.0 (2022-12-06) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 0739bf6446..bcae54d64d 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Fix deprecation warnings when compiling (`#478 `_) +* Contributors: Bence Magyar, Denis Štogl + 2.15.0 (2022-12-06) ------------------- * [TricycleController] Removed “publish period” functionality ⏱ #abi-break #behavior-break (`#468 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index e47e52cdac..d40a89e5aa 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- From fd6a1d9244d83fac44d1e55724cbb88c565d3147 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 12 Jan 2023 09:13:04 +0000 Subject: [PATCH 079/126] 2.16.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 8d22d15d43..5c0538708d 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 69180a290f..cbd4e8fe69 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 2.15.0 + 2.16.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index bcfc7d3573..1295f4627e 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * diff_drive base_frame_id param (`#495 `_) changed default value from `odom` -> `base_link` * Add backward_ros to all controllers (`#489 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 4feec7fe37..efbed7275f 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.15.0 + 2.16.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index d156da8f65..6649f57519 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 0ca0d8dffb..8f1fec01c8 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.15.0 + 2.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 18c0aafdf6..a4fe25b808 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 174499c573..112f96ec64 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.15.0 + 2.16.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index e76e1c41ce..d165863adf 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f6ded4ff93..612f7025b3 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.15.0 + 2.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index a39b783eed..7aba44fa13 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 2734c54933..57f4103186 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.15.0 + 2.16.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 5457affdd1..a8f27c3be4 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 40836967c3..311cc9d4d6 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.15.0 + 2.16.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 5da825bc04..51d0f23c4b 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9fd5942d5d..b439073e9c 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.15.0 + 2.16.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 0bbd1f3eab..5da7cda268 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * [JTC] Add pid gain structure to documentation (`#485 `_) * [JTC] Activate test for only velocity controller (`#487 `_) * [JTC] Allow ff_velocity_scale=0 without deprecated warning (`#490 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index f64bdf635e..55edbb5d6b 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.15.0 + 2.16.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 079f474cd9..cd4ca184f0 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 086afccc1f..e7b47e3ed8 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.15.0 + 2.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index a44b6f10b0..1cbfbc61d2 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index e91679802c..40bacb9edc 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.15.0 + 2.16.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 6195129137..7052690d65 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 269d1da466..2243905f88 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.15.0 + 2.16.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index ed3ae63656..a23d99e206 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.15.0", + version="2.16.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index c1c59d44cf..83bb1bd382 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * 🔧 Fixes and updated on pre-commit hooks and their action (`#492 `_) * Contributors: Denis Štogl diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 81e69f88fd..096f822988 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.15.0 + 2.16.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index afb93527f1..a1d67a4c6a 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.15.0", + version="2.16.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index bcae54d64d..062188197e 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Fix deprecation warnings when compiling (`#478 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 3c853872e9..e4147fe2c5 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.15.0 + 2.16.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index d40a89e5aa..3eb3798c5a 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-12) +------------------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 03f1bed657..c746c8101a 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.15.0 + 2.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From f4788c594026bc3ecf0d944e1fa39f3f2d40673b Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 13 Jan 2023 14:23:34 +0100 Subject: [PATCH 080/126] [CI] Add dependabot to automatically update actions in workflows --- .github/dependabot.yml | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 .github/dependabot.yml diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000000..05a48fc654 --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,13 @@ +# To get started with Dependabot version updates, you'll need to specify which +# package ecosystems to update and where the package manifests are located. +# Please see the documentation for all configuration options: +# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates + +version: 2 +updates: + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" From 8cfa22dce5e8c6c201b0f50f1a6453bfa0c78030 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 17 Jan 2023 07:27:32 +0000 Subject: [PATCH 081/126] Bump actions/setup-python from 4.4.0 to 4.5.0 (#499) Bumps [actions/setup-python](https://github.com/actions/setup-python) from 4.4.0 to 4.5.0. - [Release notes](https://github.com/actions/setup-python/releases) - [Commits](https://github.com/actions/setup-python/compare/v4.4.0...v4.5.0) --- updated-dependencies: - dependency-name: actions/setup-python dependency-type: direct:production update-type: version-update:semver-minor ... Signed-off-by: dependabot[bot] Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index b54c76d3e5..9816e137c6 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,7 +12,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: actions/setup-python@v4.4.0 + - uses: actions/setup-python@v4.5.0 with: python-version: '3.10' - name: Install system hooks From ccc3ed2e35c2773b3d296f0ccf7c7116ecb41c51 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 17 Jan 2023 07:27:51 +0000 Subject: [PATCH 082/126] Bump ros-tooling/setup-ros from 0.3.4 to 0.4.2 (#500) Bumps [ros-tooling/setup-ros](https://github.com/ros-tooling/setup-ros) from 0.3.4 to 0.4.2. - [Release notes](https://github.com/ros-tooling/setup-ros/releases) - [Commits](https://github.com/ros-tooling/setup-ros/compare/0.3.4...0.4.2) --- updated-dependencies: - dependency-name: ros-tooling/setup-ros dependency-type: direct:production update-type: version-update:semver-minor ... Signed-off-by: dependabot[bot] Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-ros-lint.yml | 4 ++-- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index d9b4ec4494..2cf4712d1f 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -16,7 +16,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.3.4 + - uses: ros-tooling/setup-ros@0.4.2 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index d8f77a323a..87d1c8f959 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -12,7 +12,7 @@ jobs: linter: [cppcheck, copyright, lint_cmake] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@v0.2 + - uses: ros-tooling/setup-ros@0.4.2 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling @@ -41,7 +41,7 @@ jobs: linter: [cpplint] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@v0.2 + - uses: ros-tooling/setup-ros@0.4.2 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 2ad549b0f8..3c1fb519d1 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.3.4 + - uses: ros-tooling/setup-ros@0.4.2 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 From 2d0f5b4c6a2307763b39d86576003dc7738e8a75 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 17 Jan 2023 07:28:10 +0000 Subject: [PATCH 083/126] Bump codecov/codecov-action from 3.1.0 to 3.1.1 (#501) Bumps [codecov/codecov-action](https://github.com/codecov/codecov-action) from 3.1.0 to 3.1.1. - [Release notes](https://github.com/codecov/codecov-action/releases) - [Changelog](https://github.com/codecov/codecov-action/blob/main/CHANGELOG.md) - [Commits](https://github.com/codecov/codecov-action/compare/v3.1.0...v3.1.1) --- updated-dependencies: - dependency-name: codecov/codecov-action dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 2cf4712d1f..79e0b96ca6 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -37,7 +37,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.0 + - uses: codecov/codecov-action@v3.1.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests From 1c6f26768c43f8224fb71fe44fbfafdfdc314543 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 17 Jan 2023 07:28:38 +0000 Subject: [PATCH 084/126] Bump ros-tooling/action-ros-ci from 0.2.6 to 0.2.7 (#502) Bumps [ros-tooling/action-ros-ci](https://github.com/ros-tooling/action-ros-ci) from 0.2.6 to 0.2.7. - [Release notes](https://github.com/ros-tooling/action-ros-ci/releases) - [Commits](https://github.com/ros-tooling/action-ros-ci/compare/0.2.6...0.2.7) --- updated-dependencies: - dependency-name: ros-tooling/action-ros-ci dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 79e0b96ca6..82c37c746a 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -20,7 +20,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 - - uses: ros-tooling/action-ros-ci@0.2.6 + - uses: ros-tooling/action-ros-ci@0.2.7 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 3c1fb519d1..d5b4efd437 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -32,7 +32,7 @@ jobs: - uses: actions/checkout@v3 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.2.6 + - uses: ros-tooling/action-ros-ci@0.2.7 with: target-ros2-distro: ${{ inputs.ros_distro }} # build all packages listed in the meta package From 2f9f4ea238480d97bc123bf2b4f2c0d104962b9d Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 17 Jan 2023 07:28:57 +0000 Subject: [PATCH 085/126] Bump pat-s/always-upload-cache from 2.1.5 to 3.0.11 (#503) Bumps [pat-s/always-upload-cache](https://github.com/pat-s/always-upload-cache) from 2.1.5 to 3.0.11. - [Release notes](https://github.com/pat-s/always-upload-cache/releases) - [Changelog](https://github.com/pat-s/always-upload-cache/blob/main/RELEASES.md) - [Commits](https://github.com/pat-s/always-upload-cache/compare/v2.1.5...v3.0.11) --- updated-dependencies: - dependency-name: pat-s/always-upload-cache dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/reusable-industrial-ci-with-cache.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml index 490b680e7c..bca08ce5d2 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -66,14 +66,14 @@ jobs: ref: ${{ inputs.ref_for_scheduled_build }} - name: cache target_ws if: ${{ ! matrix.env.CCOV }} - uses: pat-s/always-upload-cache@v2.1.5 + uses: pat-s/always-upload-cache@v3.0.11 with: path: ${{ env.BASEDIR }}/target_ws key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }} restore-keys: | target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} - name: cache ccache - uses: pat-s/always-upload-cache@v2.1.5 + uses: pat-s/always-upload-cache@v3.0.11 with: path: ${{ env.CCACHE_DIR }} key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} From 628fe779c12ffeb7aad46a19e99e0567658c9583 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 19 Jan 2023 07:48:38 +0000 Subject: [PATCH 086/126] Revert "2.16.0" This reverts commit fd6a1d9244d83fac44d1e55724cbb88c565d3147. --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 5c0538708d..8d22d15d43 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index cbd4e8fe69..69180a290f 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 2.16.0 + 2.15.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 1295f4627e..bcfc7d3573 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * diff_drive base_frame_id param (`#495 `_) changed default value from `odom` -> `base_link` * Add backward_ros to all controllers (`#489 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index efbed7275f..4feec7fe37 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.16.0 + 2.15.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6649f57519..d156da8f65 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 8f1fec01c8..0ca0d8dffb 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.16.0 + 2.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index a4fe25b808..18c0aafdf6 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 112f96ec64..174499c573 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.16.0 + 2.15.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index d165863adf..e76e1c41ce 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 612f7025b3..f6ded4ff93 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.16.0 + 2.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 7aba44fa13..a39b783eed 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 57f4103186..2734c54933 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.16.0 + 2.15.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index a8f27c3be4..5457affdd1 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 311cc9d4d6..40836967c3 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.16.0 + 2.15.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 51d0f23c4b..5da825bc04 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index b439073e9c..9fd5942d5d 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.16.0 + 2.15.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 5da7cda268..0bbd1f3eab 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * [JTC] Add pid gain structure to documentation (`#485 `_) * [JTC] Activate test for only velocity controller (`#487 `_) * [JTC] Allow ff_velocity_scale=0 without deprecated warning (`#490 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 55edbb5d6b..f64bdf635e 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.16.0 + 2.15.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index cd4ca184f0..079f474cd9 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/position_controllers/package.xml b/position_controllers/package.xml index e7b47e3ed8..086afccc1f 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.16.0 + 2.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 1cbfbc61d2..a44b6f10b0 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 40bacb9edc..e91679802c 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.16.0 + 2.15.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 7052690d65..6195129137 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 2243905f88..269d1da466 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.16.0 + 2.15.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index a23d99e206..ed3ae63656 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.16.0", + version="2.15.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 83bb1bd382..c1c59d44cf 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * 🔧 Fixes and updated on pre-commit hooks and their action (`#492 `_) * Contributors: Denis Štogl diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 096f822988..81e69f88fd 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.16.0 + 2.15.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index a1d67a4c6a..afb93527f1 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.16.0", + version="2.15.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 062188197e..bcae54d64d 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Fix deprecation warnings when compiling (`#478 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index e4147fe2c5..3c853872e9 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.16.0 + 2.15.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 3eb3798c5a..d40a89e5aa 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.16.0 (2023-01-12) -------------------- +Forthcoming +----------- * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index c746c8101a..03f1bed657 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.16.0 + 2.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From f617b28be3ff82c8882bb83a32c1ee17e3b1859c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 19 Jan 2023 07:49:19 +0000 Subject: [PATCH 087/126] 3.0.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 8d22d15d43..766cc8429e 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 69180a290f..f9794297c9 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 2.15.0 + 3.0.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index bcfc7d3573..bc295586e5 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * diff_drive base_frame_id param (`#495 `_) changed default value from `odom` -> `base_link` * Add backward_ros to all controllers (`#489 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 4feec7fe37..8617f5fb1a 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.15.0 + 3.0.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index d156da8f65..c93a755898 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 0ca0d8dffb..f5d69e0d7e 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.15.0 + 3.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 18c0aafdf6..d5f331bba6 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 174499c573..1050d7ca32 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.15.0 + 3.0.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index e76e1c41ce..25265a6680 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f6ded4ff93..f54a0b328a 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.15.0 + 3.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index a39b783eed..d0fc61b87d 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 2734c54933..946347e2db 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.15.0 + 3.0.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 5457affdd1..0030a214ce 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 40836967c3..da27be0b33 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.15.0 + 3.0.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 5da825bc04..c4f2fc3c43 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9fd5942d5d..01ee249201 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.15.0 + 3.0.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 0bbd1f3eab..c84b521cb0 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * [JTC] Add pid gain structure to documentation (`#485 `_) * [JTC] Activate test for only velocity controller (`#487 `_) * [JTC] Allow ff_velocity_scale=0 without deprecated warning (`#490 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index f64bdf635e..9eb02f1039 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.15.0 + 3.0.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 079f474cd9..57182eb6e7 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 086afccc1f..8b582130a5 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.15.0 + 3.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index a44b6f10b0..07938dcf99 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index e91679802c..3e9a0b53ab 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.15.0 + 3.0.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 6195129137..39e6646486 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 269d1da466..7c64aceed1 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.15.0 + 3.0.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index ed3ae63656..82a97cbef6 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.15.0", + version="3.0.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index c1c59d44cf..d0c5983bbb 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * 🔧 Fixes and updated on pre-commit hooks and their action (`#492 `_) * Contributors: Denis Štogl diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 81e69f88fd..e4b5a70b4b 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 2.15.0 + 3.0.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index afb93527f1..bb54029c86 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.15.0", + version="3.0.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index bcae54d64d..3e811b8a42 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Fix deprecation warnings when compiling (`#478 `_) * Contributors: Bence Magyar, Denis Štogl diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 3c853872e9..90a0967f17 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.15.0 + 3.0.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index d40a89e5aa..bd9bea87eb 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.0.0 (2023-01-19) +------------------ * Add backward_ros to all controllers (`#489 `_) * Contributors: Bence Magyar diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 03f1bed657..0cb0cacf13 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.15.0 + 3.0.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 67e5d4d394921212aa8f00eb09a68e51ef1d581e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 20 Jan 2023 10:55:25 +0100 Subject: [PATCH 088/126] [JTC] Configurable joint positon error normalization behavior (#491) * Use the same variables for state feedback and PID loop * Make joint_error normalization configurable * Activate test for only velocity controller * Allow ff_velocity_scale=0 without deprecated warning * Add test for setpoint due to normalized position error * Update comments in test Co-authored-by: Bence Magyar --- .../joint_trajectory_controller.hpp | 2 + .../src/joint_trajectory_controller.cpp | 24 ++- ...oint_trajectory_controller_parameters.yaml | 5 + .../test/test_trajectory_controller.cpp | 204 ++++++++++++++++++ .../test/test_trajectory_controller_utils.hpp | 17 +- 5 files changed, 242 insertions(+), 10 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index a229489422..3eff7f17b1 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -166,6 +166,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::vector pids_; // Feed-forward velocity weight factor when calculating closed loop pid adapter's command std::vector ff_velocity_scale_; + // Configuration for every joint, if position error is normalized + std::vector normalize_joint_error_; // reserved storage for result of the command when closed loop pid adapter is used std::vector tmp_command_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 8df4193361..733018b924 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -125,8 +125,17 @@ controller_interface::return_type JointTrajectoryController::update( const JointTrajectoryPoint & desired) { // error defined as the difference between current and desired - error.positions[index] = - angles::shortest_angular_distance(current.positions[index], desired.positions[index]); + if (normalize_joint_error_[index]) + { + // if desired, the shortest_angular_distance is calculated, i.e., the error is + // normalized between -picomputeCommand( - state_desired_.positions[i] - state_current_.positions[i], - state_desired_.velocities[i] - state_current_.velocities[i], + state_error_.positions[i], state_error_.velocities[i], (uint64_t)period.nanoseconds()); } } @@ -567,6 +575,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( } } + // Configure joint position error normalization from ROS parameters + normalize_joint_error_.resize(dof_); + for (size_t i = 0; i < dof_; ++i) + { + const auto & gains = params_.gains.joints_map.at(params_.joints[i]); + normalize_joint_error_[i] = gains.normalize_error; + } + if (params_.state_interfaces.empty()) { RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 80aa32585b..f767b15b27 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -101,6 +101,11 @@ joint_trajectory_controller: default_value: 0.0, description: "Feed-forward scaling of velocity." } + normalize_error: { + type: bool, + default_value: false, + description: "Use position error normalization to -pi to pi." + } constraints: stopped_velocity_tolerance: { type: double, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 216c574228..511ae0869e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -487,6 +487,210 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); } +// Floating-point value comparison threshold +const double EPS = 1e-6; +/** + * @brief check if position error of revolute joints are normalized if not configured so +*/ +TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, false); + subscribeToState(); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + const auto allowed_delta = 0.1; + + // no update of state_interface + EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->desired.positions.size()); + EXPECT_EQ(n_joints, state_msg->actual.positions.size()); + EXPECT_EQ(n_joints, state_msg->error.positions.size()); + + // are the correct desired positions used? + EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta); + + // no normalization of position error + EXPECT_NEAR( + state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR( + state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR( + state_msg->error.positions[2], state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2], EPS); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); + EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); + EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_vel_[0]); + EXPECT_LE(0.0, joint_vel_[1]); + EXPECT_LE(0.0, joint_vel_[2]); + + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) + EXPECT_NEAR( + k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * allowed_delta); + EXPECT_NEAR( + k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * allowed_delta); + // no normalization of position error + EXPECT_NEAR( + k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], + k_p * allowed_delta); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_eff_[0]); + EXPECT_LE(0.0, joint_eff_[1]); + EXPECT_LE(0.0, joint_eff_[2]); + } + + executor.cancel(); +} + +/** + * @brief check if position error of revolute joints are normalized if configured so +*/ +TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, true); + subscribeToState(); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + const auto allowed_delta = 0.1; + + // no update of state_interface + EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->desired.positions.size()); + EXPECT_EQ(n_joints, state_msg->actual.positions.size()); + EXPECT_EQ(n_joints, state_msg->error.positions.size()); + + // are the correct desired positions used? + EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta); + + // is error.positions[2] normalized? + EXPECT_NEAR( + state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR( + state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR( + state_msg->error.positions[2], + state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); + EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); + EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_vel_[0]); + EXPECT_LE(0.0, joint_vel_[1]); + + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + EXPECT_GE(0.0, joint_vel_[2]); + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] + EXPECT_NEAR( + k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * allowed_delta); + EXPECT_NEAR( + k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * allowed_delta); + // is error of positions[2] normalized? + EXPECT_NEAR( + k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], + k_p * allowed_delta); + } + else + { + // interpolated points_velocities only + EXPECT_LE(0.0, joint_vel_[2]); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_eff_[0]); + EXPECT_LE(0.0, joint_eff_[1]); + EXPECT_LE(0.0, joint_eff_[2]); + } + + executor.cancel(); +} + void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) { rclcpp::Parameter state_publish_rate_param( diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 0d523cc88d..ca58b124a5 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -112,6 +112,8 @@ class TestableJointTrajectoryController bool has_effort_command_interface() { return has_effort_command_interface_; } + bool use_closed_loop_pid_adapter() { return use_closed_loop_pid_adapter_; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -171,7 +173,8 @@ class TrajectoryControllerTest : public ::testing::Test node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params}); } - void SetPidParameters() + void SetPidParameters( + double p_default = 0.0, double ff_default = 1.0, bool normalize_error_default = false) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); @@ -179,24 +182,26 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < joint_names_.size(); ++i) { const std::string prefix = "gains." + joint_names_[i]; - const rclcpp::Parameter k_p(prefix + ".p", 0.0); + const rclcpp::Parameter k_p(prefix + ".p", p_default); const rclcpp::Parameter k_i(prefix + ".i", 0.0); const rclcpp::Parameter k_d(prefix + ".d", 0.0); const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); - const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", 1.0); - node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); + const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_default); + const rclcpp::Parameter normalize_error(prefix + ".normalize_error", normalize_error_default); + node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, normalize_error}); } } void SetUpAndActivateTrajectoryController( rclcpp::Executor & executor, bool use_local_parameters = true, const std::vector & parameters = {}, - bool separate_cmd_and_state_values = false) + bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, + bool normalize_error = false) { SetUpTrajectoryController(executor, use_local_parameters); // set pid parameters before configure - SetPidParameters(); + SetPidParameters(k_p, ff, normalize_error); for (const auto & param : parameters) { traj_controller_->get_node()->set_parameter(param); From 691e010a95dd3284fff32070d535cb3ae73e14af Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 24 Jan 2023 09:04:21 +0000 Subject: [PATCH 089/126] Bump actions/upload-artifact from 3.1.0 to 3.1.2 (#508) Bumps [actions/upload-artifact](https://github.com/actions/upload-artifact) from 3.1.0 to 3.1.2. - [Release notes](https://github.com/actions/upload-artifact/releases) - [Commits](https://github.com/actions/upload-artifact/compare/v3.1.0...v3.1.2) --- updated-dependencies: - dependency-name: actions/upload-artifact dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 82c37c746a..04cc3953d7 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -42,7 +42,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.0 + - uses: actions/upload-artifact@v3.1.2 with: name: colcon-logs-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index d5b4efd437..ceb4facde7 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -43,7 +43,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v1 + - uses: actions/upload-artifact@v3.1.2 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log From a741d4d75031cdafbb970ff0ccf2f2f8df3faaf1 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Thu, 26 Jan 2023 08:26:17 +0100 Subject: [PATCH 090/126] add publisher topic parameter to forward_position_controller (#494) --- .../publisher_forward_position_controller.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py index 60ddd75854..59a39164ec 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py @@ -25,14 +25,14 @@ class PublisherForwardPosition(Node): def __init__(self): super().__init__("publisher_forward_position_controller") # Declare all parameters - self.declare_parameter("controller_name", "forward_position_controller") + self.declare_parameter("publish_topic", "/position_commands") self.declare_parameter("wait_sec_between_publish", 5) self.declare_parameter("goal_names", ["pos1", "pos2"]) # Read parameters - controller_name = self.get_parameter("controller_name").value wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value goal_names = self.get_parameter("goal_names").value + publish_topic = self.get_parameter("publish_topic").value # Read all positions from parameters self.goals = [] @@ -45,8 +45,6 @@ def __init__(self): float_goal = [float(value) for value in goal] self.goals.append(float_goal) - publish_topic = "/" + controller_name + "/" + "commands" - self.get_logger().info( f'Publishing {len(goal_names)} goals on topic "{publish_topic}"\ every {wait_sec_between_publish} s' From 7c79feb7b3e76f14899efc0e3baf8a768577c90f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 26 Jan 2023 11:34:38 +0100 Subject: [PATCH 091/126] ported the joint_trajectory_controller query_state service to ROS2 (#481) * ported the joint_trajectory_controller query_state service to ROS2 * reiterate over the implementation and simplify it * updated test for long past time and some comments on service * address PR review changes Co-authored-by: Bence Magyar --- .../joint_trajectory_controller.hpp | 7 +++ .../src/joint_trajectory_controller.cpp | 51 +++++++++++++++++++ .../test/test_trajectory.cpp | 11 ++++ 3 files changed, 69 insertions(+) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 3eff7f17b1..45d979b152 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -24,6 +24,7 @@ #include "control_msgs/action/follow_joint_trajectory.hpp" #include "control_msgs/msg/joint_trajectory_controller_state.hpp" +#include "control_msgs/srv/query_trajectory_state.hpp" #include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -176,6 +177,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Subscription::SharedPtr joint_command_subscriber_ = nullptr; + rclcpp::Service::SharedPtr query_state_srv_; + std::shared_ptr * traj_point_active_ptr_ = nullptr; std::shared_ptr traj_external_point_ptr_ = nullptr; std::shared_ptr traj_home_point_ptr_ = nullptr; @@ -256,6 +259,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool read_state_from_command_interfaces(JointTrajectoryPoint & state); + void query_state_service( + const std::shared_ptr request, + std::shared_ptr response); + private: bool contains_interface_type( const std::vector & interface_type_list, const std::string & interface_type); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 733018b924..1e0ded289b 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -470,6 +470,53 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto return has_values; } +void JointTrajectoryController::query_state_service( + const std::shared_ptr request, + std::shared_ptr response) +{ + const auto logger = get_node()->get_logger(); + // Preconditions + if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + RCLCPP_ERROR(logger, "Can't sample trajectory. Controller is not active."); + response->success = false; + return; + } + const auto active_goal = *rt_active_goal_.readFromRT(); + response->name = params_.joints; + trajectory_msgs::msg::JointTrajectoryPoint state_requested = state_current_; + if ((traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg())) + { + TrajectoryPointConstIter start_segment_itr, end_segment_itr; + response->success = (*traj_point_active_ptr_) + ->sample( + static_cast(request->time), interpolation_method_, + state_requested, start_segment_itr, end_segment_itr); + // If the requested sample time precedes the trajectory finish time respond as failure + if (response->success) + { + if (end_segment_itr == (*traj_point_active_ptr_)->end()) + { + RCLCPP_ERROR(logger, "Requested sample time precedes the current trajectory end time."); + response->success = false; + } + } + else + { + RCLCPP_ERROR( + logger, "Requested sample time is earlier than the current trajectory start time."); + } + } + else + { + RCLCPP_ERROR(logger, "Currently there is no valid trajectory instance."); + response->success = false; + } + response->position = state_requested.positions; + response->velocity = state_requested.velocities; + response->acceleration = state_requested.accelerations; +} + controller_interface::CallbackReturn JointTrajectoryController::on_configure( const rclcpp_lifecycle::State &) { @@ -721,6 +768,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( resize_joint_trajectory_point(state_error_, dof_); resize_joint_trajectory_point(last_commanded_state_, dof_); + query_state_srv_ = get_node()->create_service( + std::string(get_node()->get_name()) + "/query_state", + std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); + return CallbackReturn::SUCCESS; } diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index fbf198300b..b52aa67a04 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -189,6 +189,17 @@ TEST(TestTrajectory, sample_trajectory_positions) ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } + + // sample long past given points for same trajectory, it should receive the same end point + // so later in the query_state_service we set it to failure + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(30.0), DEFAULT_INTERPOLATION, expected_state, start, + end); + ASSERT_EQ((--traj.end()), start); + ASSERT_EQ(traj.end(), end); + EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); + } } TEST(TestTrajectory, interpolation_pos_vel) From ec80aba8d64a222ae482476791ce4d0040803498 Mon Sep 17 00:00:00 2001 From: Dan Wahl Date: Thu, 26 Jan 2023 03:54:09 -0700 Subject: [PATCH 092/126] Changing to_chrono to use nanoseconds (#507) * Changing to_chrono to use nanoseconds Previously std::chrono::seconds, which rounds to 0 if action_monitor_period_ < 1 * Reset gripper action goal timer to match JTC impl --- .../gripper_controllers/gripper_action_controller_impl.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index e28495fd1e..629f749050 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -105,9 +105,13 @@ void GripperActionController::accepted_callback( rt_active_goal_ = rt_goal; rt_active_goal_->execute(); } + + // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list + goal_handle_timer_.reset(); + // Setup goal status checking timer goal_handle_timer_ = get_node()->create_wall_timer( - action_monitor_period_.to_chrono(), + action_monitor_period_.to_chrono(), std::bind(&RealtimeGoalHandle::runNonRealtime, rt_active_goal_)); } From ae3df62ccdba42473141fe1ebc5fb45c3923edb7 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 26 Jan 2023 12:48:10 +0000 Subject: [PATCH 093/126] Update changelogs --- admittance_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 3 +++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 6 ++++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 5 +++++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 15 files changed, 52 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 766cc8429e..00362994d9 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index bc295586e5..90ec702ab4 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ * diff_drive base_frame_id param (`#495 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index c93a755898..397c64e569 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index d5f331bba6..82c921f0ab 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 25265a6680..4dbb74e174 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index d0fc61b87d..bc42d03a91 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Changing to_chrono to use nanoseconds & Reset gripper action goal timer to match JTC impl (`#507 `_) +* Contributors: Dan Wahl + 3.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 0030a214ce..ea09f0f4ee 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index c4f2fc3c43..6fdfff8463 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index c84b521cb0..c06ce877ec 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* ported the joint_trajectory_controller query_state service to ROS2 (`#481 `_) +* [JTC] Configurable joint positon error normalization behavior (`#491 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Bence Magyar + 3.0.0 (2023-01-19) ------------------ * [JTC] Add pid gain structure to documentation (`#485 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 57182eb6e7..5c112f681d 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 07938dcf99..745b489882 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 39e6646486..8f29332391 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* add publisher topic parameter to forward_position_controller (`#494 `_) +* Contributors: Manuel Muth + 3.0.0 (2023-01-19) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index d0c5983bbb..7966781559 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ * 🔧 Fixes and updated on pre-commit hooks and their action (`#492 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 3e811b8a42..cdc8804291 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index bd9bea87eb..eb75415edd 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.0.0 (2023-01-19) ------------------ * Add backward_ros to all controllers (`#489 `_) From 5298e73ebb175e0c3d08c40ea4f75c8492d5ccb7 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 26 Jan 2023 12:48:47 +0000 Subject: [PATCH 094/126] 3.1.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 00362994d9..85f54999dd 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index f9794297c9..6783a5d3cd 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.0.0 + 3.1.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 90ec702ab4..69c60f6cd6 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 8617f5fb1a..baff27bfe2 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.0.0 + 3.1.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 397c64e569..4517ec229e 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index f5d69e0d7e..56d136045d 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.0.0 + 3.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 82c921f0ab..a09cc78472 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 1050d7ca32..28ac065ec0 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.0.0 + 3.1.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 4dbb74e174..de39f35666 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f54a0b328a..ead0fbb536 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.0.0 + 3.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index bc42d03a91..6c72b59184 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ * Changing to_chrono to use nanoseconds & Reset gripper action goal timer to match JTC impl (`#507 `_) * Contributors: Dan Wahl diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 946347e2db..30af5a6302 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 3.0.0 + 3.1.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index ea09f0f4ee..455112d852 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index da27be0b33..6a4e025c5e 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.0.0 + 3.1.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 6fdfff8463..529a197858 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 01ee249201..4ef019d7a1 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.0.0 + 3.1.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index c06ce877ec..a7f1fa723d 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ * ported the joint_trajectory_controller query_state service to ROS2 (`#481 `_) * [JTC] Configurable joint positon error normalization behavior (`#491 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Bence Magyar diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 9eb02f1039..fd1eec3679 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.0.0 + 3.1.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 5c112f681d..ed743b759c 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 8b582130a5..d9d2852d9e 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.0.0 + 3.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 745b489882..a86ee0c76e 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 3e9a0b53ab..22951780d2 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.0.0 + 3.1.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 8f29332391..1521d21aae 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ * add publisher topic parameter to forward_position_controller (`#494 `_) * Contributors: Manuel Muth diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 7c64aceed1..e1be6359bb 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.0.0 + 3.1.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 82a97cbef6..ded28dba8f 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="3.0.0", + version="3.1.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 7966781559..987db88ed8 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index e4b5a70b4b..6d696618c7 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 3.0.0 + 3.1.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index bb54029c86..48b57fe993 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="3.0.0", + version="3.1.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index cdc8804291..122763475b 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 90a0967f17..9db6233bc8 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.0.0 + 3.1.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index eb75415edd..64a674146b 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.1.0 (2023-01-26) +------------------ 3.0.0 (2023-01-19) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 0cb0cacf13..d47cfbad59 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.0.0 + 3.1.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 6594e72b8768b01f07b5bc7518ccf6e55d17dea6 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 31 Jan 2023 07:51:48 +0000 Subject: [PATCH 095/126] Bump ros-tooling/setup-ros from 0.4.2 to 0.5.0 (#513) Bumps [ros-tooling/setup-ros](https://github.com/ros-tooling/setup-ros) from 0.4.2 to 0.5.0. - [Release notes](https://github.com/ros-tooling/setup-ros/releases) - [Commits](https://github.com/ros-tooling/setup-ros/compare/0.4.2...0.5.0) --- updated-dependencies: - dependency-name: ros-tooling/setup-ros dependency-type: direct:production update-type: version-update:semver-minor ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-ros-lint.yml | 4 ++-- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 04cc3953d7..ada62cd1c3 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -16,7 +16,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.4.2 + - uses: ros-tooling/setup-ros@0.5.0 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 87d1c8f959..e5534056b8 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -12,7 +12,7 @@ jobs: linter: [cppcheck, copyright, lint_cmake] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.4.2 + - uses: ros-tooling/setup-ros@0.5.0 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling @@ -41,7 +41,7 @@ jobs: linter: [cpplint] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.4.2 + - uses: ros-tooling/setup-ros@0.5.0 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index ceb4facde7..f3e258a2e5 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.4.2 + - uses: ros-tooling/setup-ros@0.5.0 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 From d9577d1d753971c4786d9a8d3edbeb9c83ee80cf Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 7 Feb 2023 17:39:33 +0100 Subject: [PATCH 096/126] Remove compile warnings. (#519) --- diff_drive_controller/src/diff_drive_controller.cpp | 4 ++-- joint_trajectory_controller/src/trajectory.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 75d8b1fa35..b6d9b07a49 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -150,7 +150,7 @@ controller_interface::return_type DiffDriveController::update( { double left_feedback_mean = 0.0; double right_feedback_mean = 0.0; - for (size_t index = 0; index < params_.wheels_per_side; ++index) + for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) { const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value(); const double right_feedback = @@ -244,7 +244,7 @@ controller_interface::return_type DiffDriveController::update( (linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius; // Set wheels velocities: - for (size_t index = 0; index < params_.wheels_per_side; ++index) + for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) { registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left); registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right); diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index f28703efc9..0bdc5f7c82 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -19,8 +19,8 @@ #include "hardware_interface/macros.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" -#include "rcppmath/clamp.hpp" #include "std_msgs/msg/header.hpp" + namespace joint_trajectory_controller { Trajectory::Trajectory() : trajectory_start_time_(0), time_before_traj_msg_(0) {} From 762ae2b4b6012292d0f0379b215b3809deee58ae Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 7 Feb 2023 10:40:54 -0600 Subject: [PATCH 097/126] Don't set interpolation_method_ twice (#517) --- .../src/joint_trajectory_controller.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 1e0ded289b..101dec8c68 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -57,9 +57,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() // Create the parameter listener and get the parameters param_listener_ = std::make_shared(get_node()); params_ = param_listener_->get_params(); - - // Set interpolation method from string parameter - interpolation_method_ = interpolation_methods::from_string(params_.interpolation_method); } catch (const std::exception & e) { From 6369faaa8a8bcd1bf02710370690ad9290488cfc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A1rk=20Szitanics?= Date: Wed, 8 Feb 2023 08:30:42 +0100 Subject: [PATCH 098/126] Fix JTC segfault on unload (#515) This commit fixes the segfault which happens when the JTC is unloaded, if it is in inactive state but was never activated. --- .../src/joint_trajectory_controller.cpp | 7 +++++-- .../test/test_trajectory_controller.cpp | 16 ++++++++++++++++ 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 101dec8c68..99ff99515c 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -885,8 +885,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_cleanup( const rclcpp_lifecycle::State &) { // go home - traj_home_point_ptr_->update(traj_msg_home_ptr_); - traj_point_active_ptr_ = &traj_home_point_ptr_; + if (traj_home_point_ptr_ != nullptr) + { + traj_home_point_ptr_->update(traj_msg_home_ptr_); + traj_point_active_ptr_ = &traj_home_point_ptr_; + } return CallbackReturn::SUCCESS; } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 511ae0869e..8c027b4ae3 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -349,6 +349,22 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) executor.cancel(); } +TEST_P(TrajectoryControllerTestParameterized, cleanup_after_configure) +{ + rclcpp::executors::MultiThreadedExecutor executor; + SetUpTrajectoryController(executor); + + // configure controller + auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + + // cleanup controller + state = traj_controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + + executor.cancel(); +} + TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_parameters) { rclcpp::executors::MultiThreadedExecutor executor; From 2a870b26d10e9f29e29dd02d22cbfc4f8db9b78b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 10 Feb 2023 17:14:06 +0100 Subject: [PATCH 099/126] Add JTC normalize_error parameter to doc (#511) * Add normalize_error parameter to doc * Add usecase for normalize_error * Update alignment Co-authored-by: Dr. Denis --------- Co-authored-by: Dr. Denis --- joint_trajectory_controller/doc/userdoc.rst | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 5cfbb40852..9abb84545a 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -168,11 +168,10 @@ gains (structure) .. math:: - u = k_ff v_d + k_p (s_d - s) + k_i \sum(s_d - s) dt + k_d (v_d - v) + u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) - with the desired velocity :math:`v_d` and position :math:`s_d`, - the measured velocity :math:`v` and position :math:`s`, the controller period :math:`dt`, - and the ``velocity`` or ``effort`` setpoint :math:`u`, respectively. + with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below), + the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. gains..p (double) Proportional gain :math:`k_p` for PID @@ -195,10 +194,18 @@ gains..i_clamp (double) Default: 0.0 gains..ff_velocity_scale (double) - Feed-forward scaling :math:`k_ff` of velocity + Feed-forward scaling :math:`k_{ff}` of velocity Default: 0.0 +gains..normalize_error (bool) + If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. + Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured + position :math:`s` from the state interface. Use this for revolute joints without end stop, + where the shortest rotation to the target position is the desired motion. + + Default: false + ROS2 interface of the controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ From 2ce1f21c4864bcea2707d596fe59d1f814fadf04 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 10 Feb 2023 09:24:42 -0700 Subject: [PATCH 100/126] Fix overriding of install (#510) * Fix overriding of install Signed-off-by: Tyler Weaver * Update admittance_controller/CMakeLists.txt Co-authored-by: Chris Thrasher * Update ros2_controllers/CMakeLists.txt Co-authored-by: Chris Thrasher * Remove unused lists * Use THIS_PACKAGE_INCLUDE_DEPENDS list --------- Signed-off-by: Tyler Weaver Co-authored-by: Chris Thrasher --- admittance_controller/CMakeLists.txt | 89 +++++++------- admittance_controller/package.xml | 2 - diff_drive_controller/CMakeLists.txt | 90 ++++++--------- diff_drive_controller/package.xml | 3 +- effort_controllers/CMakeLists.txt | 77 +++++-------- effort_controllers/package.xml | 3 +- .../CMakeLists.txt | 97 +++++++--------- force_torque_sensor_broadcaster/package.xml | 1 - forward_command_controller/CMakeLists.txt | 99 ++++++++-------- forward_command_controller/package.xml | 3 +- gripper_controllers/CMakeLists.txt | 52 ++++----- gripper_controllers/package.xml | 3 +- imu_sensor_broadcaster/CMakeLists.txt | 67 +++++------ imu_sensor_broadcaster/package.xml | 1 - joint_state_broadcaster/CMakeLists.txt | 108 ++++++++--------- joint_state_broadcaster/package.xml | 11 +- joint_trajectory_controller/CMakeLists.txt | 109 ++++++++---------- position_controllers/CMakeLists.txt | 77 +++++-------- position_controllers/package.xml | 3 +- ros2_controllers/CMakeLists.txt | 4 +- tricycle_controller/CMakeLists.txt | 102 +++++++--------- tricycle_controller/package.xml | 8 +- velocity_controllers/CMakeLists.txt | 80 ++++++------- velocity_controllers/package.xml | 3 +- 24 files changed, 465 insertions(+), 627 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 3f121d9c3c..9ece5e7fb0 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -1,26 +1,21 @@ -cmake_minimum_required(VERSION 3.5) -project(admittance_controller) +cmake_minimum_required(VERSION 3.16) +project(admittance_controller LANGUAGES CXX) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS angles control_msgs control_toolbox controller_interface - kinematics_interface Eigen3 generate_parameter_library geometry_msgs hardware_interface joint_trajectory_controller + kinematics_interface pluginlib rclcpp rclcpp_lifecycle @@ -39,11 +34,22 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library(admittance_controller SHARED src/admittance_controller.cpp) -target_include_directories(admittance_controller PRIVATE include) -generate_parameter_library(admittance_controller_parameters src/admittance_controller_parameters.yaml) -target_link_libraries(admittance_controller admittance_controller_parameters) -ament_target_dependencies(admittance_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +generate_parameter_library(admittance_controller_parameters + src/admittance_controller_parameters.yaml +) + +add_library(admittance_controller SHARED + src/admittance_controller.cpp +) +target_compile_features(admittance_controller PUBLIC cxx_std_17) +target_include_directories(admittance_controller PUBLIC + $ + $ +) +target_link_libraries(admittance_controller PUBLIC + admittance_controller_parameters +) +ament_target_dependencies(admittance_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -51,43 +57,32 @@ target_compile_definitions(admittance_controller PRIVATE "ADMITTANCE_CONTROLLER_ pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) -install(DIRECTORY include/ - DESTINATION include -) - -install(TARGETS admittance_controller admittance_controller_parameters - EXPORT export_admittance_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(control_msgs REQUIRED) find_package(controller_manager REQUIRED) - find_package(controller_interface REQUIRED) - find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) + # Dynamically loaded during test + find_package(kinematics_interface_kdl REQUIRED) + # test loading admittance controller - add_rostest_with_parameters_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) - target_include_directories(test_load_admittance_controller PUBLIC ${GMOCK_INCLUDE_DIRS}) - target_link_libraries(test_load_admittance_controller ${GMOCK_LIBRARIES}) - ament_target_dependencies( - test_load_admittance_controller + add_rostest_with_parameters_gmock(test_load_admittance_controller + test/test_load_admittance_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml + ) + ament_target_dependencies(test_load_admittance_controller controller_manager hardware_interface ros2_control_test_assets ) + # test admittance controller function - add_rostest_with_parameters_gmock(test_admittance_controller test/test_admittance_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml) - target_include_directories(test_admittance_controller PRIVATE include) + add_rostest_with_parameters_gmock(test_admittance_controller + test/test_admittance_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml + ) target_link_libraries(test_admittance_controller admittance_controller) - ament_target_dependencies( - test_admittance_controller + ament_target_dependencies(test_admittance_controller control_msgs controller_interface hardware_interface @@ -95,10 +90,18 @@ if(BUILD_TESTING) ) endif() -ament_export_targets( - export_admittance_controller HAS_LIBRARY_TARGET +install( + DIRECTORY include/ + DESTINATION include/admittance_controller ) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} + +install(TARGETS admittance_controller admittance_controller_parameters + EXPORT export_admittance_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_admittance_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 6783a5d3cd..f2e4e227a4 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -33,9 +33,7 @@ trajectory_msgs ament_cmake_gmock - control_msgs controller_manager - hardware_interface kinematics_interface_kdl ros2_control_test_assets diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 7188f09f18..1077982380 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -1,28 +1,23 @@ -cmake_minimum_required(VERSION 3.5) -project(diff_drive_controller) +cmake_minimum_required(VERSION 3.16) +project(diff_drive_controller LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - geometry_msgs - hardware_interface - nav_msgs - pluginlib - rclcpp - rclcpp_lifecycle - rcpputils - realtime_tools - tf2 - tf2_msgs + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs ) find_package(ament_cmake REQUIRED) @@ -31,39 +26,27 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -find_package(generate_parameter_library REQUIRED) - generate_parameter_library(diff_drive_controller_parameters src/diff_drive_controller_parameter.yaml ) -add_library(${PROJECT_NAME} SHARED +add_library(diff_drive_controller SHARED src/diff_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp ) -target_include_directories(${PROJECT_NAME} - PUBLIC $ - $) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(${PROJECT_NAME} - diff_drive_controller_parameters +target_compile_features(diff_drive_controller PUBLIC cxx_std_17) +target_include_directories(diff_drive_controller PUBLIC + $ + $ ) +target_link_libraries(diff_drive_controller PUBLIC diff_drive_controller_parameters) +ament_target_dependencies(diff_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "DIFF_DRIVE_CONTROLLER_BUILDING_DLL") +target_compile_definitions(diff_drive_controller PRIVATE "DIFF_DRIVE_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface diff_drive_plugin.xml) -install(DIRECTORY include/ - DESTINATION include -) - -install(TARGETS ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) @@ -72,11 +55,9 @@ if(BUILD_TESTING) ament_add_gmock(test_diff_drive_controller test/test_diff_drive_controller.cpp ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml) - target_include_directories(test_diff_drive_controller PRIVATE include) target_link_libraries(test_diff_drive_controller - ${PROJECT_NAME} + diff_drive_controller ) - ament_target_dependencies(test_diff_drive_controller geometry_msgs hardware_interface @@ -88,25 +69,26 @@ if(BUILD_TESTING) tf2_msgs ) - ament_add_gmock( - test_load_diff_drive_controller + ament_add_gmock(test_load_diff_drive_controller test/test_load_diff_drive_controller.cpp ) - target_include_directories(test_load_diff_drive_controller PRIVATE include) ament_target_dependencies(test_load_diff_drive_controller controller_manager ros2_control_test_assets ) - endif() -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/diff_drive_controller ) -ament_export_libraries( - ${PROJECT_NAME} +install(TARGETS diff_drive_controller diff_drive_controller_parameters + EXPORT export_diff_drive_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_diff_drive_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index baff27bfe2..32dc834e4e 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -16,6 +16,7 @@ geometry_msgs hardware_interface nav_msgs + pluginlib rclcpp rclcpp_lifecycle rcpputils @@ -23,8 +24,6 @@ tf2 tf2_msgs - pluginlib - ament_cmake_gmock controller_manager ros2_control_test_assets diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index d0e92dda9a..340bb19825 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -1,20 +1,14 @@ -cmake_minimum_required(VERSION 3.5) -project(effort_controllers) +cmake_minimum_required(VERSION 3.16) +project(effort_controllers LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - forward_command_controller - pluginlib - rclcpp + forward_command_controller + pluginlib + rclcpp ) find_package(ament_cmake REQUIRED) @@ -23,64 +17,57 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library(${PROJECT_NAME} - SHARED +add_library(effort_controllers SHARED src/joint_group_effort_controller.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE include) -ament_target_dependencies(${PROJECT_NAME} - ${THIS_PACKAGE_INCLUDE_DEPENDS} +target_compile_features(effort_controllers PUBLIC cxx_std_17) +target_include_directories(effort_controllers PUBLIC + $ + $ ) +ament_target_dependencies(effort_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "EFFORT_CONTROLLERS_BUILDING_DLL") +target_compile_definitions(effort_controllers PRIVATE "EFFORT_CONTROLLERS_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface effort_controllers_plugins.xml) -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_joint_group_effort_controller + ament_add_gmock(test_load_joint_group_effort_controller test/test_load_joint_group_effort_controller.cpp ) - target_include_directories(test_load_joint_group_effort_controller PRIVATE include) + target_link_libraries(test_load_joint_group_effort_controller + effort_controllers + ) ament_target_dependencies(test_load_joint_group_effort_controller controller_manager ros2_control_test_assets ) - ament_add_gmock( - test_joint_group_effort_controller + ament_add_gmock(test_joint_group_effort_controller test/test_joint_group_effort_controller.cpp ) - target_include_directories(test_joint_group_effort_controller PRIVATE include) target_link_libraries(test_joint_group_effort_controller - ${PROJECT_NAME} + effort_controllers ) endif() -ament_export_dependencies( - forward_command_controller -) -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/effort_controllers ) -ament_export_libraries( - ${PROJECT_NAME} +install( + TARGETS effort_controllers + EXPORT export_effort_controllers + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_effort_controllers HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 56d136045d..6c486e059f 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -12,10 +12,9 @@ backward_ros forward_command_controller + pluginlib rclcpp - pluginlib - ament_cmake_gmock controller_manager ros2_control_test_assets diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index eac3fe9d8b..207e978c10 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -1,25 +1,19 @@ -cmake_minimum_required(VERSION 3.5) -project(force_torque_sensor_broadcaster) +cmake_minimum_required(VERSION 3.16) +project(force_torque_sensor_broadcaster LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - generate_parameter_library - geometry_msgs - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools ) find_package(ament_cmake REQUIRED) @@ -32,81 +26,68 @@ generate_parameter_library(force_torque_sensor_broadcaster_parameters src/force_torque_sensor_broadcaster_parameters.yaml ) -add_library( - ${PROJECT_NAME} - SHARED +add_library(force_torque_sensor_broadcaster SHARED src/force_torque_sensor_broadcaster.cpp ) -target_include_directories(${PROJECT_NAME} - PUBLIC $ - $) -ament_target_dependencies( - ${PROJECT_NAME} - ${THIS_PACKAGE_INCLUDE_DEPENDS} +target_compile_features(force_torque_sensor_broadcaster PUBLIC cxx_std_17) +target_include_directories(force_torque_sensor_broadcaster PUBLIC + $ + $ ) -target_link_libraries(force_torque_sensor_broadcaster +target_link_libraries(force_torque_sensor_broadcaster PUBLIC force_torque_sensor_broadcaster_parameters ) +ament_target_dependencies(force_torque_sensor_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "FORCE_TORQUE_SENSOR_BROADCASTER_BUILDING_DLL") +target_compile_definitions(force_torque_sensor_broadcaster PRIVATE "FORCE_TORQUE_SENSOR_BROADCASTER_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface force_torque_sensor_broadcaster.xml) -install( - TARGETS force_torque_sensor_broadcaster force_torque_sensor_broadcaster_parameters - EXPORT export_force_torque_sensor_broadcaster - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install( - DIRECTORY include/ - DESTINATION include -) - 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_force_torque_sensor_broadcaster + ament_add_gmock(test_load_force_torque_sensor_broadcaster test/test_load_force_torque_sensor_broadcaster.cpp ) - target_include_directories(test_load_force_torque_sensor_broadcaster PRIVATE include) + target_link_libraries(test_load_force_torque_sensor_broadcaster + force_torque_sensor_broadcaster + ) ament_target_dependencies(test_load_force_torque_sensor_broadcaster controller_manager hardware_interface ros2_control_test_assets ) - ament_add_gmock( - test_force_torque_sensor_broadcaster + ament_add_gmock(test_force_torque_sensor_broadcaster test/test_force_torque_sensor_broadcaster.cpp ) - target_include_directories(test_force_torque_sensor_broadcaster PRIVATE include) target_link_libraries(test_force_torque_sensor_broadcaster - ${PROJECT_NAME} + force_torque_sensor_broadcaster ) ament_target_dependencies(test_force_torque_sensor_broadcaster hardware_interface ) - ament_target_dependencies(test_load_force_torque_sensor_broadcaster - controller_manager - hardware_interface - ros2_control_test_assets - ) endif() -ament_export_targets( - export_force_torque_sensor_broadcaster HAS_LIBRARY_TARGET +install( + DIRECTORY include/ + DESTINATION include/force_torque_sensor_broadcaster ) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} +install( + TARGETS + force_torque_sensor_broadcaster + force_torque_sensor_broadcaster_parameters + EXPORT export_force_torque_sensor_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) +ament_export_targets(export_force_torque_sensor_broadcaster HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 28ac065ec0..e2f9b11860 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -23,7 +23,6 @@ ament_cmake_gmock controller_manager - hardware_interface ros2_control_test_assets diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 5dacdf66c1..46189fe5ac 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -1,25 +1,19 @@ -cmake_minimum_required(VERSION 3.5) -project(forward_command_controller) +cmake_minimum_required(VERSION 3.16) +project(forward_command_controller LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - std_msgs + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_msgs ) find_package(ament_cmake REQUIRED) @@ -37,88 +31,87 @@ generate_parameter_library( src/multi_interface_forward_command_controller_parameters.yaml ) -add_library(forward_command_controller - SHARED +add_library(forward_command_controller SHARED src/forward_controllers_base.cpp src/forward_command_controller.cpp src/multi_interface_forward_command_controller.cpp ) -target_include_directories(forward_command_controller PRIVATE include) -ament_target_dependencies(forward_command_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(forward_command_controller +target_compile_features(forward_command_controller PUBLIC cxx_std_17) +target_include_directories(forward_command_controller PUBLIC + $ + $ +) +target_link_libraries(forward_command_controller PUBLIC forward_command_controller_parameters multi_interface_forward_command_controller_parameters ) +ament_target_dependencies(forward_command_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(forward_command_controller PRIVATE "FORWARD_COMMAND_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface forward_command_plugin.xml) -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - forward_command_controller - forward_command_controller_parameters - multi_interface_forward_command_controller_parameters - EXPORT export_forward_command_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - INCLUDES DESTINATION include -) 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_forward_command_controller + ament_add_gmock(test_load_forward_command_controller test/test_load_forward_command_controller.cpp ) - target_include_directories(test_load_forward_command_controller PRIVATE include) + target_link_libraries(test_load_forward_command_controller + forward_command_controller + ) ament_target_dependencies(test_load_forward_command_controller controller_manager hardware_interface ros2_control_test_assets ) - ament_add_gmock( - test_forward_command_controller + ament_add_gmock(test_forward_command_controller test/test_forward_command_controller.cpp ) - target_include_directories(test_forward_command_controller PRIVATE include) target_link_libraries(test_forward_command_controller forward_command_controller ) - ament_add_gmock( - test_load_multi_interface_forward_command_controller + ament_add_gmock(test_load_multi_interface_forward_command_controller test/test_load_multi_interface_forward_command_controller.cpp ) - target_include_directories(test_load_multi_interface_forward_command_controller PRIVATE include) - ament_target_dependencies( - test_load_multi_interface_forward_command_controller + target_link_libraries(test_load_multi_interface_forward_command_controller + forward_command_controller + ) + ament_target_dependencies(test_load_multi_interface_forward_command_controller controller_manager hardware_interface ros2_control_test_assets ) - ament_add_gmock( - test_multi_interface_forward_command_controller + ament_add_gmock(test_multi_interface_forward_command_controller test/test_multi_interface_forward_command_controller.cpp ) - target_include_directories(test_multi_interface_forward_command_controller PRIVATE include) target_link_libraries(test_multi_interface_forward_command_controller forward_command_controller ) endif() +install( + DIRECTORY include/ + DESTINATION include/forward_command_controller +) +install( + TARGETS + forward_command_controller + forward_command_controller_parameters + multi_interface_forward_command_controller_parameters + EXPORT export_forward_command_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + ament_export_targets(export_forward_command_controller HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index ead0fbb536..8f241e0554 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -14,13 +14,12 @@ controller_interface generate_parameter_library hardware_interface + pluginlib rclcpp rclcpp_lifecycle realtime_tools std_msgs - pluginlib - ament_cmake_gmock controller_manager ros2_control_test_assets diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index e1b32ac2b3..e8fd1e5d7e 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -1,31 +1,25 @@ -cmake_minimum_required(VERSION 3.5) -project(gripper_controllers) +cmake_minimum_required(VERSION 3.16) +project(gripper_controllers LANGUAGES CXX) if(APPLE OR WIN32) message(WARNING "gripper controllers are not available on OSX or Windows") return() endif() -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - control_msgs - control_toolbox - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_action - realtime_tools + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_action + realtime_tools ) find_package(ament_cmake REQUIRED) @@ -41,14 +35,16 @@ generate_parameter_library(gripper_action_controller_parameters add_library(gripper_action_controller SHARED src/gripper_action_controller.cpp ) -ament_target_dependencies(gripper_action_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_include_directories(gripper_action_controller - PRIVATE $ - PRIVATE $ +target_compile_features(gripper_action_controller PUBLIC cxx_std_17) +target_include_directories(gripper_action_controller PUBLIC + $ + $ ) -target_link_libraries(gripper_action_controller +target_link_libraries(gripper_action_controller PUBLIC gripper_action_controller_parameters ) +ament_target_dependencies(gripper_action_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) if(BUILD_TESTING) @@ -56,8 +52,7 @@ if(BUILD_TESTING) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_gripper_action_controllers + ament_add_gmock(test_load_gripper_action_controllers test/test_load_gripper_action_controllers.cpp ) ament_target_dependencies(test_load_gripper_action_controllers @@ -65,11 +60,9 @@ if(BUILD_TESTING) ros2_control_test_assets ) - ament_add_gmock( - test_gripper_controllers + ament_add_gmock(test_gripper_controllers test/test_gripper_controllers.cpp ) - target_include_directories(test_gripper_controllers PRIVATE include) target_link_libraries(test_gripper_controllers gripper_action_controller ) @@ -77,9 +70,8 @@ endif() install( DIRECTORY include/ - DESTINATION include + DESTINATION include/gripper_action_controller ) - install( TARGETS gripper_action_controller diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 30af5a6302..c73a85859e 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -20,12 +20,11 @@ controller_interface generate_parameter_library hardware_interface + pluginlib rclcpp rclcpp_action realtime_tools - pluginlib - ament_cmake_gmock controller_manager ros2_control_test_assets diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index ef9d5e14e4..18d883503b 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -1,25 +1,19 @@ -cmake_minimum_required(VERSION 3.5) -project(imu_sensor_broadcaster) +cmake_minimum_required(VERSION 3.16) +project(imu_sensor_broadcaster LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - sensor_msgs + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + sensor_msgs ) find_package(ament_cmake REQUIRED) @@ -32,21 +26,22 @@ generate_parameter_library(imu_sensor_broadcaster_parameters src/imu_sensor_broadcaster_parameters.yaml ) -add_library( - imu_sensor_broadcaster SHARED +add_library(imu_sensor_broadcaster SHARED src/imu_sensor_broadcaster.cpp ) -target_include_directories(imu_sensor_broadcaster - PRIVATE $ - PRIVATE $ +target_compile_features(imu_sensor_broadcaster PUBLIC cxx_std_17) +target_include_directories(imu_sensor_broadcaster PUBLIC + $ + $ +) +target_link_libraries(imu_sensor_broadcaster PUBLIC + imu_sensor_broadcaster_parameters ) -ament_target_dependencies(imu_sensor_broadcaster ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(imu_sensor_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(imu_sensor_broadcaster PRIVATE "IMU_SENSOR_BROADCASTER_BUILDING_DLL") -target_link_libraries(imu_sensor_broadcaster - imu_sensor_broadcaster_parameters -) pluginlib_export_plugin_description_file( controller_interface imu_sensor_broadcaster.xml) @@ -54,43 +49,35 @@ pluginlib_export_plugin_description_file( 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_imu_sensor_broadcaster + ament_add_gmock(test_load_imu_sensor_broadcaster test/test_load_imu_sensor_broadcaster.cpp ) - target_include_directories(test_load_imu_sensor_broadcaster PRIVATE include) + target_link_libraries(test_load_imu_sensor_broadcaster + imu_sensor_broadcaster + ) ament_target_dependencies(test_load_imu_sensor_broadcaster controller_manager hardware_interface ros2_control_test_assets ) - ament_add_gmock( - test_imu_sensor_broadcaster + ament_add_gmock(test_imu_sensor_broadcaster test/test_imu_sensor_broadcaster.cpp ) - target_include_directories(test_imu_sensor_broadcaster PRIVATE include) target_link_libraries(test_imu_sensor_broadcaster imu_sensor_broadcaster ) ament_target_dependencies(test_imu_sensor_broadcaster hardware_interface ) - ament_target_dependencies(test_load_imu_sensor_broadcaster - controller_manager - hardware_interface - ros2_control_test_assets - ) endif() install( DIRECTORY include/ - DESTINATION include + DESTINATION include/imu_sensor_broadcaster ) - install( TARGETS imu_sensor_broadcaster diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 6a4e025c5e..10cd44ccfa 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -25,7 +25,6 @@ ament_lint_auto ament_lint_common controller_manager - hardware_interface ros2_control_test_assets diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index ee46d8eaab..018adef23a 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -1,37 +1,11 @@ -cmake_minimum_required(VERSION 3.5) -project(joint_state_broadcaster) +cmake_minimum_required(VERSION 3.16) +project(joint_state_broadcaster LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra) endif() -find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) -find_package(control_msgs REQUIRED) -find_package(controller_interface REQUIRED) -find_package(generate_parameter_library REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(rcutils REQUIRED) -find_package(realtime_tools REQUIRED) -find_package(sensor_msgs REQUIRED) - -generate_parameter_library(joint_state_broadcaster_parameters - src/joint_state_broadcaster_parameters.yaml -) - -add_library(joint_state_broadcaster - SHARED - src/joint_state_broadcaster.cpp -) -target_include_directories(joint_state_broadcaster PRIVATE include) -ament_target_dependencies(joint_state_broadcaster +set(THIS_PACKAGE_INCLUDE_DEPENDS builtin_interfaces control_msgs controller_interface @@ -42,27 +16,34 @@ ament_target_dependencies(joint_state_broadcaster realtime_tools sensor_msgs ) -target_link_libraries(joint_state_broadcaster + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(joint_state_broadcaster_parameters + src/joint_state_broadcaster_parameters.yaml +) + +add_library(joint_state_broadcaster SHARED + src/joint_state_broadcaster.cpp +) +target_compile_features(joint_state_broadcaster PUBLIC cxx_std_17) +target_include_directories(joint_state_broadcaster PUBLIC + $ + $ +) +target_link_libraries(joint_state_broadcaster PUBLIC joint_state_broadcaster_parameters ) +ament_target_dependencies(joint_state_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(joint_state_broadcaster PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface joint_state_plugin.xml) -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - joint_state_broadcaster - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) @@ -70,38 +51,43 @@ if(BUILD_TESTING) find_package(rclcpp REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_joint_state_broadcaster + ament_add_gmock(test_load_joint_state_broadcaster test/test_load_joint_state_broadcaster.cpp ) - target_include_directories(test_load_joint_state_broadcaster PRIVATE include) + target_link_libraries(test_load_joint_state_broadcaster + joint_state_broadcaster + ) ament_target_dependencies(test_load_joint_state_broadcaster controller_manager hardware_interface ros2_control_test_assets ) - ament_add_gmock( - test_joint_state_broadcaster + ament_add_gmock(test_joint_state_broadcaster test/test_joint_state_broadcaster.cpp ) - target_include_directories(test_joint_state_broadcaster PRIVATE include) target_link_libraries(test_joint_state_broadcaster joint_state_broadcaster ) + ament_target_dependencies(test_joint_state_broadcaster + hardware_interface + ) endif() -ament_export_dependencies( - control_msgs - controller_interface - generate_parameter_library - rclcpp_lifecycle - sensor_msgs -) -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/joint_state_broadcaster ) -ament_export_libraries( - joint_state_broadcaster +install( + TARGETS + joint_state_broadcaster + joint_state_broadcaster_parameters + EXPORT export_joint_state_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_joint_state_broadcaster HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 4ef019d7a1..2de9e16b63 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -12,23 +12,20 @@ ament_cmake - pluginlib - rcutils - - pluginlib - rcutils - backward_ros + builtin_interfaces control_msgs controller_interface generate_parameter_library - hardware_interface + pluginlib rclcpp_lifecycle + rcutils realtime_tools sensor_msgs ament_cmake_gmock controller_manager + hardware_interface rclcpp ros2_control_test_assets diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index af0bdb5109..5ec478d989 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -1,30 +1,24 @@ -cmake_minimum_required(VERSION 3.5) -project(joint_trajectory_controller) +cmake_minimum_required(VERSION 3.16) +project(joint_trajectory_controller LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - angles - control_msgs - control_toolbox - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - rsl - tl_expected - trajectory_msgs + angles + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + rsl + tl_expected + trajectory_msgs ) find_package(ament_cmake REQUIRED) @@ -38,55 +32,47 @@ generate_parameter_library(joint_trajectory_controller_parameters include/joint_trajectory_controller/validate_jtc_parameters.hpp ) -add_library(${PROJECT_NAME} SHARED +add_library(joint_trajectory_controller SHARED src/joint_trajectory_controller.cpp src/trajectory.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE include) -target_link_libraries(${PROJECT_NAME} joint_trajectory_controller_parameters) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_features(joint_trajectory_controller PUBLIC cxx_std_17) +target_include_directories(joint_trajectory_controller PUBLIC + $ + $ +) +target_link_libraries(joint_trajectory_controller PUBLIC + joint_trajectory_controller_parameters +) +ament_target_dependencies(joint_trajectory_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") +target_compile_definitions(joint_trajectory_controller PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") pluginlib_export_plugin_description_file(controller_interface joint_trajectory_plugin.xml) -install(DIRECTORY include/ - DESTINATION include -) - -install(TARGETS joint_trajectory_controller joint_trajectory_controller_parameters - EXPORT export_joint_trajectory_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_trajectory test/test_trajectory.cpp) - target_include_directories(test_trajectory PRIVATE include) - target_link_libraries(test_trajectory ${PROJECT_NAME}) + target_link_libraries(test_trajectory joint_trajectory_controller) ament_add_gmock(test_trajectory_controller test/test_trajectory_controller.cpp ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml) set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) - target_include_directories(test_trajectory_controller PRIVATE include) target_link_libraries(test_trajectory_controller - ${PROJECT_NAME} - ) - ament_target_dependencies(test_trajectory_controller - ${THIS_PACKAGE_INCLUDE_DEPENDS} + joint_trajectory_controller ) - ament_add_gmock( - test_load_joint_trajectory_controller + ament_add_gmock(test_load_joint_trajectory_controller test/test_load_joint_trajectory_controller.cpp ) - target_include_directories(test_load_joint_trajectory_controller PRIVATE include) + target_link_libraries(test_load_joint_trajectory_controller + joint_trajectory_controller + ) ament_target_dependencies(test_load_joint_trajectory_controller controller_manager control_toolbox @@ -95,23 +81,28 @@ if(BUILD_TESTING) ) # TODO(andyz): Disabled due to flakiness - # ament_add_gmock( - # test_trajectory_actions + # ament_add_gmock(test_trajectory_actions # test/test_trajectory_actions.cpp # ) - # target_include_directories(test_trajectory_actions PRIVATE include) # target_link_libraries(test_trajectory_actions - # ${PROJECT_NAME} - # ) - # ament_target_dependencies(test_trajectory_actions - # ${THIS_PACKAGE_INCLUDE_DEPENDS} + # joint_trajectory_controller # ) endif() -ament_export_targets( - export_joint_trajectory_controller HAS_LIBRARY_TARGET + +install( + DIRECTORY include/ + DESTINATION include/joint_trajectory_controller ) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} +install(TARGETS + joint_trajectory_controller + joint_trajectory_controller_parameters + EXPORT export_joint_trajectory_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_joint_trajectory_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index faf32a0e0b..05714a6e46 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -1,20 +1,14 @@ -cmake_minimum_required(VERSION 3.5) -project(position_controllers) +cmake_minimum_required(VERSION 3.16) +project(position_controllers LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - forward_command_controller - pluginlib - rclcpp + forward_command_controller + pluginlib + rclcpp ) find_package(ament_cmake REQUIRED) @@ -23,64 +17,57 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library(${PROJECT_NAME} - SHARED +add_library(position_controllers SHARED src/joint_group_position_controller.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE include) -ament_target_dependencies(${PROJECT_NAME} - ${THIS_PACKAGE_INCLUDE_DEPENDS} +target_compile_features(position_controllers PUBLIC cxx_std_17) +target_include_directories(position_controllers PUBLIC + $ + $ ) +ament_target_dependencies(position_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "POSITION_CONTROLLERS_BUILDING_DLL") +target_compile_definitions(position_controllers PRIVATE "POSITION_CONTROLLERS_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface position_controllers_plugins.xml) -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_joint_group_position_controller + ament_add_gmock(test_load_joint_group_position_controller test/test_load_joint_group_position_controller.cpp ) - target_include_directories(test_load_joint_group_position_controller PRIVATE include) + target_link_libraries(test_load_joint_group_position_controller + position_controllers + ) ament_target_dependencies(test_load_joint_group_position_controller controller_manager ros2_control_test_assets ) - ament_add_gmock( - test_joint_group_position_controller + ament_add_gmock(test_joint_group_position_controller test/test_joint_group_position_controller.cpp ) - target_include_directories(test_joint_group_position_controller PRIVATE include) target_link_libraries(test_joint_group_position_controller - ${PROJECT_NAME} + position_controllers ) endif() -ament_export_dependencies( - forward_command_controller -) -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/position_controllers ) -ament_export_libraries( - ${PROJECT_NAME} +install( + TARGETS position_controllers + EXPORT export_position_controllers + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_position_controllers HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/position_controllers/package.xml b/position_controllers/package.xml index d9d2852d9e..a624b73066 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -12,10 +12,9 @@ backward_ros forward_command_controller + pluginlib rclcpp - pluginlib - ament_cmake_gmock controller_manager ros2_control_test_assets diff --git a/ros2_controllers/CMakeLists.txt b/ros2_controllers/CMakeLists.txt index 8e3b75944d..bb7e2d25bb 100644 --- a/ros2_controllers/CMakeLists.txt +++ b/ros2_controllers/CMakeLists.txt @@ -1,5 +1,5 @@ -cmake_minimum_required(VERSION 3.5) -project(ros2_controllers) +cmake_minimum_required(VERSION 3.16) +project(ros2_controllers LANGUAGES NONE) find_package(ament_cmake REQUIRED) ament_package() diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index e9342e8696..ea2232f88f 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -1,71 +1,47 @@ -cmake_minimum_required(VERSION 3.8) -project(tricycle_controller) +cmake_minimum_required(VERSION 3.16) +project(tricycle_controller LANGUAGES CXX) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) -find_package(controller_interface REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(pluginlib REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rcpputils REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(realtime_tools REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_msgs REQUIRED) -find_package(ackermann_msgs REQUIRED) - -add_library( - ${PROJECT_NAME} - SHARED - src/tricycle_controller.cpp - src/odometry.cpp - src/traction_limiter.cpp - src/steering_limiter.cpp -) -target_include_directories( - ${PROJECT_NAME} - PRIVATE - include -) - -ament_target_dependencies( - ${PROJECT_NAME} +set(THIS_PACKAGE_INCLUDE_DEPENDS ackermann_msgs builtin_interfaces controller_interface geometry_msgs hardware_interface nav_msgs - std_srvs pluginlib rclcpp rclcpp_lifecycle rcpputils realtime_tools + std_srvs tf2 tf2_msgs ) -pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() -install(TARGETS ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib +add_library(tricycle_controller SHARED + src/tricycle_controller.cpp + src/odometry.cpp + src/traction_limiter.cpp + src/steering_limiter.cpp ) -install( - DIRECTORY - include/ - DESTINATION include +target_compile_features(tricycle_controller PUBLIC cxx_std_17) +target_include_directories(tricycle_controller PUBLIC + $ + $ ) +ament_target_dependencies(tricycle_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) @@ -75,11 +51,9 @@ if(BUILD_TESTING) ament_add_gmock(test_tricycle_controller test/test_tricycle_controller.cpp ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml) - target_include_directories(test_tricycle_controller PRIVATE include) target_link_libraries(test_tricycle_controller - ${PROJECT_NAME} + tricycle_controller ) - ament_target_dependencies(test_tricycle_controller geometry_msgs hardware_interface @@ -91,29 +65,31 @@ if(BUILD_TESTING) tf2_msgs ) - ament_add_gmock( - test_load_tricycle_controller + ament_add_gmock(test_load_tricycle_controller test/test_load_tricycle_controller.cpp ) - target_include_directories(test_load_tricycle_controller PRIVATE include) + target_link_libraries(test_load_tricycle_controller + tricycle_controller + ) ament_target_dependencies(test_load_tricycle_controller controller_manager ros2_control_test_assets ) endif() -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} +install( + DIRECTORY include/ + DESTINATION include/tricycle_controller ) -ament_export_dependencies( - controller_interface - geometry_msgs - hardware_interface - rclcpp - rclcpp_lifecycle +install( + TARGETS + tricycle_controller + EXPORT export_tricycle_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) +ament_export_targets(export_tricycle_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 9db6233bc8..f5a649ab7a 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -11,21 +11,21 @@ ament_cmake + ackermann_msgs backward_ros + builtin_interfaces controller_interface geometry_msgs hardware_interface nav_msgs - std_srvs + pluginlib rclcpp rclcpp_lifecycle rcpputils realtime_tools + std_srvs tf2 tf2_msgs - ackermann_msgs - - pluginlib ament_cmake_gmock controller_manager diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index 4d7ad07dbd..27ec8417bb 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -1,20 +1,14 @@ -cmake_minimum_required(VERSION 3.5) -project(velocity_controllers) +cmake_minimum_required(VERSION 3.16) +project(velocity_controllers LANGUAGES CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - forward_command_controller - pluginlib - rclcpp + forward_command_controller + pluginlib + rclcpp ) find_package(ament_cmake REQUIRED) @@ -23,67 +17,59 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library(${PROJECT_NAME} - SHARED +add_library(velocity_controllers SHARED src/joint_group_velocity_controller.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE include) -ament_target_dependencies(${PROJECT_NAME} - forward_command_controller - pluginlib - rclcpp +target_compile_features(velocity_controllers PUBLIC cxx_std_17) +target_include_directories(velocity_controllers PUBLIC + $ + $ ) +ament_target_dependencies(velocity_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "VELOCITY_CONTROLLERS_BUILDING_DLL") +target_compile_definitions(velocity_controllers PRIVATE "VELOCITY_CONTROLLERS_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface velocity_controllers_plugins.xml) -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock( - test_load_joint_group_velocity_controller + ament_add_gmock(test_load_joint_group_velocity_controller test/test_load_joint_group_velocity_controller.cpp ) - target_include_directories(test_load_joint_group_velocity_controller PRIVATE include) + target_link_libraries(test_load_joint_group_velocity_controller + velocity_controllers + ) ament_target_dependencies(test_load_joint_group_velocity_controller controller_manager ros2_control_test_assets ) - ament_add_gmock( - test_joint_group_velocity_controller + ament_add_gmock(test_joint_group_velocity_controller test/test_joint_group_velocity_controller.cpp ) - target_include_directories(test_joint_group_velocity_controller PRIVATE include) target_link_libraries(test_joint_group_velocity_controller - ${PROJECT_NAME} + velocity_controllers ) endif() -ament_export_dependencies( - forward_command_controller -) -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/velocity_controllers ) -ament_export_libraries( - ${PROJECT_NAME} +install( + TARGETS + velocity_controllers + EXPORT export_velocity_controllers + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_velocity_controllers HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index d47cfbad59..e48b8b331e 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -12,10 +12,9 @@ backward_ros forward_command_controller + pluginlib rclcpp - pluginlib - ament_cmake_gmock controller_manager hardware_interface From 7cc34343e8becfa0cd408ca2890992caa6e8262f Mon Sep 17 00:00:00 2001 From: Solomon Wiznitzer <31753673+swiz23@users.noreply.github.com> Date: Fri, 10 Feb 2023 11:38:51 -0500 Subject: [PATCH 101/126] fix interpolation logic (#516) Co-authored-by: Solomon Wiznitzer --- .../joint_trajectory_controller/interpolation_methods.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp index 766be6e0f4..df8c9da0c4 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp @@ -45,7 +45,7 @@ const std::unordered_map InterpolationMethodMa return InterpolationMethod::NONE; } else if ( - !interpolation_method.compare( + interpolation_method.compare( InterpolationMethodMap.at(InterpolationMethod::VARIABLE_DEGREE_SPLINE)) == 0) { return InterpolationMethod::VARIABLE_DEGREE_SPLINE; From 3a1402e5fb3862c718a36d863d2f7cfde852cb23 Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer <31753650+mechwiz@users.noreply.github.com> Date: Fri, 10 Feb 2023 11:59:46 -0500 Subject: [PATCH 102/126] fix JTC segfault (#518) Co-authored-by: Michael Wiznitzer --- joint_trajectory_controller/src/trajectory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 0bdc5f7c82..06b3ca6e9b 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -64,7 +64,6 @@ bool Trajectory::sample( TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr) { THROW_ON_NULLPTR(trajectory_msg_) - output_state = trajectory_msgs::msg::JointTrajectoryPoint(); if (trajectory_msg_->points.empty()) { @@ -90,6 +89,7 @@ bool Trajectory::sample( return false; } + output_state = trajectory_msgs::msg::JointTrajectoryPoint(); auto & first_point_in_msg = trajectory_msg_->points[0]; const rclcpp::Time first_point_timestamp = trajectory_start_time_ + first_point_in_msg.time_from_start; From bebe50627427c1a87428435f3942ee01d94d23a8 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 10 Feb 2023 17:38:11 +0000 Subject: [PATCH 103/126] Update changelogs --- admittance_controller/CHANGELOG.rst | 5 +++++ diff_drive_controller/CHANGELOG.rst | 6 ++++++ effort_controllers/CHANGELOG.rst | 5 +++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 11 +++++++++++ position_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 5 +++++ 15 files changed, 78 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 85f54999dd..0391596f3a 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 69c60f6cd6..a88f5fbd5e 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Remove compile warnings. (`#519 `_) +* Contributors: Dr. Denis, Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 4517ec229e..cd0f62e23a 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index a09cc78472..1323085416 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index de39f35666..900a4bdbcd 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 6c72b59184..1438138806 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ * Changing to_chrono to use nanoseconds & Reset gripper action goal timer to match JTC impl (`#507 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 455112d852..573824d698 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 529a197858..3df34a4f74 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index a7f1fa723d..1348c97891 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix JTC segfault (`#518 `_) +* fix interpolation logic (`#516 `_) +* Fix overriding of install (`#510 `_) +* Add JTC normalize_error parameter to doc (`#511 `_) +* Fix JTC segfault on unload (`#515 `_) +* Don't set interpolation_method\_ twice (`#517 `_) +* Remove compile warnings. (`#519 `_) +* Contributors: Andy Zelenak, Christoph Fröhlich, Dr. Denis, Michael Wiznitzer, Márk Szitanics, Solomon Wiznitzer, Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ * ported the joint_trajectory_controller query_state service to ROS2 (`#481 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index ed743b759c..c1e0d551ef 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index a86ee0c76e..3c4d2067ca 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 1521d21aae..a6ed0ae7cd 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.1.0 (2023-01-26) ------------------ * add publisher topic parameter to forward_position_controller (`#494 `_) diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 987db88ed8..e36f0590db 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.1.0 (2023-01-26) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 122763475b..680c90331b 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 64a674146b..fd114dbb25 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix overriding of install (`#510 `_) +* Contributors: Tyler Weaver, Chris Thrasher + 3.1.0 (2023-01-26) ------------------ From d8c2946a89f7d7218bf4c36436e7364517975345 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 10 Feb 2023 17:38:49 +0000 Subject: [PATCH 104/126] 3.2.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 0391596f3a..f06aeb1811 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index f2e4e227a4..63101a6fca 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.1.0 + 3.2.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index a88f5fbd5e..e783350de9 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Remove compile warnings. (`#519 `_) * Contributors: Dr. Denis, Tyler Weaver, Chris Thrasher diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 32dc834e4e..869ee842b1 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.1.0 + 3.2.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index cd0f62e23a..b689368ba0 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 6c486e059f..685c71638f 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.1.0 + 3.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 1323085416..21063cf757 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index e2f9b11860..b80ef06beb 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.1.0 + 3.2.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 900a4bdbcd..28b949f0f4 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 8f241e0554..c1c1f8b03d 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.1.0 + 3.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 1438138806..8b14f7d03f 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index c73a85859e..28ff8e3045 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 3.1.0 + 3.2.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 573824d698..52cd4bedbc 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 10cd44ccfa..55a196b0cf 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.1.0 + 3.2.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 3df34a4f74..7ff57c523c 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 2de9e16b63..1cf8db7390 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.1.0 + 3.2.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 1348c97891..ee749061b4 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * fix JTC segfault (`#518 `_) * fix interpolation logic (`#516 `_) * Fix overriding of install (`#510 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index fd1eec3679..77042cff45 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.1.0 + 3.2.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index c1e0d551ef..e96b0bd8e2 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/position_controllers/package.xml b/position_controllers/package.xml index a624b73066..c574571958 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.1.0 + 3.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 3c4d2067ca..6ea4b96eae 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 22951780d2..89c958d363 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.1.0 + 3.2.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index a6ed0ae7cd..8b6a0ea2ac 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ 3.1.0 (2023-01-26) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index e1be6359bb..9de7202716 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.1.0 + 3.2.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index ded28dba8f..71733ca335 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="3.1.0", + version="3.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index e36f0590db..a5b01a0360 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ 3.1.0 (2023-01-26) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 6d696618c7..f0189c15ca 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 3.1.0 + 3.2.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 48b57fe993..8673f708e4 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="3.1.0", + version="3.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 680c90331b..3d58adaa84 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index f5a649ab7a..c34e7ef743 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.1.0 + 3.2.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index fd114dbb25..42c0eccc77 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.2.0 (2023-02-10) +------------------ * Fix overriding of install (`#510 `_) * Contributors: Tyler Weaver, Chris Thrasher diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index e48b8b331e..4d03238ddf 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.1.0 + 3.2.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From abe9efdb03cd6f5e68f788547a87b90ec229b24b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 18 Feb 2023 07:12:26 +0100 Subject: [PATCH 105/126] Add FTS broadcaster to list (#528) --- doc/controllers_index.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index dee9d31abe..5eccbd9841 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -56,3 +56,4 @@ Available Broadcasters Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> Imu Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> + Force Torque Sensor Broadcaster <../force_torque_sensor_broadcaster/doc/userdoc.rst> From 520036dac2b548e24b76ee0bd84658af36bfa7d8 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Mon, 20 Feb 2023 02:02:33 -0500 Subject: [PATCH 106/126] Fix Segfault in GripperActionController (#527) * Use RT buffer for gripper action controller's goal handle * fix format --------- Co-authored-by: Bence Magyar --- .../gripper_action_controller.hpp | 4 +- .../gripper_action_controller_impl.hpp | 57 ++++++++++--------- 2 files changed, 32 insertions(+), 29 deletions(-) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 830de7b514..89bf232fad 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -115,6 +115,7 @@ class GripperActionController : public controller_interface::ControllerInterface using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; using HwIfaceAdapter = HardwareInterfaceAdapter; @@ -134,7 +135,8 @@ class GripperActionController : public controller_interface::ControllerInterface HwIfaceAdapter hw_iface_adapter_; ///< Adapts desired goal state to HW interface. - RealtimeGoalHandlePtr rt_active_goal_; ///< Currently active action goal, if any. + RealtimeGoalHandleBuffer + rt_active_goal_; ///< Container for the currently active action goal, if any. control_msgs::action::GripperCommand::Result::SharedPtr pre_alloc_result_; rclcpp::Duration action_monitor_period_; diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 629f749050..666fced1c0 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -28,11 +28,12 @@ template void GripperActionController::preempt_active_goal() { // Cancels the currently active goal - if (rt_active_goal_) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) { // Marks the current goal as canceled - rt_active_goal_->setCanceled(std::make_shared()); - rt_active_goal_.reset(); + active_goal->setCanceled(std::make_shared()); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } } @@ -86,25 +87,23 @@ template void GripperActionController::accepted_callback( std::shared_ptr goal_handle) // Try to update goal { - { - auto rt_goal = std::make_shared(goal_handle); + auto rt_goal = std::make_shared(goal_handle); - // Accept new goal - preempt_active_goal(); + // Accept new goal + preempt_active_goal(); - // This is the non-realtime command_struct - // We use command_ for sharing - command_struct_.position_ = goal_handle->get_goal()->command.position; - command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort; - command_.writeFromNonRT(command_struct_); + // This is the non-realtime command_struct + // We use command_ for sharing + command_struct_.position_ = goal_handle->get_goal()->command.position; + command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort; + command_.writeFromNonRT(command_struct_); - pre_alloc_result_->reached_goal = false; - pre_alloc_result_->stalled = false; + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = false; - last_movement_time_ = get_node()->now(); - rt_active_goal_ = rt_goal; - rt_active_goal_->execute(); - } + last_movement_time_ = get_node()->now(); + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list goal_handle_timer_.reset(); @@ -112,7 +111,7 @@ void GripperActionController::accepted_callback( // Setup goal status checking timer goal_handle_timer_ = get_node()->create_wall_timer( action_monitor_period_.to_chrono(), - std::bind(&RealtimeGoalHandle::runNonRealtime, rt_active_goal_)); + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); } template @@ -122,7 +121,8 @@ rclcpp_action::CancelResponse GripperActionController::cancel RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); // Check that cancel request refers to currently active goal (if any) - if (rt_active_goal_ && rt_active_goal_->gh_ == goal_handle) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) { // Enter hold current position mode set_hold_position(); @@ -132,9 +132,9 @@ rclcpp_action::CancelResponse GripperActionController::cancel // Mark the current goal as canceled auto action_res = std::make_shared(); - rt_active_goal_->setCanceled(action_res); + active_goal->setCanceled(action_res); // Reset current goal - rt_active_goal_.reset(); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } return rclcpp_action::CancelResponse::ACCEPT; } @@ -152,7 +152,8 @@ void GripperActionController::check_for_success( const rclcpp::Time & time, double error_position, double current_position, double current_velocity) { - if (!rt_active_goal_) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (!active_goal) { return; } @@ -164,8 +165,8 @@ void GripperActionController::check_for_success( pre_alloc_result_->reached_goal = true; pre_alloc_result_->stalled = false; RCLCPP_DEBUG(get_node()->get_logger(), "Successfully moved to goal."); - rt_active_goal_->setSucceeded(pre_alloc_result_); - rt_active_goal_.reset(); + active_goal->setSucceeded(pre_alloc_result_); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } else { @@ -183,14 +184,14 @@ void GripperActionController::check_for_success( if (params_.allow_stalling) { RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Returning success."); - rt_active_goal_->setSucceeded(pre_alloc_result_); + active_goal->setSucceeded(pre_alloc_result_); } else { RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Aborting action!"); - rt_active_goal_->setAborted(pre_alloc_result_); + active_goal->setAborted(pre_alloc_result_); } - rt_active_goal_.reset(); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } } } From 5a7aedeae7e4c1fb76a0498112ee22bc2b8e8a1d Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 20 Feb 2023 16:55:34 +0100 Subject: [PATCH 107/126] Remove publish_rate argument (#529) It was removed: https://github.com/ros-controls/ros2_controllers/pull/468 --- tricycle_controller/config/tricycle_drive_controller.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/tricycle_controller/config/tricycle_drive_controller.yaml b/tricycle_controller/config/tricycle_drive_controller.yaml index c1077b13c3..fba6f34bb2 100644 --- a/tricycle_controller/config/tricycle_drive_controller.yaml +++ b/tricycle_controller/config/tricycle_drive_controller.yaml @@ -9,7 +9,6 @@ tricycle_controller: # Odometry odom_frame_id: odom base_frame_id: base_link - publish_rate: 50.0 # publish rate of odom and tf open_loop: false # if True, uses cmd_vel instead of hardware interface feedback to compute odometry enable_odom_tf: true # If True, publishes odom<-base_link TF odom_only_twist: false # If True, publishes on /odom only linear.x and angular.z; Useful for computing odometry in another node, e.g robot_localization's ekf From 9c53e68c2bc4c35477f634c9e056e610da592d34 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 22 Feb 2023 15:26:05 +0100 Subject: [PATCH 108/126] =?UTF-8?q?=F0=9F=95=B0=EF=B8=8F=20remove=20state?= =?UTF-8?q?=20publish=20rate=20from=20JTC.=20(#520)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../joint_trajectory_controller.hpp | 7 +-- .../src/joint_trajectory_controller.cpp | 35 ++------------ ...oint_trajectory_controller_parameters.yaml | 8 ---- .../test/test_trajectory_controller.cpp | 47 ------------------- .../test/test_trajectory_controller_utils.hpp | 3 -- 5 files changed, 7 insertions(+), 93 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 45d979b152..c8c94aa95d 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -192,9 +192,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Publisher::SharedPtr publisher_; StatePublisherPtr state_publisher_; - rclcpp::Duration state_publisher_period_ = rclcpp::Duration(20ms); - rclcpp::Time last_state_publish_time_; - using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; @@ -252,8 +249,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void publish_state( - const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, - const JointTrajectoryPoint & state_error); + const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, + const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error); void read_state_from_hardware(JointTrajectoryPoint & state); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 99ff99515c..bb26a9bb46 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -356,7 +356,7 @@ controller_interface::return_type JointTrajectoryController::update( } } - publish_state(state_desired_, state_current_, state_error_); + publish_state(time, state_desired_, state_current_, state_error_); return controller_interface::return_type::OK; } @@ -705,17 +705,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1)); - // State publisher - RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", params_.state_publish_rate); - if (params_.state_publish_rate > 0.0) - { - state_publisher_period_ = rclcpp::Duration::from_seconds(1.0 / params_.state_publish_rate); - } - else - { - state_publisher_period_ = rclcpp::Duration::from_seconds(0.0); - } - publisher_ = get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(publisher_); @@ -739,8 +728,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( } state_publisher_->unlock(); - last_state_publish_time_ = get_node()->now(); - // action server configuration if (params_.allow_partial_joints_goal) { @@ -826,7 +813,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( subscriber_is_active_ = true; traj_point_active_ptr_ = &traj_external_point_ptr_; - last_state_publish_time_ = get_node()->now(); // Initialize current state storage if hardware state has tracking offset read_state_from_hardware(state_current_); @@ -936,23 +922,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_shutdown( } void JointTrajectoryController::publish_state( - const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, - const JointTrajectoryPoint & state_error) + const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, + const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) { - if (state_publisher_period_.seconds() <= 0.0) - { - return; - } - - if (get_node()->now() < (last_state_publish_time_ + state_publisher_period_)) - { - return; - } - - if (state_publisher_ && state_publisher_->trylock()) + if (state_publisher_->trylock()) { - last_state_publish_time_ = get_node()->now(); - state_publisher_->msg_.header.stamp = last_state_publish_time_; + state_publisher_->msg_.header.stamp = time; state_publisher_->msg_.desired.positions = desired_state.positions; state_publisher_->msg_.desired.velocities = desired_state.velocities; state_publisher_->msg_.desired.accelerations = desired_state.accelerations; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index f767b15b27..2e0d2018f8 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -50,14 +50,6 @@ joint_trajectory_controller: default_value: false, description: "Allow integration in goal trajectories to accept goals without position or velocity specified", } - state_publish_rate: { - type: double, - default_value: 50.0, - description: "Rate controller state is published", - validation: { - lower_bounds: [0.1] - } - } action_monitor_rate: { type: double, default_value: 20.0, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 8c027b4ae3..1c0dbb5482 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -707,53 +707,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) executor.cancel(); } -void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) -{ - rclcpp::Parameter state_publish_rate_param( - "state_publish_rate", static_cast(target_msg_count)); - rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, true, {state_publish_rate_param}); - - auto future_handle = std::async(std::launch::async, [&executor]() -> void { executor.spin(); }); - - using control_msgs::msg::JointTrajectoryControllerState; - - const int qos_level = 10; - int echo_received_counter = 0; - rclcpp::Subscription::SharedPtr subs = - traj_controller_->get_node()->create_subscription( - controller_name_ + "/state", qos_level, - [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; }); - - // update for 1second - auto clock = rclcpp::Clock(RCL_STEADY_TIME); - const auto start_time = clock.now(); - const rclcpp::Duration wait = rclcpp::Duration::from_seconds(1.0); - const auto end_time = start_time + wait; - while (clock.now() < end_time) - { - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - } - - // We may miss the last message since time allowed is exactly the time needed - EXPECT_NEAR(target_msg_count, echo_received_counter, 1); - - executor.cancel(); -} - -/** - * @brief test_state_publish_rate Test that state publish rate matches configure rate - */ -TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) -{ - test_state_publish_rate_target(10); -} - -// TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) -// { -// test_state_publish_rate_target(0); -// } - // /** // * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from // * internal controller order diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index ca58b124a5..25612f56e9 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -273,8 +273,6 @@ class TrajectoryControllerTest : public ::testing::Test void subscribeToState() { auto traj_lifecycle_node = traj_controller_->get_node(); - traj_lifecycle_node->set_parameter( - rclcpp::Parameter("state_publish_rate", static_cast(100))); using control_msgs::msg::JointTrajectoryControllerState; @@ -410,7 +408,6 @@ class TrajectoryControllerTest : public ::testing::Test std::lock_guard guard(state_mutex_); return state_msg_; } - void test_state_publish_rate_target(int target_msg_count); std::string controller_name_; From bc5d285b6cd56668371eba90e718de0be0bfc79a Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 1 Mar 2023 14:57:03 +0000 Subject: [PATCH 109/126] Bump ros-tooling/setup-ros from 0.5.0 to 0.6.0 (#534) Bumps [ros-tooling/setup-ros](https://github.com/ros-tooling/setup-ros) from 0.5.0 to 0.6.0. - [Release notes](https://github.com/ros-tooling/setup-ros/releases) - [Commits](https://github.com/ros-tooling/setup-ros/compare/0.5.0...0.6.0) --- updated-dependencies: - dependency-name: ros-tooling/setup-ros dependency-type: direct:production update-type: version-update:semver-minor ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-ros-lint.yml | 4 ++-- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index ada62cd1c3..9b6723e1c9 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -16,7 +16,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.5.0 + - uses: ros-tooling/setup-ros@0.6.0 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index e5534056b8..17fa2a7f25 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -12,7 +12,7 @@ jobs: linter: [cppcheck, copyright, lint_cmake] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.5.0 + - uses: ros-tooling/setup-ros@0.6.0 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling @@ -41,7 +41,7 @@ jobs: linter: [cpplint] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.5.0 + - uses: ros-tooling/setup-ros@0.6.0 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index f3e258a2e5..57ef0dd0e6 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.5.0 + - uses: ros-tooling/setup-ros@0.6.0 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 From 5e943b6f88d23174f388deccc2ef276bb7764085 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 1 Mar 2023 14:57:49 +0000 Subject: [PATCH 110/126] Bump ros-tooling/action-ros-ci from 0.2.7 to 0.3.0 (#535) Bumps [ros-tooling/action-ros-ci](https://github.com/ros-tooling/action-ros-ci) from 0.2.7 to 0.3.0. - [Release notes](https://github.com/ros-tooling/action-ros-ci/releases) - [Commits](https://github.com/ros-tooling/action-ros-ci/compare/0.2.7...0.3.0) --- updated-dependencies: - dependency-name: ros-tooling/action-ros-ci dependency-type: direct:production update-type: version-update:semver-minor ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 9b6723e1c9..120376c285 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -20,7 +20,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 - - uses: ros-tooling/action-ros-ci@0.2.7 + - uses: ros-tooling/action-ros-ci@0.3.0 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 57ef0dd0e6..487151a99b 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -32,7 +32,7 @@ jobs: - uses: actions/checkout@v3 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.2.7 + - uses: ros-tooling/action-ros-ci@0.3.0 with: target-ros2-distro: ${{ inputs.ros_distro }} # build all packages listed in the meta package From cfaa7088f67df55d51ba9f1e2207bdb01faf3dfb Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 1 Mar 2023 14:58:59 +0000 Subject: [PATCH 111/126] Bump actions/checkout from 1 to 3 (#536) Bumps [actions/checkout](https://github.com/actions/checkout) from 1 to 3. - [Release notes](https://github.com/actions/checkout/releases) - [Changelog](https://github.com/actions/checkout/blob/main/CHANGELOG.md) - [Commits](https://github.com/actions/checkout/compare/v1...v3) --- updated-dependencies: - dependency-name: actions/checkout dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/reviewer_lottery.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index f7ae929e5b..94f3d9bde6 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -7,7 +7,7 @@ jobs: test: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v3 - uses: uesteibar/reviewer-lottery@v2 with: repo-token: ${{ secrets.GITHUB_TOKEN }} From 1bdc4a49f25db7188fe02f20edd473a670e7cf02 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 1 Mar 2023 09:50:37 -0600 Subject: [PATCH 112/126] Add comments about auto-generated header files (#539) --- .../include/admittance_controller/admittance_controller.hpp | 2 +- .../include/diff_drive_controller/diff_drive_controller.hpp | 1 + .../force_torque_sensor_broadcaster.hpp | 1 + .../forward_command_controller/forward_command_controller.hpp | 1 + .../include/gripper_controllers/gripper_action_controller.hpp | 4 +++- .../include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp | 1 + .../joint_state_broadcaster/joint_state_broadcaster.hpp | 1 + .../joint_trajectory_controller.hpp | 1 + 8 files changed, 10 insertions(+), 2 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index f776646d42..976c877b6f 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -22,7 +22,7 @@ #include #include -// include generated parameter library +// auto-generated by generate_parameter_library #include "admittance_controller_parameters.hpp" #include "admittance_controller/admittance_rule.hpp" diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index f229667fb9..277c7bd99b 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -42,6 +42,7 @@ #include "realtime_tools/realtime_publisher.h" #include "tf2_msgs/msg/tf_message.hpp" +// auto-generated by generate_parameter_library #include "diff_drive_controller_parameters.hpp" namespace diff_drive_controller diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 6166adfd9a..754d1de9ba 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -25,6 +25,7 @@ #include "controller_interface/controller_interface.hpp" #include "force_torque_sensor_broadcaster/visibility_control.h" +// auto-generated by generate_parameter_library #include "force_torque_sensor_broadcaster_parameters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index 8a697847cd..91e3aae480 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -21,6 +21,7 @@ #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" +// auto-generated by generate_parameter_library #include "forward_command_controller_parameters.hpp" namespace forward_command_controller diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 89bf232fad..9d67249405 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -41,9 +41,11 @@ #include "realtime_tools/realtime_server_goal_handle.h" // Project -#include "gripper_action_controller_parameters.hpp" #include "gripper_controllers/hardware_interface_adapter.hpp" +// auto-generated by generate_parameter_library +#include "gripper_action_controller_parameters.hpp" + namespace gripper_action_controller { /** diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index d78e28780e..cd2a44d32b 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -25,6 +25,7 @@ #include "controller_interface/controller_interface.hpp" #include "imu_sensor_broadcaster/visibility_control.h" +// auto-generated by generate_parameter_library #include "imu_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 4761f3f250..f1c532dce9 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -23,6 +23,7 @@ #include "control_msgs/msg/dynamic_joint_state.hpp" #include "controller_interface/controller_interface.hpp" #include "joint_state_broadcaster/visibility_control.h" +// auto-generated by generate_parameter_library #include "joint_state_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index c8c94aa95d..2a6454f454 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -45,6 +45,7 @@ #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +// auto-generated by generate_parameter_library #include "joint_trajectory_controller_parameters.hpp" using namespace std::chrono_literals; // NOLINT From 2f01639ebed824ceebe76ae76778ad7f61dcfe58 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 2 Mar 2023 22:44:12 +0100 Subject: [PATCH 113/126] Use std::clamp instead of rcppmath::clamp (#540) * Use std::clamp * Remove includes --- tricycle_controller/src/steering_limiter.cpp | 7 +++---- tricycle_controller/src/traction_limiter.cpp | 7 +++---- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/tricycle_controller/src/steering_limiter.cpp b/tricycle_controller/src/steering_limiter.cpp index b76cc0f602..eec4b20811 100644 --- a/tricycle_controller/src/steering_limiter.cpp +++ b/tricycle_controller/src/steering_limiter.cpp @@ -20,7 +20,6 @@ #include #include -#include "rcppmath/clamp.hpp" #include "tricycle_controller/steering_limiter.hpp" namespace tricycle_controller @@ -76,7 +75,7 @@ double SteeringLimiter::limit(double & p, double p0, double p1, double dt) double SteeringLimiter::limit_position(double & p) { const double tmp = p; - p = rcppmath::clamp(p, min_position_, max_position_); + p = std::clamp(p, min_position_, max_position_); return tmp != 0.0 ? p / tmp : 1.0; } @@ -88,7 +87,7 @@ double SteeringLimiter::limit_velocity(double & p, double p0, double dt) const double dv_min = min_velocity_ * dt; const double dv_max = max_velocity_ * dt; - double dv = rcppmath::clamp(std::fabs(p - p0), dv_min, dv_max); + double dv = std::clamp(std::fabs(p - p0), dv_min, dv_max); dv *= (p - p0 >= 0 ? 1 : -1); p = p0 + dv; @@ -107,7 +106,7 @@ double SteeringLimiter::limit_acceleration(double & p, double p0, double p1, dou const double da_min = min_acceleration_ * dt2; const double da_max = max_acceleration_ * dt2; - double da = rcppmath::clamp(std::fabs(dv - dp0), da_min, da_max); + double da = std::clamp(std::fabs(dv - dp0), da_min, da_max); da *= (dv - dp0 >= 0 ? 1 : -1); p = p0 + dp0 + da; diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp index 7865d4be71..b9759e3624 100644 --- a/tricycle_controller/src/traction_limiter.cpp +++ b/tricycle_controller/src/traction_limiter.cpp @@ -20,7 +20,6 @@ #include #include -#include "rcppmath/clamp.hpp" #include "tricycle_controller/traction_limiter.hpp" namespace tricycle_controller @@ -91,7 +90,7 @@ double TractionLimiter::limit_velocity(double & v) { const double tmp = v; - v = rcppmath::clamp(std::fabs(v), min_velocity_, max_velocity_); + v = std::clamp(std::fabs(v), min_velocity_, max_velocity_); v *= tmp >= 0 ? 1 : -1; return tmp != 0.0 ? v / tmp : 1.0; @@ -113,7 +112,7 @@ double TractionLimiter::limit_acceleration(double & v, double v0, double dt) dv_min = min_deceleration_ * dt; dv_max = max_deceleration_ * dt; } - double dv = rcppmath::clamp(std::fabs(v - v0), dv_min, dv_max); + double dv = std::clamp(std::fabs(v - v0), dv_min, dv_max); dv *= (v - v0 >= 0 ? 1 : -1); v = v0 + dv; @@ -132,7 +131,7 @@ double TractionLimiter::limit_jerk(double & v, double v0, double v1, double dt) const double da_min = min_jerk_ * dt2; const double da_max = max_jerk_ * dt2; - double da = rcppmath::clamp(std::fabs(dv - dv0), da_min, da_max); + double da = std::clamp(std::fabs(dv - dv0), da_min, da_max); da *= (dv - dv0 >= 0 ? 1 : -1); v = v0 + dv0 + da; From ee3fdd020228d63e9efaf86d2315405de1f4e5b1 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 7 Mar 2023 21:03:21 +0000 Subject: [PATCH 114/126] Update changelogs --- admittance_controller/CHANGELOG.rst | 5 +++++ diff_drive_controller/CHANGELOG.rst | 5 +++++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 6 ++++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 6 ++++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 3 +++ 15 files changed, 66 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index f06aeb1811..3af07c8680 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add comments about auto-generated header files (`#539 `_) +* Contributors: AndyZe + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index e783350de9..db1f5da5db 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add comments about auto-generated header files (`#539 `_) +* Contributors: AndyZe + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index b689368ba0..5aaca59498 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 21063cf757..a9dc27dd3a 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add comments about auto-generated header files (`#539 `_) +* Contributors: AndyZe + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 28b949f0f4..149cda8e29 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add comments about auto-generated header files (`#539 `_) +* Contributors: AndyZe + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 8b14f7d03f..1416f78be8 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add comments about auto-generated header files (`#539 `_) +* Fix Segfault in GripperActionController (`#527 `_) +* Contributors: AndyZe, Erik Holum + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 52cd4bedbc..a682550c63 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add comments about auto-generated header files (`#539 `_) +* Contributors: AndyZe + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 7ff57c523c..663546aa6d 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add comments about auto-generated header files (`#539 `_) +* Contributors: AndyZe + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index ee749061b4..c7e4884466 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add comments about auto-generated header files (`#539 `_) +* 🕰️ remove state publish rate from JTC. (`#520 `_) +* Contributors: AndyZe, Dr. Denis + 3.2.0 (2023-02-10) ------------------ * fix JTC segfault (`#518 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index e96b0bd8e2..e567a2e196 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 6ea4b96eae..cc68f9b8e8 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 8b6a0ea2ac..5d9f97e0be 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.2.0 (2023-02-10) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index a5b01a0360..7182e5a8cc 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.2.0 (2023-02-10) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 3d58adaa84..bf762f882b 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use std::clamp instead of rcppmath::clamp (`#540 `_) +* Remove publish_rate argument (`#529 `_) +* Contributors: Christoph Fröhlich, Tony Najjar + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 42c0eccc77..649232a83d 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.2.0 (2023-02-10) ------------------ * Fix overriding of install (`#510 `_) From d78425359fcc3d3f16fbe7f9302d5fbc673ef4c1 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 7 Mar 2023 21:03:47 +0000 Subject: [PATCH 115/126] 3.3.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 3af07c8680..f0ffaa5f41 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ * Add comments about auto-generated header files (`#539 `_) * Contributors: AndyZe diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 63101a6fca..1b7a90e7e7 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.2.0 + 3.3.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index db1f5da5db..1c20abd5d8 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ * Add comments about auto-generated header files (`#539 `_) * Contributors: AndyZe diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 869ee842b1..16973cf821 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.2.0 + 3.3.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 5aaca59498..e38c07b589 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ 3.2.0 (2023-02-10) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 685c71638f..c72b974644 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.2.0 + 3.3.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index a9dc27dd3a..a0b8cf5bd0 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ * Add comments about auto-generated header files (`#539 `_) * Contributors: AndyZe diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index b80ef06beb..fd6a0de5bf 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.2.0 + 3.3.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 149cda8e29..d87577328b 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ * Add comments about auto-generated header files (`#539 `_) * Contributors: AndyZe diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index c1c1f8b03d..388b931a93 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.2.0 + 3.3.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 1416f78be8..9291bb6098 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ * Add comments about auto-generated header files (`#539 `_) * Fix Segfault in GripperActionController (`#527 `_) * Contributors: AndyZe, Erik Holum diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 28ff8e3045..cc61c5d105 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 3.2.0 + 3.3.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index a682550c63..a2bd1899f5 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ * Add comments about auto-generated header files (`#539 `_) * Contributors: AndyZe diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 55a196b0cf..aa5d4ffa6e 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.2.0 + 3.3.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 663546aa6d..8502816fd7 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ * Add comments about auto-generated header files (`#539 `_) * Contributors: AndyZe diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 1cf8db7390..e8af283875 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.2.0 + 3.3.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index c7e4884466..5ab2f01fcd 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ * Add comments about auto-generated header files (`#539 `_) * 🕰️ remove state publish rate from JTC. (`#520 `_) * Contributors: AndyZe, Dr. Denis diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 77042cff45..7c1f9c74c1 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.2.0 + 3.3.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index e567a2e196..ef05e136bd 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ 3.2.0 (2023-02-10) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index c574571958..180348f776 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.2.0 + 3.3.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index cc68f9b8e8..8933ec9141 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ 3.2.0 (2023-02-10) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 89c958d363..429efd1aa0 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.2.0 + 3.3.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 5d9f97e0be..157b9a853c 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ 3.2.0 (2023-02-10) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 9de7202716..01a961571a 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.2.0 + 3.3.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 71733ca335..a30e10b9df 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="3.2.0", + version="3.3.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 7182e5a8cc..8c44f8ab1d 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ 3.2.0 (2023-02-10) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index f0189c15ca..9ffac65a15 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 3.2.0 + 3.3.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 8673f708e4..62259dcf39 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="3.2.0", + version="3.3.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index bf762f882b..c86256d5e5 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ * Use std::clamp instead of rcppmath::clamp (`#540 `_) * Remove publish_rate argument (`#529 `_) * Contributors: Christoph Fröhlich, Tony Najjar diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index c34e7ef743..0c89f6dc05 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.2.0 + 3.3.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 649232a83d..11291b3b5d 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.3.0 (2023-03-07) +------------------ 3.2.0 (2023-02-10) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 4d03238ddf..e197b29f72 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.2.0 + 3.3.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 8bbabb5193abc8a233753e949e5597b5499dad92 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 7 Mar 2023 22:56:05 +0100 Subject: [PATCH 116/126] Update JTC documentation (#541) * Update JTC documentation Update, e.g. parameter changed with #520 * Fix whitespaces --- joint_trajectory_controller/doc/userdoc.rst | 22 ++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 9abb84545a..e3d5c1976a 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -89,7 +89,6 @@ A yaml file for using it could be: - position - velocity - state_publish_rate: 50.0 action_monitor_rate: 20.0 allow_partial_joints_goal: false @@ -122,11 +121,6 @@ state_interfaces (list(string)) Values: position (mandatory) [velocity, [acceleration]]. Acceleration interface can only be used in combination with position and velocity. -state_publish_rate (double) - Publish-rate of the controller's "state" topic. - - Default: 50.0 - action_monitor_rate (double) Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). @@ -135,9 +129,23 @@ action_monitor_rate (double) allow_partial_joints_goal (boolean) Allow joint goals defining trajectory for only some joints. + Default: false + +allow_integration_in_goal_trajectories (boolean) + Allow integration in goal trajectories to accept goals without position or velocity specified + + Default: false + +interpolation_method (string) + The type of interpolation to use, if any. Can be "splines" or "none". + + Default: splines + open_loop_control (boolean) Use controller in open-loop control mode using ignoring the states provided by hardware interface and using last commands as states in the next control step. This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). + Default: false + If this flag is set, the controller tries to read the values from the command interfaces on starting. If they have real numeric values, those will be used instead of state interfaces. Therefore it is important set command interfaces to NaN (std::numeric_limits::quiet_NaN()) or state values when the hardware is started. constraints (structure) @@ -213,7 +221,7 @@ ROS2 interface of the controller Topic for commanding the controller. ~/state (output topic) [control_msgs::msg::JointTrajectoryControllerState] - Topic publishing internal states. + Topic publishing internal states with the update-rate of the controller manager. ~/follow_joint_trajectory (action server) [control_msgs::action::FollowJointTrajectory] Action server for commanding the controller. From 2bf8ce028ec408df9f7e1114e54070215e1846ec Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 14 Mar 2023 06:27:25 +0000 Subject: [PATCH 117/126] Bump ros-tooling/setup-ros from 0.6.0 to 0.6.1 (#544) Bumps [ros-tooling/setup-ros](https://github.com/ros-tooling/setup-ros) from 0.6.0 to 0.6.1. - [Release notes](https://github.com/ros-tooling/setup-ros/releases) - [Commits](https://github.com/ros-tooling/setup-ros/compare/0.6.0...0.6.1) --- updated-dependencies: - dependency-name: ros-tooling/setup-ros dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-ros-lint.yml | 4 ++-- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 120376c285..f7854aae5b 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -16,7 +16,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.6.0 + - uses: ros-tooling/setup-ros@0.6.1 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 17fa2a7f25..5dd0b6f086 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -12,7 +12,7 @@ jobs: linter: [cppcheck, copyright, lint_cmake] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.0 + - uses: ros-tooling/setup-ros@0.6.1 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling @@ -41,7 +41,7 @@ jobs: linter: [cpplint] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.0 + - uses: ros-tooling/setup-ros@0.6.1 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 487151a99b..51519dfb2c 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.6.0 + - uses: ros-tooling/setup-ros@0.6.1 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 From 82a4e2434c8f766c22c23fc11ffb38d54404c212 Mon Sep 17 00:00:00 2001 From: GuiHome Date: Thu, 16 Mar 2023 22:33:22 +0100 Subject: [PATCH 118/126] Removed auto param decl (#546) automatically_declare_parameters_from_overrides True hinders from dynamically loading external libraries declaring their own parameters. --- admittance_controller/test/test_admittance_controller.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index be78f05bb9..499aec7de9 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -167,7 +167,7 @@ class AdmittanceControllerTest : public ::testing::Test auto options = rclcpp::NodeOptions() .allow_undeclared_parameters(false) .parameter_overrides(parameter_overrides) - .automatically_declare_parameters_from_overrides(true); + .automatically_declare_parameters_from_overrides(false); return SetUpControllerCommon(controller_name, options); } @@ -176,7 +176,7 @@ class AdmittanceControllerTest : public ::testing::Test { auto options = rclcpp::NodeOptions() .allow_undeclared_parameters(false) - .automatically_declare_parameters_from_overrides(true); + .automatically_declare_parameters_from_overrides(false); return SetUpControllerCommon(controller_name, options); } From eb76cd75aa4b902be309d97ff90ba7e69b39cf01 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 28 Mar 2023 20:45:11 +0200 Subject: [PATCH 119/126] Update docs (#552) * Add missing controllers to doc index * Fix wrong forward_command controller doc --- doc/controllers_index.rst | 8 +++++--- forward_command_controller/doc/userdoc.rst | 8 ++++++-- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 5eccbd9841..15d4bdd631 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -40,9 +40,11 @@ Available Controllers .. toctree:: :titlesonly: - Differential Drive <../diff_drive_controller/doc/userdoc.rst> - Forward Command <../forward_command_controller/doc/userdoc.rst> - Joint Trajectory <../joint_trajectory_controller/doc/userdoc.rst> + Admittance Controller <../admittance_controller/doc/userdoc.rst> + Tricycle Controller <../tricycle_controller/doc/userdoc.rst> + Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> + Forward Command Controller <../forward_command_controller/doc/userdoc.rst> + Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> Effort Controllers <../effort_controllers/doc/userdoc.rst> diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index ce653fe714..8532a2dee9 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -3,9 +3,13 @@ forward_command_controller ========================== -This is a collection of controllers that work using the "effort" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the effort on a certain joint to achieve a set position. +This is a base class implementing a feedforward controller. Specific implementations can be found in: + +- `Position Controllers <../position_controllers/doc/userdoc.rst>`_ +- `Velocity Controllers <../velocity_controllers/doc/userdoc.rst>`_ +- `Effort Controllers <../effort_controllers/doc/userdoc.rst>`_ Hardware interface type ----------------------- -These controllers work with joints using the "effort" command interface. +This controller can be used for every type of command interface. From 03dfb84a258f63484b5cd986547982404317a7ea Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 28 Mar 2023 20:46:14 +0200 Subject: [PATCH 120/126] [AdmittanceController] Addintional argument in methods of ControllerInterface (#553) * [AdmittanceController] Addintional argument in methods of ControllerInterface * Update admittance_controller.cpp --- .../include/admittance_controller/admittance_controller.hpp | 3 ++- admittance_controller/src/admittance_controller.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 976c877b6f..e4aef2cf3d 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -97,7 +97,8 @@ class AdmittanceController : public controller_interface::ChainableControllerInt protected: std::vector on_export_reference_interfaces() override; - controller_interface::return_type update_reference_from_subscribers() override; + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; size_t num_joints_ = 0; std::vector command_joint_names_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 1af4e1317f..573d8ba06a 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -370,7 +370,8 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type AdmittanceController::update_reference_from_subscribers() +controller_interface::return_type AdmittanceController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // update input reference from ros subscriber message if (!admittance_) From b70a8d0f685574f7e9647f8f4b4518097d505214 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 30 Mar 2023 07:42:27 +0200 Subject: [PATCH 121/126] Fix broken links (#554) --- forward_command_controller/doc/userdoc.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index 8532a2dee9..1f3a47997a 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -5,9 +5,9 @@ forward_command_controller This is a base class implementing a feedforward controller. Specific implementations can be found in: -- `Position Controllers <../position_controllers/doc/userdoc.rst>`_ -- `Velocity Controllers <../velocity_controllers/doc/userdoc.rst>`_ -- `Effort Controllers <../effort_controllers/doc/userdoc.rst>`_ +- :ref:`position_controllers_userdoc` +- :ref:`velocity_controllers_userdoc` +- :ref:`effort_controllers_userdoc` Hardware interface type ----------------------- From 05011b465f10e6f67363de0b6ab7bee510bd8552 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 2 Apr 2023 08:56:12 +0100 Subject: [PATCH 122/126] Update changelogs --- admittance_controller/CHANGELOG.rst | 6 ++++++ diff_drive_controller/CHANGELOG.rst | 3 +++ effort_controllers/CHANGELOG.rst | 3 +++ force_torque_sensor_broadcaster/CHANGELOG.rst | 3 +++ forward_command_controller/CHANGELOG.rst | 6 ++++++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 15 files changed, 53 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index f0ffaa5f41..0ad86e0867 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [AdmittanceController] Addintional argument in methods of ControllerInterface (`#553 `_) +* Removed auto param decl (`#546 `_) +* Contributors: Dr. Denis, GuiHome + 3.3.0 (2023-03-07) ------------------ * Add comments about auto-generated header files (`#539 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 1c20abd5d8..64b942787c 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ * Add comments about auto-generated header files (`#539 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index e38c07b589..1363bdb535 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index a0b8cf5bd0..b5d9285840 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ * Add comments about auto-generated header files (`#539 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index d87577328b..512f3027e9 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix broken links (`#554 `_) +* Update docs (`#552 `_) +* Contributors: Christoph Fröhlich + 3.3.0 (2023-03-07) ------------------ * Add comments about auto-generated header files (`#539 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 9291bb6098..b52afc1292 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ * Add comments about auto-generated header files (`#539 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index a2bd1899f5..722dab7434 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ * Add comments about auto-generated header files (`#539 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 8502816fd7..dd3aac0935 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ * Add comments about auto-generated header files (`#539 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 5ab2f01fcd..31b6ff904e 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update JTC documentation (`#541 `_) +* Contributors: Christoph Fröhlich + 3.3.0 (2023-03-07) ------------------ * Add comments about auto-generated header files (`#539 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index ef05e136bd..ec15b1625d 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 8933ec9141..ce928074fb 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 157b9a853c..e1347215c6 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 8c44f8ab1d..0aa39f89b8 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index c86256d5e5..cda719df02 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ * Use std::clamp instead of rcppmath::clamp (`#540 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 11291b3b5d..3ba330db77 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.3.0 (2023-03-07) ------------------ From ccc4ab333dd0e0ffcc05d51ea71b771b175f8162 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sun, 2 Apr 2023 08:56:45 +0100 Subject: [PATCH 123/126] 3.4.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 0ad86e0867..aaee61bb37 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ * [AdmittanceController] Addintional argument in methods of ControllerInterface (`#553 `_) * Removed auto param decl (`#546 `_) * Contributors: Dr. Denis, GuiHome diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 1b7a90e7e7..047e42b098 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.3.0 + 3.4.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 64b942787c..4265863ac9 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 16973cf821..db993d32cf 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.3.0 + 3.4.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 1363bdb535..8fcd65c3dc 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index c72b974644..9f988f052e 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.3.0 + 3.4.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index b5d9285840..819d48b324 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index fd6a0de5bf..6db14c62e1 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.3.0 + 3.4.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 512f3027e9..0ea80f13fe 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ * Fix broken links (`#554 `_) * Update docs (`#552 `_) * Contributors: Christoph Fröhlich diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 388b931a93..a490369e96 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.3.0 + 3.4.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index b52afc1292..18dc5cf460 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index cc61c5d105..7cb3ef3a89 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 3.3.0 + 3.4.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 722dab7434..ed969086e6 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index aa5d4ffa6e..e28a48d3c2 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.3.0 + 3.4.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index dd3aac0935..331862cb96 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index e8af283875..19e545e44f 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.3.0 + 3.4.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 31b6ff904e..998463f27a 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ * Update JTC documentation (`#541 `_) * Contributors: Christoph Fröhlich diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 7c1f9c74c1..59ba2a291d 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.3.0 + 3.4.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index ec15b1625d..509f562201 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 180348f776..eb524c526a 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.3.0 + 3.4.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index ce928074fb..5ede96124f 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 429efd1aa0..973d2dc759 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.3.0 + 3.4.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index e1347215c6..409fb0ec91 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 01a961571a..c481adf3c7 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.3.0 + 3.4.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index a30e10b9df..031e08e22e 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="3.3.0", + version="3.4.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 0aa39f89b8..eab9296f8c 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 9ffac65a15..a2a54c29e5 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 3.3.0 + 3.4.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 62259dcf39..e3fc02453b 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="3.3.0", + version="3.4.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index cda719df02..a5c51feece 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 0c89f6dc05..c33f099d0c 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.3.0 + 3.4.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 3ba330db77..ac39176a5e 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.4.0 (2023-04-02) +------------------ 3.3.0 (2023-03-07) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index e197b29f72..1fe9aee0d5 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.3.0 + 3.4.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From a6c573dc0ad8aa41e2d6524c073197ebbbe2e2d8 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Wed, 15 Mar 2023 11:16:58 +0100 Subject: [PATCH 124/126] Switched to plugin-based filters --- admittance_controller/CMakeLists.txt | 1 + .../admittance_controller.hpp | 4 + .../admittance_controller/admittance_rule.hpp | 49 +++++------ .../admittance_rule_impl.hpp | 81 ++++++++----------- .../src/admittance_controller.cpp | 32 +++++--- .../src/admittance_controller_parameters.yaml | 32 -------- .../test/test_admittance_controller.cpp | 20 +++-- .../test/test_admittance_controller.hpp | 8 +- admittance_controller/test/test_params.yaml | 38 ++++----- 9 files changed, 125 insertions(+), 140 deletions(-) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 9ece5e7fb0..f1564ef37b 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -11,6 +11,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS control_toolbox controller_interface Eigen3 + filters generate_parameter_library geometry_msgs hardware_interface diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index e4aef2cf3d..1e1f9fe61c 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -29,6 +29,7 @@ #include "admittance_controller/visibility_control.h" #include "control_msgs/msg/admittance_controller_state.hpp" #include "controller_interface/chainable_controller_interface.hpp" +#include "filters/filter_chain.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" @@ -146,6 +147,9 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // admittance parameters std::shared_ptr parameter_handler_; + // filter chain for Wrench data + std::shared_ptr> filter_chain_; + // ROS messages std::shared_ptr joint_command_msg_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 0f0aabb063..a49360473d 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -25,8 +25,8 @@ #include #include "control_msgs/msg/admittance_controller_state.hpp" -#include "control_toolbox/filters.hpp" #include "controller_interface/controller_interface.hpp" +#include "filters/filter_chain.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "kinematics_interface/kinematics_interface.hpp" #include "pluginlib/class_loader.hpp" @@ -41,18 +41,14 @@ namespace admittance_controller { struct AdmittanceTransforms { - // transformation from force torque sensor frame to base link frame at reference joint angles + // transformation from FT sensor frame to base frame at ref joint angles Eigen::Isometry3d ref_base_ft_; - // transformation from force torque sensor frame to base link frame at reference + admittance offset joint angles + // transformation from FT sensor frame to base frame at ref + admittance offset joint angles Eigen::Isometry3d base_ft_; - // transformation from control frame to base link frame at reference + admittance offset joint angles + // transformation from control frame to base frame at ref + admittance offset joint angles Eigen::Isometry3d base_control_; - // transformation from end effector frame to base link frame at reference + admittance offset joint angles + // transformation from end effector frame to base frame at ref + admittance offset joint angles Eigen::Isometry3d base_tip_; - // transformation from center of gravity frame to base link frame at reference + admittance offset joint angles - Eigen::Isometry3d base_cog_; - // transformation from world frame to base link frame - Eigen::Isometry3d world_base_; }; struct AdmittanceState @@ -94,9 +90,11 @@ class AdmittanceRule { public: explicit AdmittanceRule( - const std::shared_ptr & parameter_handler) + const std::shared_ptr & parameter_handler, + const std::shared_ptr> & filter_chain) { parameter_handler_ = parameter_handler; + filter_chain_ = filter_chain; parameters_ = parameter_handler_->get_params(); num_joints_ = parameters_.joints.size(); admittance_state_ = AdmittanceState(num_joints_); @@ -156,6 +154,8 @@ class AdmittanceRule // admittance config parameters std::shared_ptr parameter_handler_; admittance_controller::Params parameters_; + // Filter chain for Wrench data + std::shared_ptr> filter_chain_; protected: /** @@ -169,17 +169,13 @@ class AdmittanceRule bool calculate_admittance_rule(AdmittanceState & admittance_state, double dt); /** - * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement `measured_wrench`, - * the sensor to base frame rotation `sensor_world_rot`, and the center of gravity frame to base frame rotation `cog_world_rot`. - * The `wrench_world_` estimate includes gravity compensation + * Updates internal estimate of wrench in ft frame `measured_wrench_filtered_` + * given the new measurement `measured_wrench` + * The `measured_wrench_filtered_` might include gravity compensation if such a filter is loaded * \param[in] measured_wrench most recent measured wrench from force torque sensor - * \param[in] sensor_world_rot rotation matrix from world frame to sensor frame - * \param[in] cog_world_rot rotation matrix from world frame to center of gravity frame + * \param[out] success false if processing failed (=filter update failed) */ - void process_wrench_measurements( - const geometry_msgs::msg::Wrench & measured_wrench, - const Eigen::Matrix & sensor_world_rot, - const Eigen::Matrix & cog_world_rot); + bool process_wrench_measurements(const geometry_msgs::msg::Wrench & measured_wrench); template void vec_to_eigen(const std::vector & data, T2 & matrix); @@ -192,8 +188,13 @@ class AdmittanceRule kinematics_loader_; std::unique_ptr kinematics_; - // filtered wrench in world frame - Eigen::Matrix wrench_world_; + // filtered wrench in base frame + Eigen::Matrix wrench_base_; + // input wrench in sensor frame + geometry_msgs::msg::WrenchStamped measured_wrench_; + // filtered wrench in sensor_frame + geometry_msgs::msg::WrenchStamped measured_wrench_filtered_; + Eigen::Matrix wrench_filtered_ft_; // admittance controllers internal state AdmittanceState admittance_state_{0}; @@ -201,12 +202,6 @@ class AdmittanceRule // transforms needed for admittance update AdmittanceTransforms admittance_transforms_; - // position of center of gravity in cog_frame - Eigen::Vector3d cog_pos_; - - // force applied to sensor due to weight of end effector - Eigen::Vector3d end_effector_weight_; - // ROS control_msgs::msg::AdmittanceControllerState state_message_; }; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 87c16e4787..557a385a02 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -22,8 +22,11 @@ #include #include +#include "geometry_msgs/msg/transform_stamped.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/utilities.hpp" +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/transform_listener.h" namespace admittance_controller @@ -100,8 +103,7 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints) admittance_transforms_ = AdmittanceTransforms(); // reset forces - wrench_world_.setZero(); - end_effector_weight_.setZero(); + wrench_base_.setZero(); // load/initialize Eigen types from parameters apply_parameters_update(); @@ -116,8 +118,6 @@ void AdmittanceRule::apply_parameters_update() parameters_ = parameter_handler_->get_params(); } // update param values - end_effector_weight_[2] = -parameters_.gravity_compensation.CoG.force; - vec_to_eigen(parameters_.gravity_compensation.CoG.pos, cog_pos_); vec_to_eigen(parameters_.admittance.mass, admittance_state_.mass); vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness); vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes); @@ -144,12 +144,6 @@ bool AdmittanceRule::get_all_transforms( current_joint_state.positions, parameters_.ft_sensor.frame.id, admittance_transforms_.base_ft_); success &= kinematics_->calculate_link_transform( current_joint_state.positions, parameters_.kinematics.tip, admittance_transforms_.base_tip_); - success &= kinematics_->calculate_link_transform( - current_joint_state.positions, parameters_.fixed_world_frame.frame.id, - admittance_transforms_.world_base_); - success &= kinematics_->calculate_link_transform( - current_joint_state.positions, parameters_.gravity_compensation.frame.id, - admittance_transforms_.base_cog_); success &= kinematics_->calculate_link_transform( current_joint_state.positions, parameters_.control.frame.id, admittance_transforms_.base_control_); @@ -173,18 +167,25 @@ controller_interface::return_type AdmittanceRule::update( bool success = get_all_transforms(current_joint_state, reference_joint_state); - // apply filter and update wrench_world_ vector - Eigen::Matrix rot_world_sensor = - admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_ft_.rotation(); - Eigen::Matrix rot_world_cog = - admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_cog_.rotation(); - process_wrench_measurements(measured_wrench, rot_world_sensor, rot_world_cog); - - // transform wrench_world_ into base frame + // process the wrench measurements + if (!process_wrench_measurements(measured_wrench)) + { + desired_joint_state = reference_joint_state; + return controller_interface::return_type::ERROR; + } + // fill the Wrench (there is no fromMsg for wrench) + wrench_filtered_ft_(0) = measured_wrench_filtered_.wrench.force.x; + wrench_filtered_ft_(1) = measured_wrench_filtered_.wrench.force.y; + wrench_filtered_ft_(2) = measured_wrench_filtered_.wrench.force.z; + wrench_filtered_ft_(3) = measured_wrench_filtered_.wrench.torque.x; + wrench_filtered_ft_(4) = measured_wrench_filtered_.wrench.torque.y; + wrench_filtered_ft_(5) = measured_wrench_filtered_.wrench.torque.z; + + // Rotate (and not transform) to the base frame, explanation lower admittance_state_.wrench_base.block<3, 1>(0, 0) = - admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(0, 0); + admittance_transforms_.base_ft_.rotation() * wrench_filtered_ft_.block<3, 1>(0, 0); admittance_state_.wrench_base.block<3, 1>(3, 0) = - admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(3, 0); + admittance_transforms_.base_ft_.rotation() * wrench_filtered_ft_.block<3, 1>(3, 0); // Compute admittance control law vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos); @@ -225,8 +226,8 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat Eigen::Matrix K_rot = Eigen::Matrix::Zero(); K_pos.diagonal() = admittance_state.stiffness.block<3, 1>(0, 0); K_rot.diagonal() = admittance_state.stiffness.block<3, 1>(3, 0); - // Transform to the control frame - // A reference is here: https://users.wpi.edu/~jfu2/rbe502/files/force_control.pdf + // Rotate (and not transform) to the control frame, explanation can be found + // in the reference here: https://users.wpi.edu/~jfu2/rbe502/files/force_control.pdf // Force Control by Luigi Villani and Joris De Schutter // Page 200 K_pos = rot_base_control * K_pos * rot_base_control.transpose(); @@ -266,9 +267,14 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat // zero out any forces in the control frame Eigen::Matrix F_control; + // rotate to control frame F_control.block<3, 1>(0, 0) = rot_base_control.transpose() * F_base.block<3, 1>(0, 0); F_control.block<3, 1>(3, 0) = rot_base_control.transpose() * F_base.block<3, 1>(3, 0); + + // select the axis to apply the zeroing to F_control = F_control.cwiseProduct(admittance_state.selected_axes); + + // rotate to base frame F_base.block<3, 1>(0, 0) = rot_base_control * F_control.block<3, 1>(0, 0); F_base.block<3, 1>(3, 0) = rot_base_control * F_control.block<3, 1>(3, 0); @@ -301,32 +307,13 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat return success; } -void AdmittanceRule::process_wrench_measurements( - const geometry_msgs::msg::Wrench & measured_wrench, - const Eigen::Matrix & sensor_world_rot, - const Eigen::Matrix & cog_world_rot) +bool AdmittanceRule::process_wrench_measurements(const geometry_msgs::msg::Wrench & measured_wrench) { - Eigen::Matrix new_wrench; - new_wrench(0, 0) = measured_wrench.force.x; - new_wrench(1, 0) = measured_wrench.force.y; - new_wrench(2, 0) = measured_wrench.force.z; - new_wrench(0, 1) = measured_wrench.torque.x; - new_wrench(1, 1) = measured_wrench.torque.y; - new_wrench(2, 1) = measured_wrench.torque.z; - - // transform to world frame - Eigen::Matrix new_wrench_base = sensor_world_rot * new_wrench; - - // apply gravity compensation - new_wrench_base(2, 0) -= end_effector_weight_[2]; - new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_pos_).cross(end_effector_weight_); - - // apply smoothing filter - for (size_t i = 0; i < 6; ++i) - { - wrench_world_(i) = filters::exponentialSmoothing( - new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient); - } + // pass the measured_wrench in its original frame, output will be in the same frame + measured_wrench_.header.frame_id = parameters_.ft_sensor.frame.id; + measured_wrench_.wrench = measured_wrench; + // apply the filter + return filter_chain_->update(measured_wrench_, measured_wrench_filtered_); } const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state() diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 573d8ba06a..f06546bcf9 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -24,7 +24,9 @@ #include #include "admittance_controller/admittance_rule_impl.hpp" +#include "filters/filter_chain.hpp" #include "geometry_msgs/msg/wrench.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" #include "rcutils/logging_macros.h" #include "tf2_ros/buffer.h" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" @@ -37,7 +39,10 @@ controller_interface::CallbackReturn AdmittanceController::on_init() try { parameter_handler_ = std::make_shared(get_node()); - admittance_ = std::make_unique(parameter_handler_); + filter_chain_ = std::make_unique>( + "geometry_msgs::msg::WrenchStamped"); + admittance_ = + std::make_unique(parameter_handler_, filter_chain_); } catch (const std::exception & e) { @@ -147,19 +152,16 @@ AdmittanceController::on_export_reference_interfaces() controller_interface::CallbackReturn AdmittanceController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - try - { - parameter_handler_ = std::make_shared(get_node()); - admittance_ = std::make_unique(parameter_handler_); - } - catch (const std::exception & e) + if (!admittance_) { RCLCPP_ERROR( - get_node()->get_logger(), "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::CallbackReturn::ERROR; + get_node()->get_logger(), + "on_configure should not be called unless Init successfully completed"); + return CallbackReturn::ERROR; } command_joint_names_ = admittance_->parameters_.command_joints; + if (command_joint_names_.empty()) { command_joint_names_ = admittance_->parameters_.joints; @@ -274,6 +276,18 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( get_interface_list(admittance_->parameters_.command_interfaces).c_str(), get_interface_list(admittance_->parameters_.state_interfaces).c_str()); + // configure filters + if (!filter_chain_->configure( + "sensor_filter_chain", get_node()->get_node_logging_interface(), + get_node()->get_node_parameters_interface())) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Could not configure sensor filter chain, please check if the " + "parameters are provided correctly."); + return CallbackReturn::FAILURE; + } + // setup subscribers and publishers auto joint_command_callback = [this](const std::shared_ptr msg) diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index 5bdc40e398..5347270a5e 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -64,11 +64,6 @@ admittance_controller: type: string, description: "Specifies the frame/link name of the force torque sensor." } - filter_coefficient: { - type: double, - default_value: 0.05, - description: "Specifies the filter coefficient for the sensor's exponential filter." - } control: frame: @@ -77,33 +72,6 @@ admittance_controller: description: "Specifies the control frame used for admittance calculation." } - fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) - frame: - id: { - type: string, - description: "Specifies the world frame use for admittance calculation. Gravity must point down in this frame." - } - - gravity_compensation: - frame: - id: { - type: string, - description: "Specifies the frame which center of gravity (CoG) is defined in. Normally, the force torque sensor frame should be used." - } - CoG: - pos: { - type: double_array, - description: "Specifies the position of the center of gravity (CoG) of the end effector in the gravity compensation frame.", - validation: { - fixed_size<>: 3 - } - } - force: { - type: double, - default_value: 0.0, - description: "Specifies the weight of the end effector, e.g mass * 9.81." - } - admittance: selected_axes: { diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index d056e0406c..5a4beb5043 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -24,8 +24,16 @@ // Test on_configure returns ERROR when a required parameter is missing TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_parameter_is_missing) { - ASSERT_EQ(SetUpController(GetParam()), controller_interface::return_type::ERROR); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); + auto ret = SetUpController(GetParam()); + // additionally, test params required during configure only if init worked + if (ret == controller_interface::return_type::OK) + { + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_FAILURE); + } + else + { + ASSERT_EQ(ret, controller_interface::return_type::ERROR); + } } INSTANTIATE_TEST_SUITE_P( @@ -33,9 +41,9 @@ INSTANTIATE_TEST_SUITE_P( AdmittanceControllerTestParameterizedMissingParameters, ::testing::Values( "admittance.mass", "admittance.selected_axes", "admittance.stiffness", - "chainable_command_interfaces", "command_interfaces", "control.frame.id", - "fixed_world_frame.frame.id", "ft_sensor.frame.id", "ft_sensor.name", - "gravity_compensation.CoG.pos", "gravity_compensation.frame.id", "joints", "kinematics.base", + "chainable_command_interfaces", "command_interfaces", "control.frame.id", "ft_sensor.frame.id", + "ft_sensor.name", "sensor_filter_chain.filter2.params.CoG.pos", + "sensor_filter_chain.filter2.params.sensor_frame", "joints", "kinematics.base", "kinematics.plugin_name", "kinematics.plugin_package", "kinematics.tip", "state_interfaces")); INSTANTIATE_TEST_SUITE_P( @@ -43,7 +51,7 @@ INSTANTIATE_TEST_SUITE_P( ::testing::Values( // wrong length COG std::make_tuple( - std::string("gravity_compensation.CoG.pos"), + std::string("sensor_filter_chain.filter2.params.CoG.pos"), rclcpp::ParameterValue(std::vector() = {1, 2, 3, 4})), // wrong length stiffness std::make_tuple( diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 499aec7de9..052c38cfe3 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -53,6 +53,8 @@ const double COMMON_THRESHOLD = 0.001; constexpr auto NODE_SUCCESS = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +constexpr auto NODE_FAILURE = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; constexpr auto NODE_ERROR = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -248,9 +250,10 @@ class AdmittanceControllerTest : public ::testing::Test transform_stamped.transform.rotation.w = 1; transform_stamped.child_frame_id = ik_base_frame_; + // world to kinematic base frame br.sendTransform(transform_stamped); - transform_stamped.child_frame_id = ik_tip_frame_; + // world to kinematic tip frame br.sendTransform(transform_stamped); transform_stamped.header.frame_id = ik_tip_frame_; @@ -263,14 +266,17 @@ class AdmittanceControllerTest : public ::testing::Test transform_stamped.transform.rotation.w = 1; transform_stamped.child_frame_id = control_frame_; + // kinematic tip to control frame br.sendTransform(transform_stamped); transform_stamped.transform.translation.z = 0.05; transform_stamped.child_frame_id = sensor_frame_; + // kinematic tip to sensor frame br.sendTransform(transform_stamped); transform_stamped.transform.translation.z = 0.2; transform_stamped.child_frame_id = endeffector_frame_; + // kinematic tip to end-effector frame br.sendTransform(transform_stamped); } diff --git a/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml index 22cbda2df9..3bd1355def 100644 --- a/admittance_controller/test/test_params.yaml +++ b/admittance_controller/test/test_params.yaml @@ -51,30 +51,32 @@ test_admittance_controller: frame: id: link_6 # tool0 Wrench measurements are in this frame external: false # force torque frame exists within URDF kinematic chain - filter_coefficient: 0.005 + sensor_filter_chain: + filter1: + type: "control_filters/LowPassFilterWrench" + name: "low_pass_filter" + params: + # coefficients make up for same value of alpha = 0.005of the exponentialSmooth filter + sampling_frequency: 100.0 + damping_frequency: 134.0 + damping_intensity: 1.0 + divider: 1 + filter2: + type: "control_filters/GravityCompensationWrench" + name: "gravity_compensation" + params: + world_frame: "base_link" + sensor_frame: "tool0" # tool0 Wrench measurements are in this frame + force_frame: "base_link" # gravity is expressed in this frame + CoG: + pos: [0.1, 0.0, 0.0] + force: [0.0, 0.0, -23.0] # -gravity_acc * mass control: frame: id: tool0 # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector external: false # control frame exists within URDF kinematic chain - fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link) - frame: - id: base_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector - external: false # control frame exists within URDF kinematic chain - - gravity_compensation: - frame: - id: tool0 - external: false - - CoG: # specifies the center of gravity of the end effector - pos: - - 0.1 # x - - 0.0 # y - - 0.0 # z - force: 23.0 # mass * 9.81 - admittance: selected_axes: - true # x From 8a1e58828433ff07c0a229676965d33ac38eb8b0 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Sat, 18 Mar 2023 17:58:03 +0100 Subject: [PATCH 125/126] Handled offset from ft sensor frame to meas frame Test currently fails at joint state matching commands, but this should be expected as there is gravity compensation with a 2.3 kg mass --- .../admittance_rule_impl.hpp | 23 +++++++++++++++---- .../src/admittance_controller_parameters.yaml | 5 ++++ .../test/test_admittance_controller.cpp | 22 ++++++++++-------- .../test/test_admittance_controller.hpp | 9 +++++--- admittance_controller/test/test_params.yaml | 11 +++++---- 5 files changed, 50 insertions(+), 20 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 557a385a02..f125baf57b 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -167,7 +167,7 @@ controller_interface::return_type AdmittanceRule::update( bool success = get_all_transforms(current_joint_state, reference_joint_state); - // process the wrench measurements + // process the wrench measurements, expect the result in ft_sensor.frame (=FK-accessible frame) if (!process_wrench_measurements(measured_wrench)) { desired_joint_state = reference_joint_state; @@ -309,11 +309,26 @@ bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_stat bool AdmittanceRule::process_wrench_measurements(const geometry_msgs::msg::Wrench & measured_wrench) { - // pass the measured_wrench in its original frame, output will be in the same frame - measured_wrench_.header.frame_id = parameters_.ft_sensor.frame.id; + // pass the measured_wrench in its ft_sensor meas_frame + measured_wrench_.header.frame_id = parameters_.ft_sensor.meas_frame.id; measured_wrench_.wrench = measured_wrench; + // output should be ft_sensor frame (because kinematics is only up to there) + measured_wrench_filtered_.header.frame_id = parameters_.ft_sensor.frame.id; // apply the filter - return filter_chain_->update(measured_wrench_, measured_wrench_filtered_); + auto ret = filter_chain_->update(measured_wrench_, measured_wrench_filtered_); + // check the output wrench was correctly transformed into the desired frame + if (measured_wrench_filtered_.header.frame_id != parameters_.ft_sensor.frame.id) + { + RCLCPP_ERROR_ONCE( + rclcpp::get_logger("AdmittanceRule"), + "Wrench frame transformation missing.\n" + "If ft_sensor.frame != ft_sensor.meas_frame, kinematics has no idea about " + "ft_sensor.meas_frame.\n" + "Fix the frames or provide a filter that transforms the wrench from ft_sensor.meas_frame" + " to ft_sensor.frame"); + return false; + } + return ret; } const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state() diff --git a/admittance_controller/src/admittance_controller_parameters.yaml b/admittance_controller/src/admittance_controller_parameters.yaml index 5347270a5e..63f3fd612f 100644 --- a/admittance_controller/src/admittance_controller_parameters.yaml +++ b/admittance_controller/src/admittance_controller_parameters.yaml @@ -64,6 +64,11 @@ admittance_controller: type: string, description: "Specifies the frame/link name of the force torque sensor." } + meas_frame: + id: { + type: string, + description: "Specifies the measurement frame of the force torque sensor (depends on sensor thickness)." + } control: frame: diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 5a4beb5043..cb868bd812 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -114,6 +114,7 @@ TEST_F(AdmittanceControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.name, ft_sensor_name_); ASSERT_EQ(controller_->admittance_->parameters_.kinematics.base, ik_base_frame_); ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.frame.id, sensor_frame_); + ASSERT_EQ(controller_->admittance_->parameters_.ft_sensor.meas_frame.id, sensor_meas_frame_); ASSERT_TRUE(!controller_->admittance_->parameters_.admittance.selected_axes.empty()); ASSERT_TRUE( @@ -232,14 +233,14 @@ TEST_F(AdmittanceControllerTest, publish_status_success) ControllerStateMsg msg; subscribe_and_get_messages(msg); - // // Check that wrench command are all zero since not used - // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); - // ASSERT_EQ(msg.wrench_base.wrench.force.x, 0.0); - // ASSERT_EQ(msg.wrench_base.wrench.force.y, 0.0); - // ASSERT_TRUE(msg.wrench_base.wrench.force.z > 0.15); - // ASSERT_TRUE(msg.wrench_base.wrench.torque.x != 0.0); - // ASSERT_TRUE(msg.wrench_base.wrench.torque.y != 0.0); - // ASSERT_EQ(msg.wrench_base.wrench.torque.z, 0.0); + // // Check that wrench command match the compensation for -m*g =-23 and CoG 0.1 x + ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); + EXPECT_NEAR(msg.wrench_base.wrench.force.x, -20.713, COMMON_THRESHOLD); + EXPECT_NEAR(msg.wrench_base.wrench.force.y, 3.3487, COMMON_THRESHOLD); + EXPECT_NEAR(msg.wrench_base.wrench.force.z, -9.42043, COMMON_THRESHOLD); + EXPECT_NEAR(msg.wrench_base.wrench.torque.x, 0.7209, COMMON_THRESHOLD); + EXPECT_NEAR(msg.wrench_base.wrench.torque.y, -1.001, COMMON_THRESHOLD); + EXPECT_NEAR(msg.wrench_base.wrench.torque.z, -1.94115, COMMON_THRESHOLD); // // Check joint command message // for (auto i = 0ul; i < joint_names_.size(); i++) @@ -283,7 +284,10 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NEAR(joint_command_values_[0], joint_state_values_[0], COMMON_THRESHOLD); + for (auto i = 0ul; i < joint_state_values_.size(); i++) + { + EXPECT_NEAR(joint_state_values_[i], joint_command_values_[i], COMMON_THRESHOLD); + } subscribe_and_get_messages(msg); } diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 052c38cfe3..2dbec367d4 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -269,9 +269,11 @@ class AdmittanceControllerTest : public ::testing::Test // kinematic tip to control frame br.sendTransform(transform_stamped); + // no sensor_frame transform, as it must be one kinematic link it is attached to + transform_stamped.transform.translation.z = 0.05; - transform_stamped.child_frame_id = sensor_frame_; - // kinematic tip to sensor frame + transform_stamped.child_frame_id = sensor_meas_frame_; + // kinematic tip to sensor measurement frame br.sendTransform(transform_stamped); transform_stamped.transform.translation.z = 0.2; @@ -380,7 +382,7 @@ class AdmittanceControllerTest : public ::testing::Test bool hardware_state_has_offset_ = false; const std::string ik_base_frame_ = "base_link"; - const std::string ik_tip_frame_ = "tool0"; + const std::string ik_tip_frame_ = "link_6"; const std::string ik_group_name_ = "arm"; // const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; // const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; @@ -389,6 +391,7 @@ class AdmittanceControllerTest : public ::testing::Test const std::string endeffector_frame_ = "endeffector_frame"; const std::string fixed_world_frame_ = "fixed_world_frame"; const std::string sensor_frame_ = "link_6"; + const std::string sensor_meas_frame_ = "ft_mount"; std::array admittance_selected_axes_ = {true, true, true, true, true, true}; std::array admittance_mass_ = {5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; diff --git a/admittance_controller/test/test_params.yaml b/admittance_controller/test/test_params.yaml index 3bd1355def..5cedea3a36 100644 --- a/admittance_controller/test/test_params.yaml +++ b/admittance_controller/test/test_params.yaml @@ -49,8 +49,11 @@ test_admittance_controller: ft_sensor: name: ft_sensor_name frame: - id: link_6 # tool0 Wrench measurements are in this frame + id: link_6 # Where the sensor is located in the kinematics (usually tool frame) external: false # force torque frame exists within URDF kinematic chain + meas_frame: + id: ft_mount # Wrench measurements are in this frame + external: true # not part of robot URDF, (depends on sensor thickness for instance) sensor_filter_chain: filter1: @@ -66,12 +69,12 @@ test_admittance_controller: type: "control_filters/GravityCompensationWrench" name: "gravity_compensation" params: - world_frame: "base_link" - sensor_frame: "tool0" # tool0 Wrench measurements are in this frame - force_frame: "base_link" # gravity is expressed in this frame + world_frame: "fixed_world_frame" + sensor_frame: "ft_mount" # CoG is defined in this frame in this frame CoG: pos: [0.1, 0.0, 0.0] force: [0.0, 0.0, -23.0] # -gravity_acc * mass + control: frame: id: tool0 # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector From 40d47f3b32aee93e1b555e77c48080e0a998cc09 Mon Sep 17 00:00:00 2001 From: Guillaume Walck Date: Mon, 3 Apr 2023 12:08:16 +0200 Subject: [PATCH 126/126] Split tests for init and config missing params --- .../test/test_admittance_controller.cpp | 38 ++++++++++--------- .../test/test_admittance_controller.hpp | 5 +++ 2 files changed, 26 insertions(+), 17 deletions(-) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index cb868bd812..69d114dee8 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -21,30 +21,34 @@ #include #include -// Test on_configure returns ERROR when a required parameter is missing -TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_parameter_is_missing) +// Test on_init returns ERROR when a required parameter is missing +TEST_P(AdmittanceControllerTestParameterizedMissingParameters, one_control_parameter_is_missing) { - auto ret = SetUpController(GetParam()); - // additionally, test params required during configure only if init worked - if (ret == controller_interface::return_type::OK) - { - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_FAILURE); - } - else - { - ASSERT_EQ(ret, controller_interface::return_type::ERROR); - } + ASSERT_EQ(SetUpController(GetParam()), controller_interface::return_type::ERROR); } INSTANTIATE_TEST_SUITE_P( - MissingMandatoryParameterDuringConfiguration, - AdmittanceControllerTestParameterizedMissingParameters, + MissingMandatoryParameterDuringInit, AdmittanceControllerTestParameterizedMissingParameters, ::testing::Values( "admittance.mass", "admittance.selected_axes", "admittance.stiffness", "chainable_command_interfaces", "command_interfaces", "control.frame.id", "ft_sensor.frame.id", - "ft_sensor.name", "sensor_filter_chain.filter2.params.CoG.pos", - "sensor_filter_chain.filter2.params.sensor_frame", "joints", "kinematics.base", - "kinematics.plugin_name", "kinematics.plugin_package", "kinematics.tip", "state_interfaces")); + "ft_sensor.name", "joints", "kinematics.base", "kinematics.plugin_name", + "kinematics.plugin_package", "kinematics.tip", "state_interfaces")); + +// Test on_configure returns FAILURE when a required parameter is missing +TEST_P( + AdmittanceControllerTestParameterizedMissingConfigParameters, one_config_parameter_is_missing) +{ + SetUpController(GetParam()); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_FAILURE); +} + +INSTANTIATE_TEST_SUITE_P( + MissingMandatoryParameterDuringConfiguration, + AdmittanceControllerTestParameterizedMissingConfigParameters, + ::testing::Values( + "sensor_filter_chain.filter2.params.CoG.pos", + "sensor_filter_chain.filter2.params.sensor_frame")); INSTANTIATE_TEST_SUITE_P( InvalidParameterDuringConfiguration, AdmittanceControllerTestParameterizedInvalidParameters, diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 2dbec367d4..a7008d93ea 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -452,6 +452,11 @@ class AdmittanceControllerTestParameterizedMissingParameters std::map overrides_; }; +class AdmittanceControllerTestParameterizedMissingConfigParameters +: public AdmittanceControllerTestParameterizedMissingParameters +{ +}; + // From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest class AdmittanceControllerTestParameterizedInvalidParameters : public AdmittanceControllerTest,