From 0ae3e42d779daaea49a76803c839241aba447829 Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Wed, 5 Jun 2024 19:10:50 +0200 Subject: [PATCH 01/97] [RQT-JTC] limits from jtc controlled joints (#1146) --- .../joint_limits_urdf.py | 94 ++++++++++--------- .../joint_trajectory_controller.py | 3 +- 2 files changed, 50 insertions(+), 47 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 5655e12f7a..bdde80dd54 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 @@ -45,7 +45,7 @@ def subscribe_to_robot_description(node, key="robot_description"): node.create_subscription(String, key, callback, qos_profile) -def get_joint_limits(node, use_smallest_joint_limits=True): +def get_joint_limits(node, joints_names, use_smallest_joint_limits=True): global description use_small = use_smallest_joint_limits use_mimic = True @@ -71,54 +71,56 @@ def get_joint_limits(node, use_smallest_joint_limits=True): if jtype == "fixed": continue name = child.getAttribute("name") - try: - limit = child.getElementsByTagName("limit")[0] + + if name in joints_names: try: - minval = float(limit.getAttribute("lower")) - maxval = float(limit.getAttribute("upper")) - except ValueError: - if jtype == "continuous": - minval = -pi - maxval = pi - else: + limit = child.getElementsByTagName("limit")[0] + try: + minval = float(limit.getAttribute("lower")) + maxval = float(limit.getAttribute("upper")) + except ValueError: + if jtype == "continuous": + minval = -pi + maxval = pi + else: + raise Exception( + f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!" + ) + try: + maxvel = float(limit.getAttribute("velocity")) + except ValueError: raise Exception( - f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!" + f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!" ) - try: - maxvel = float(limit.getAttribute("velocity")) - except ValueError: + except IndexError: raise Exception( - f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!" + f"Missing limits tag for the joint : {name} in the robot_description!" ) - except IndexError: - raise Exception( - f"Missing limits tag for the joint : {name} in the robot_description!" - ) - 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 + 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 index 72c11625e4..4cc6c901af 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 @@ -238,7 +238,8 @@ def _update_jtc_list(self): # 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 + for jtc_info in running_jtc: + self._robot_joint_limits = get_joint_limits(self._node, _jtc_joint_names(jtc_info)) valid_jtc = [] if self._robot_joint_limits: for jtc_info in running_jtc: From d076ea2fe0555e8dc44cea1e3f9b4fc261ec4d1c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 5 Jun 2024 19:39:17 +0200 Subject: [PATCH 02/97] Add mobile robot kinematics 101 and improve steering library docs (#954) --- diff_drive_controller/doc/userdoc.rst | 2 + doc/controllers_index.rst | 7 +- doc/images/.gitignore | 1 + doc/images/ackermann_steering.drawio | 251 ++++++++++++++ doc/images/ackermann_steering.svg | 3 + doc/images/ackermann_steering_traction.drawio | 287 ++++++++++++++++ doc/images/ackermann_steering_traction.svg | 3 + doc/images/car_like_robot.drawio | 173 ++++++++++ doc/images/car_like_robot.svg | 3 + doc/images/diff_drive.drawio | 137 ++++++++ doc/images/diff_drive.svg | 3 + doc/images/double_traction.drawio | 197 +++++++++++ doc/images/double_traction.svg | 3 + doc/images/unicycle.drawio | 89 +++++ doc/images/unicycle.svg | 3 + doc/mobile_robot_kinematics.rst | 322 ++++++++++++++++++ steering_controllers_library/doc/userdoc.rst | 79 +++-- tricycle_controller/doc/userdoc.rst | 3 + 18 files changed, 1533 insertions(+), 33 deletions(-) create mode 100644 doc/images/.gitignore create mode 100644 doc/images/ackermann_steering.drawio create mode 100644 doc/images/ackermann_steering.svg create mode 100644 doc/images/ackermann_steering_traction.drawio create mode 100644 doc/images/ackermann_steering_traction.svg create mode 100644 doc/images/car_like_robot.drawio create mode 100644 doc/images/car_like_robot.svg create mode 100644 doc/images/diff_drive.drawio create mode 100644 doc/images/diff_drive.svg create mode 100644 doc/images/double_traction.drawio create mode 100644 doc/images/double_traction.svg create mode 100644 doc/images/unicycle.drawio create mode 100644 doc/images/unicycle.svg create mode 100644 doc/mobile_robot_kinematics.rst diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index 897a7b3d9c..29df4e55ba 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -11,6 +11,8 @@ As input it takes velocity commands for the robot body, which are translated to Odometry is computed from hardware feedback and published. +For an introduction to mobile robot kinematics and the nomenclature used here, see :ref:`mobile_robot_kinematics`. + Other features -------------- diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 6f31fbee7a..2cb55150ce 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -21,18 +21,15 @@ Guidelines and Best Practices * -Controllers for Mobile Robots -***************************** +Controllers for Wheeled Mobile Robots +************************************* .. toctree:: :titlesonly: - Ackermann Steering Controller <../ackermann_steering_controller/doc/userdoc.rst> - Bicycle Steering Controller <../bicycle_steering_controller/doc/userdoc.rst> Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Tricycle Controller <../tricycle_controller/doc/userdoc.rst> - Tricycle Steering Controller <../tricycle_steering_controller/doc/userdoc.rst> Controllers for Manipulators and Other Robots ********************************************* diff --git a/doc/images/.gitignore b/doc/images/.gitignore new file mode 100644 index 0000000000..8d71bf9cf4 --- /dev/null +++ b/doc/images/.gitignore @@ -0,0 +1 @@ +*.bkp diff --git a/doc/images/ackermann_steering.drawio b/doc/images/ackermann_steering.drawio new file mode 100644 index 0000000000..d0ddb06a56 --- /dev/null +++ b/doc/images/ackermann_steering.drawio @@ -0,0 +1,251 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/ackermann_steering.svg b/doc/images/ackermann_steering.svg new file mode 100644 index 0000000000..1ca6ace166 --- /dev/null +++ b/doc/images/ackermann_steering.svg @@ -0,0 +1,3 @@ + + +
ICR
 diff --git a/doc/images/ackermann_steering_traction.drawio b/doc/images/ackermann_steering_traction.drawio new file mode 100644 index 0000000000..ccf40be4d7 --- /dev/null +++ b/doc/images/ackermann_steering_traction.drawio @@ -0,0 +1,287 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/ackermann_steering_traction.svg b/doc/images/ackermann_steering_traction.svg new file mode 100644 index 0000000000..1ee9d0181b --- /dev/null +++ b/doc/images/ackermann_steering_traction.svg @@ -0,0 +1,3 @@ + + +
ICR
 diff --git a/doc/images/car_like_robot.drawio b/doc/images/car_like_robot.drawio new file mode 100644 index 0000000000..f9b87b9794 --- /dev/null +++ b/doc/images/car_like_robot.drawio @@ -0,0 +1,173 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/car_like_robot.svg b/doc/images/car_like_robot.svg new file mode 100644 index 0000000000..6fb6bf17d0 --- /dev/null +++ b/doc/images/car_like_robot.svg @@ -0,0 +1,3 @@ + + +
ICR
Front wheel
Rear wheel
 diff --git a/doc/images/diff_drive.drawio b/doc/images/diff_drive.drawio new file mode 100644 index 0000000000..f5fc9802e0 --- /dev/null +++ b/doc/images/diff_drive.drawio @@ -0,0 +1,137 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/diff_drive.svg b/doc/images/diff_drive.svg new file mode 100644 index 0000000000..9759b86048 --- /dev/null +++ b/doc/images/diff_drive.svg @@ -0,0 +1,3 @@ + + +
 diff --git a/doc/images/double_traction.drawio b/doc/images/double_traction.drawio new file mode 100644 index 0000000000..7f9989d733 --- /dev/null +++ b/doc/images/double_traction.drawio @@ -0,0 +1,197 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/double_traction.svg b/doc/images/double_traction.svg new file mode 100644 index 0000000000..ec60b11ffa --- /dev/null +++ b/doc/images/double_traction.svg @@ -0,0 +1,3 @@ + + +
ICR
 diff --git a/doc/images/unicycle.drawio b/doc/images/unicycle.drawio new file mode 100644 index 0000000000..49a1f44b5a --- /dev/null +++ b/doc/images/unicycle.drawio @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/unicycle.svg b/doc/images/unicycle.svg new file mode 100644 index 0000000000..826bbb1463 --- /dev/null +++ b/doc/images/unicycle.svg @@ -0,0 +1,3 @@ + + +
 diff --git a/doc/mobile_robot_kinematics.rst b/doc/mobile_robot_kinematics.rst new file mode 100644 index 0000000000..2ab4f1c0ee --- /dev/null +++ b/doc/mobile_robot_kinematics.rst @@ -0,0 +1,322 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/mobile_robot_kinematics.rst + +.. _mobile_robot_kinematics: + +Wheeled Mobile Robot Kinematics +-------------------------------------------------------------- + +.. _siciliano: https://link.springer.com/book/10.1007/978-1-84628-642-1 +.. _modern_robotics: http://modernrobotics.org/ + +This page introduces the kinematics of different wheeled mobile robots. For further reference see `Siciliano et.al - Robotics: Modelling, Planning and Control `_ and `Kevin M. Lynch and Frank C. Park - Modern Robotics: Mechanics, Planning, And Control `_. + +Wheeled mobile robots can be classified in two categories: + +Omnidirectional robots + which can move instantaneously in any direction in the plane, and + +Nonholonomic robots + which cannot move instantaneously in any direction in the plane. + +The forward integration of the kinematic model using the encoders of the wheel actuators — is referred to as **odometric localization** or **passive localization** or **dead reckoning**. We will call it just **odometry**. + +Omnidirectional Wheeled Mobile Robots +..................................... + +Robots with omniwheels or mecanum wheels. Section will be updated if controllers for these robots are implemented. + +Nonholonomic Wheeled Mobile Robots +..................................... + +Unicycle model +,,,,,,,,,,,,,,,, + +To define the coordinate systems (`ROS coordinate frame conventions `__, the coordinate systems follow the right-hand rule), consider the following simple unicycle model + +.. image:: images/unicycle.svg + :width: 550 + :align: center + :alt: Unicycle + +* :math:`x_b,y_b` is the robot's body-frame coordinate system, located at the contact point of the wheel on the ground. +* :math:`x_w,y_w` is the world coordinate system. +* :math:`x,y` are the robot's Cartesian coordinates in the world coordinate system. +* :math:`\theta` is the robot's heading angle, i.e. the orientation of the robot's :math:`x_b`-axis w.r.t. the world's :math:`x_w`-axis. + +In the following, we want to command the robot with a desired body twist + +.. math:: + + \vec{\nu}_b = \begin{bmatrix} + \vec{\omega}_{b} \\ + \vec{v}_{b} + \end{bmatrix}, + +where :math:`\vec{v}_{b}` is the linear velocity of the robot in its body-frame, and :math:`\vec\omega_{b}` is the angular velocity of the robot in its body-frame. As we consider steering robots on a flat surface, it is sufficient to give + +* :math:`v_{b,x}`, i.e. the linear velocity of the robot in direction of the :math:`x_b` axis. +* :math:`\omega_{b,z}`, i.e. the angular velocity of the robot about the :math:`x_z` axis. + +as desired system inputs. The forward kinematics of the unicycle can be calculated with + +.. math:: + \dot{x} &= v_{b,x} \cos(\theta) \\ + \dot{y} &= v_{b,x} \sin(\theta) \\ + \dot{\theta} &= \omega_{b,z} + +We will formulate the inverse kinematics to calculate the desired commands for the robot (wheel speed or steering) from the given body twist. + +Differential Drive Robot +,,,,,,,,,,,,,,,,,,,,,,,, + +Citing `Siciliano et.al - Robotics: Modelling, Planning and Control `_: + +.. code-block:: text + + A unicycle in the strict sense (i.e., a vehicle equipped with a single wheel) + is a robot with a serious problem of balance in static conditions. However, + there exist vehicles that are kinematically equivalent to a unicycle but more + stable from a mechanical viewpoint. + +One of these vehicles is the differential drive robot, which has two wheels, each of which is driven independently. + +.. image:: images/diff_drive.svg + :width: 550 + :align: center + :alt: Differential drive robot + +* :math:`w` is the wheel track (the distance between the wheels). + +**Forward Kinematics** + +The forward kinematics of the differential drive model can be calculated from the unicycle model above using + +.. math:: + v_{b,x} &= \frac{v_{right} + v_{left}}{2} \\ + \omega_{b,z} &= \frac{v_{right} - v_{left}}{w} + +**Inverse Kinematics** + +The necessary wheel speeds to achieve a desired body twist can be calculated with: + +.. math:: + + v_{left} &= v_{b,x} - \omega_{b,z} w / 2 \\ + v_{right} &= v_{b,x} + \omega_{b,z} w / 2 + + +**Odometry** + +We can use the forward kinematics equations above to calculate the robot's odometry directly from the encoder readings. + +Car-Like (Bicycle) Model +,,,,,,,,,,,,,,,,,,,,,,,, + +The following picture shows a car-like robot with two wheels, where the front wheel is steerable. This model is also known as the bicycle model. + +.. image:: images/car_like_robot.svg + :width: 550 + :align: center + :alt: Car-like robot + +* :math:`\phi` is the steering angle of the front wheel, counted positive in direction of rotation around :math:`x_z`-axis. +* :math:`v_{rear}, v_{front}` is the velocity of the rear and front wheel. +* :math:`l` is the wheelbase. + +We assume that the wheels are rolling without slipping. This means that the velocity of the contact point of the wheel with the ground is zero and the wheel's velocity points in the direction perpendicular to the wheel's axis. The **Instantaneous Center of Rotation** (ICR), i.e. the center of the circle around which the robot rotates, is located at the intersection of the lines that are perpendicular to the wheels' axes and pass through the contact points of the wheels with the ground. + +As a consequence of the no-slip condition, the velocity of the two wheels must satisfy the following constraint: + +.. math:: + v_{rear} = v_{front} \cos(\phi) + +**Forward Kinematics** + +The forward kinematics of the car-like model can be calculated with + +.. math:: + \dot{x} &= v_{b,x} \cos(\theta) \\ + \dot{y} &= v_{b,x} \sin(\theta) \\ + \dot{\theta} &= \frac{v_{b,x}}{l} \tan(\phi) + + +**Inverse Kinematics** + +The steering angle is one command input of the robot: + +.. math:: + \phi = \arctan\left(\frac{l w_{b,z}}{v_{b,x}} \right) + + +For the rear-wheel drive, the velocity of the rear wheel is the second input of the robot: + +.. math:: + v_{rear} = v_{b,x} + + +For the front-wheel drive, the velocity of the front wheel is the second input of the robot: + +.. math:: + v_{front} = \frac{v_{b,x}}{\cos(\phi)} + +**Odometry** + +We have to distinguish between two cases: Encoders on the rear wheel or on the front wheel. + +For the rear wheel case: + +.. math:: + \dot{x} &= v_{rear} \cos(\theta) \\ + \dot{y} &= v_{rear} \sin(\theta) \\ + \dot{\theta} &= \frac{v_{rear}}{l} \tan(\phi) + + +For the front wheel case: + +.. math:: + \dot{x} &= v_{front} \cos(\theta) \cos(\phi)\\ + \dot{y} &= v_{front} \sin(\theta) \cos(\phi)\\ + \dot{\theta} &= \frac{v_{front}}{l} \sin(\phi) + + +Double-Traction Axle +,,,,,,,,,,,,,,,,,,,,, + +The following image shows a car-like robot with three wheels, with two independent traction wheels at the rear. + +.. image:: images/double_traction.svg + :width: 550 + :align: center + :alt: A car-like robot with two traction wheels at the rear + +* :math:`w_r` is the wheel track of the rear axle. + +**Forward Kinematics** + +The forward kinematics is the same as the car-like model above. + +**Inverse Kinematics** + +The turning radius of the robot is + +.. math:: + R_b = \frac{l}{\tan(\phi)} + +Then the velocity of the rear wheels must satisfy these conditions to avoid skidding + +.. math:: + v_{rear,left} &= v_{b,x}\frac{R_b - w_r/2}{R_b}\\ + v_{rear,right} &= v_{b,x}\frac{R_b + w_r/2}{R_b} + +**Odometry** + +The calculation of :math:`v_{b,x}` from two encoder measurements of the traction axle is overdetermined. +If there is no slip and the encoders are ideal, + +.. math:: + v_{b,x} = v_{rear,left} \frac{R_b}{R_b - w_r/2} = v_{rear,right} \frac{R_b}{R_b + w_r/2} + +holds. But to get a more robust solution, we take the average of both , i.e., + +.. math:: + v_{b,x} = 0.5 \left(v_{rear,left} \frac{R_b}{R_b - w_r/2} + v_{rear,right} \frac{R_b}{R_b + w_r/2}\right). + + +Ackermann Steering +,,,,,,,,,,,,,,,,,,,,, + +The following image shows a four-wheeled robot with two independent steering wheels in the front. + +.. image:: images/ackermann_steering.svg + :width: 550 + :align: center + :alt: A car-like robot with two steering wheels at the front + +* :math:`w_f` is the wheel track of the front axle, measured between the two kingpins. + +To prevent the front wheels from slipping, the steering angle of the front wheels cannot be equal. +This is the so-called **Ackermann steering**. + +.. note:: + Ackermann steering can also be achieved by a `mechanical linkage between the two front wheels `__. In this case the robot has only one steering input, and the steering angle of the two front wheels is mechanically coupled. The inverse kinematics of the robot will then be the same as in the car-like model above. + +**Forward Kinematics** + +The forward kinematics is the same as for the car-like model above. + +**Inverse Kinematics** + +The turning radius of the robot is + +.. math:: + R_b = \frac{l}{\tan(\phi)} + +Then the steering angles of the front wheels must satisfy these conditions to avoid skidding + +.. math:: + \phi_{left} &= \arctan\left(\frac{l}{R_b - w_f/2}\right) &= \arctan\left(\frac{2l\sin(\phi)}{2l\cos(\phi) - w_f\sin(\phi)}\right)\\ + \phi_{right} &= \arctan\left(\frac{l}{R_b + w_f/2}\right) &= \arctan\left(\frac{2l\sin(\phi)}{2l\cos(\phi) + w_f\sin(\phi)}\right) + +**Odometry** + +The calculation of :math:`\phi` from two angle measurements of the steering axle is overdetermined. +If there is no slip and the measurements are ideal, + +.. math:: + \phi = \arctan\left(\frac{l\tan(\phi_{left})}{l + w_f/2 \tan(\phi_{left})}\right) = \arctan\left(\frac{l\tan(\phi_{right})}{l - w_f/2 \tan(\phi_{right})}\right) + +holds. But to get a more robust solution, we take the average of both , i.e., + +.. math:: + \phi = 0.5 \left(\arctan\left(\frac{l\tan(\phi_{left})}{l + w_f/2 \tan(\phi_{left})}\right) + \arctan\left(\frac{l\tan(\phi_{right})}{l - w_f/2 \tan(\phi_{right})}\right)\right). + +Ackermann Steering with Traction +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, + +The following image shows a four-wheeled car-like robot with two independent steering wheels at the front, which are also driven independently. + +.. image:: images/ackermann_steering_traction.svg + :width: 550 + :align: center + :alt: A car-like robot with two steering wheels at the front, which are also independently driven. + +* :math:`d_{kp}` is the distance from the kingpin to the contact point of the front wheel with the ground. + +**Forward Kinematics** + +The forward kinematics is the same as the car-like model above. + +**Inverse Kinematics** + +To avoid slipping of the front wheels, the velocity of the front wheels cannot be equal and + +.. math:: + \frac{v_{front,left}}{R_{left}} = \frac{v_{front,right}}{R_{right}} = \frac{v_{b,x}}{R_b} + +with turning radius of the robot and the left/right front wheel + +.. math:: + R_b &= \frac{l}{\tan(\phi)} \\ + R_{left} &= \frac{l-d_{kp}\sin(\phi_{left})}{\sin(\phi_{left})}\\ + R_{right} &= \frac{l+d_{kp}\sin(\phi_{right})}{\sin(\phi_{right})}. + +This results in the following inverse kinematics equations + +.. math:: + v_{front,left} &= \frac{v_{b,x}(l-d_{kp}\sin(\phi_{left}))}{R_b\sin(\phi_{left})}\\ + v_{front,right} &= \frac{v_{b,x}(l+d_{kp}\sin(\phi_{right}))}{R_b\sin(\phi_{right})} + +with the steering angles of the front wheels from the Ackermann steering equations above. + +**Odometry** + +The calculation of :math:`v_{b,x}` from two encoder measurements of the traction axle is again overdetermined. +If there is no slip and the encoders are ideal, + +.. math:: + v_{b,x} = v_{front,left} \frac{R_b\sin(\phi_{left})}{l-d_{kp}\sin(\phi_{left})} = v_{front,right} \frac{R_b\sin(\phi_{right})}{l+d_{kp}\sin(\phi_{right})} + +holds. But to get a more robust solution, we take the average of both , i.e., + +.. math:: + v_{b,x} = 0.5 \left( v_{front,left} \frac{R_b\sin(\phi_{left})}{l-d_{kp}\sin(\phi_{left})} + v_{front,right} \frac{R_b\sin(\phi_{right})}{l+d_{kp}\sin(\phi_{right})}\right). diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 0155bc5fd8..a52e34c120 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -5,17 +5,25 @@ steering_controllers_library ============================= -Library with shared functionalities for mobile robot controllers with steering drive (2 degrees of freedom). +.. _steering_controller_status_msg: https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/SteeringControllerStatus.msg +.. _odometry_msg: https://github.com/ros2/common_interfaces/blob/{DISTRO}/nav_msgs/msg/Odometry.msg +.. _twist_msg: https://github.com/ros2/common_interfaces/blob/{DISTRO}/geometry_msgs/msg/TwistStamped.msg +.. _tf_msg: https://github.com/ros2/geometry2/blob/{DISTRO}/tf2_msgs/msg/TFMessage.msg + +Library with shared functionalities for mobile robot controllers with steering drives, with so-called non-holonomic constraints. + The library implements generic odometry and update methods and defines the main interfaces. -Nomenclature used for the controller is used from `wikipedia `_. +The update methods only use inverse kinematics, it does not implement any feedback control loops like path-tracking controllers etc. + +For an introduction to mobile robot kinematics and the nomenclature used here, see :ref:`mobile_robot_kinematics`. Execution logic of the controller ---------------------------------- -The controller uses velocity input, i.e., stamped Twist messages where linear ``x`` and angular ``z`` components are used. -Angular component under +The controller uses velocity input, i.e., stamped `twist messages `_ where linear ``x`` and angular ``z`` components are used. Values in other components are ignored. + In the chain mode the controller provides two reference interfaces, one for linear velocity and one for steering angle position. Other relevant features are: @@ -24,65 +32,80 @@ Other relevant features are: * input command timeout based on a parameter. The command for the wheels are calculated using ``odometry`` library where based on concrete kinematics traction and steering commands are calculated. -Currently implemented kinematics in corresponding packages are: + +Currently implemented kinematics +-------------------------------------------------------------- * :ref:`Bicycle ` - with one steering and one drive joints; * :ref:`Tricylce ` - with one steering and two drive joints; -* :ref:`Ackermann ` - with two seering and two drive joints. +* :ref:`Ackermann ` - with two steering and two drive joints. +.. toctree:: + :hidden: + Bicycle <../../bicycle_steering_controller/doc/userdoc.rst> + Tricylce <../../tricycle_steering_controller/doc/userdoc.rst> + Ackermann <../../ackermann_steering_controller/doc/userdoc.rst> Description of controller's interfaces -------------------------------------- References (from a preceding controller) ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, -- /linear/velocity [double], in m/s -- /angular/position [double] # in [rad] -Commands -,,,,,,,,, -``front_steering == true`` +Used when controller is in chained mode (``in_chained_mode == true``). + +- ``/linear/velocity`` double, in m/s +- ``/angular/position`` double, in rad -- /position [double] # in [rad] -- /velocity [double] # in [m/s] +Command interfaces +,,,,,,,,,,,,,,,,,,, -``front_steering == false`` +If parameter ``front_steering == true`` -- /velocity [double] # in [m/s] -- /position [double] # in [rad] +- ``/position`` double, in rad +- ``/velocity`` double, in m/s + +If parameter ``front_steering == false`` + +- ``/velocity`` double, in m/s +- ``/position`` double, in rad + +State interfaces +,,,,,,,,,,,,,,,,, -States -,,,,,,, Depending on the ``position_feedback``, different feedback types are expected * ``position_feedback == true`` --> ``TRACTION_FEEDBACK_TYPE = position`` * ``position_feedback == false`` --> ``TRACTION_FEEDBACK_TYPE = velocity`` -``front_steering == true`` +If parameter ``front_steering == true`` -- /position [double] # in [rad] -- / [double] # in [m] or [m/s] +- ``/position`` double, in rad +- ``/`` double, in m or m/s -``front_steering == false`` +If parameter ``front_steering == false`` -- / [double] # [m] or [m/s] -- /position [double] # in [rad] +- ``/`` double, in m or m/s +- ``/position`` double, in rad Subscribers ,,,,,,,,,,,, + Used when controller is not in chained mode (``in_chained_mode == false``). -- /reference [geometry_msgs/msg/TwistStamped] +- ``/reference`` [`geometry_msgs/msg/TwistStamped `_] Publishers ,,,,,,,,,,, -- /odometry [nav_msgs/msg/Odometry] -- /tf_odometry [tf2_msgs/msg/TFMessage] -- /controller_state [control_msgs/msg/SteeringControllerStatus] + +- ``/odometry`` [`nav_msgs/msg/Odometry `_] +- ``/tf_odometry`` [`tf2_msgs/msg/TFMessage `_] +- ``/controller_state`` [`control_msgs/msg/SteeringControllerStatus `_] Parameters ,,,,,,,,,,, + This controller uses the `generate_parameter_library `_ to handle its parameters. For an exemplary parameterization see the ``test`` folder of the controller's package. diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index 30973762fd..2ede52e1b6 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -10,6 +10,9 @@ Controller for mobile robots with a single double-actuated wheel, including trac 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. +For an introduction to mobile robot kinematics and the nomenclature used here, see :ref:`mobile_robot_kinematics`. + + Other features -------------- From 619e3036afa0bc305bc10926e91e9806026ef32f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Jun 2024 18:50:13 +0100 Subject: [PATCH 03/97] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 +++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_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 | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ pid_controller/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 5 +++++ steering_controllers_library/CHANGELOG.rst | 6 ++++++ tricycle_controller/CHANGELOG.rst | 6 ++++++ tricycle_steering_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 21 files changed, 78 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 371af97240..0f0cb49cc9 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ * Add parameter check for geometric values (`#1120 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index cc2d01d333..b6e0bc3481 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 6715b36e3c..879aeee9ae 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ * Add parameter check for geometric values (`#1120 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index c045692fc9..d343ddd536 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) +* Bump version of pre-commit hooks (`#1157 `_) +* Contributors: Christoph Fröhlich, github-actions[bot] + 4.8.0 (2024-05-14) ------------------ * Remove non-existing parameter (`#1119 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index f69b29af93..aa905fd2df 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 7b9b15f785..8675e4135c 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 +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 9d2ed3f105..719ac12c2d 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index c191f02239..246d1b9b1b 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index f453510963..c3573441cf 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 14c9594643..eeeb2a1663 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index bb784700f4..df8de77c23 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* JTC trajectory end time validation fix (`#1090 `_) +* Contributors: Henry Moore + 4.8.0 (2024-05-14) ------------------ * [JTC] Remove unused test code (`#1095 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 6c45be4315..48f8796679 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ * [PID] Add example yaml to docs (`#951 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 26975a6fee..7ffffa6270 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index b396198081..cfb6952bd6 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 399bbf198a..818c437909 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add custom rosdoc2 config for ros2_controllers metapackage (`#1100 `_) +* Contributors: Christoph Fröhlich + 4.8.0 (2024-05-14) ------------------ diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index e0b2b19a05..4770c8a5b3 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 +----------- + 4.8.0 (2024-05-14) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index bf3091b934..586a28fa37 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 +----------- +* [RQT-JTC] limits from jtc controlled joints (`#1146 `_) +* Contributors: Jakub Delicat + 4.8.0 (2024-05-14) ------------------ diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index f40ad793cf..a7d1768f7b 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) +* Fix correct usage of angular velocity in update_odometry() function (`#1118 `_) +* Contributors: Christoph Fröhlich, Ferry Schoenmakers + 4.8.0 (2024-05-14) ------------------ * Deprecate non-stamped twist for tricycle_controller and steering_controllers (`#1093 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index ce997f2f8d..cca8c443be 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) +* Bump version of pre-commit hooks (`#1157 `_) +* Contributors: Christoph Fröhlich, github-actions[bot] + 4.8.0 (2024-05-14) ------------------ * Add parameter check for geometric values (`#1120 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 2ac25f4c33..2528a7d6f9 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ * Add parameter check for geometric values (`#1120 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 71946c0e55..e1ed29b996 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-05-14) ------------------ From 0b8a196b23538333bc4a92561214cc41b3e43530 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 5 Jun 2024 18:50:53 +0100 Subject: [PATCH 04/97] 4.9.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_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 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/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 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 0f0cb49cc9..5cfdefb83b 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index edd040ad5a..7b66dd765b 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.8.0 + 4.9.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index b6e0bc3481..1a2dd8ed96 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index ae1ed5fc79..2add40baf7 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.8.0 + 4.9.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 879aeee9ae..8c3d27e24f 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index be87ada566..bcad405abe 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.8.0 + 4.9.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index d343ddd536..a4473f0f83 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) * Bump version of pre-commit hooks (`#1157 `_) * Contributors: Christoph Fröhlich, github-actions[bot] diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index c6bdc4e229..8765d2506f 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.8.0 + 4.9.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 aa905fd2df..63b44cd218 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 92458dc25e..6519a07b07 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.8.0 + 4.9.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 8675e4135c..deca56a197 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 ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index f76fa39cea..4576bbb8b0 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.8.0 + 4.9.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 719ac12c2d..3f1917dcad 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index a15e03260b..b4f34fce8c 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.8.0 + 4.9.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 246d1b9b1b..e0b5a0a311 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index a942ef7362..f4ee00591a 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.8.0 + 4.9.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index c3573441cf..cebfc8d78c 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 4bb3b025cb..032e5addc1 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.8.0 + 4.9.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 eeeb2a1663..ec9b79726f 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 37e7ef518c..a34c05b269 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.8.0 + 4.9.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 df8de77c23..544cf500ef 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ * JTC trajectory end time validation fix (`#1090 `_) * Contributors: Henry Moore diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index fb1f180a40..2c04d7fa23 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.8.0 + 4.9.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 48f8796679..4b884dc386 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 8fe803626e..be598f94ae 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.8.0 + 4.9.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 7ffffa6270..f454aadc4e 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index dc3ec1d315..37bcdbe108 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.8.0 + 4.9.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index cfb6952bd6..7dd295970d 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 1cf6c7600b..31e8b1755b 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.8.0 + 4.9.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 818c437909..af872ec8a3 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ * Add custom rosdoc2 config for ros2_controllers metapackage (`#1100 `_) * Contributors: Christoph Fröhlich diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index e8d47a5e48..278ecb0b1a 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.8.0 + 4.9.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 4770c8a5b3..19ead3093f 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 ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 45fd5f713c..c321c9703d 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.8.0 + 4.9.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 f085c09134..e0dfb4d65e 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.8.0", + version="4.9.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 586a28fa37..121a5066c2 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 ------------ +4.9.0 (2024-06-05) +------------------ * [RQT-JTC] limits from jtc controlled joints (`#1146 `_) * Contributors: Jakub Delicat diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 70bf579033..68a173217a 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 - 4.8.0 + 4.9.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 12eb399a61..e9a19034c6 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.8.0", + version="4.9.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index a7d1768f7b..e1cdc069df 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) * Fix correct usage of angular velocity in update_odometry() function (`#1118 `_) * Contributors: Christoph Fröhlich, Ferry Schoenmakers diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index d7f1159cee..08d252d794 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.8.0 + 4.9.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index cca8c443be..8f36bae23c 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) * Bump version of pre-commit hooks (`#1157 `_) * Contributors: Christoph Fröhlich, github-actions[bot] diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 0bb5a61344..51fe867106 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.8.0 + 4.9.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 2528a7d6f9..f6423146a0 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index a0c90d80a2..6e1ed66edd 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.8.0 + 4.9.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index e1ed29b996..64a09e4309 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-06-05) +------------------ 4.8.0 (2024-05-14) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 2c37e0aec6..05f6245686 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.8.0 + 4.9.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 9625702643879d21468aec25fbb0a5dbec0f6b81 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 5 Jun 2024 20:14:35 +0200 Subject: [PATCH 05/97] Remove manual angle-wraparound parameter (#1152) --- doc/release_notes/Jazzy.rst | 2 +- .../src/joint_trajectory_controller.cpp | 11 ----------- .../src/joint_trajectory_controller_parameters.yaml | 6 ------ 3 files changed, 1 insertion(+), 18 deletions(-) diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes/Jazzy.rst index 8ecac3508f..4c5db18f3e 100644 --- a/doc/release_notes/Jazzy.rst +++ b/doc/release_notes/Jazzy.rst @@ -32,7 +32,7 @@ joint_trajectory_controller * Empty trajectory messages are discarded (`#902 `_). * Action field ``error_string`` is now filled with meaningful strings (`#887 `_). * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). -* The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). +* The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). ``angle_wraparound`` parameter was completely removed. pid_controller ************************ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index b250b25236..b80d1b7908 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -703,17 +703,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( joints_angle_wraparound_.resize(dof_); for (size_t i = 0; i < dof_; ++i) { - const auto & gains = params_.gains.joints_map.at(params_.joints[i]); - if (gains.angle_wraparound) - { - // TODO(christophfroehlich): remove this warning in a future release (ROS-J) - RCLCPP_WARN( - logger, - "[Deprecated] Parameter 'gains..angle_wraparound' is deprecated. The " - "angle_wraparound is now used if a continuous joint is configured in the URDF."); - joints_angle_wraparound_[i] = true; - } - if (!urdf_.empty()) { auto urdf_joint = model_.getJoint(params_.joints[i]); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index b17229cab8..8bd64b6314 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -122,12 +122,6 @@ joint_trajectory_controller: default_value: 0.0, description: "Feed-forward scaling :math:`k_{ff}` of velocity" } - angle_wraparound: { - type: bool, - default_value: false, - description: "[deprecated] For joints that wrap around (ie. are continuous). - Normalizes position-error to -pi to pi." - } constraints: stopped_velocity_tolerance: { type: double, From 64adf67017860bf414f27153efdecfca568d7c87 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 5 Jun 2024 20:14:54 +0200 Subject: [PATCH 06/97] Remove unstamped twist subscribers + parameters (#1151) --- .../ackermann_steering_controller_params.yaml | 1 - ...steering_controller_preceeding_params.yaml | 1 - .../test_ackermann_steering_controller.hpp | 1 - .../bicycle_steering_controller_params.yaml | 1 - ...steering_controller_preceeding_params.yaml | 1 - .../test/test_bicycle_steering_controller.hpp | 1 - .../steering_controllers_library.hpp | 2 - .../src/steering_controllers_library.cpp | 55 +--------------- .../src/steering_controllers_library.yaml | 8 --- .../steering_controllers_library_params.yaml | 1 - .../test_steering_controllers_library.hpp | 1 - .../config/tricycle_drive_controller.yaml | 1 - .../tricycle_controller.hpp | 2 - .../src/tricycle_controller.cpp | 63 +++++-------------- .../src/tricycle_controller_parameter.yaml | 5 -- .../test_tricycle_steering_controller.hpp | 1 - .../tricycle_steering_controller_params.yaml | 1 - ...steering_controller_preceeding_params.yaml | 1 - 18 files changed, 20 insertions(+), 127 deletions(-) diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml index 6b64f901c3..6064814d32 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -6,7 +6,6 @@ test_ackermann_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] front_wheels_names: [front_right_steering_joint, front_left_steering_joint] diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml index ecb1b071e3..5d42adcdd5 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml @@ -5,7 +5,6 @@ test_ackermann_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] front_wheels_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint] rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index a047186d14..49331ae8a2 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -280,7 +280,6 @@ class AckermannSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; std::vector front_wheels_names_ = { "front_right_steering_joint", "front_left_steering_joint"}; diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml index a2a6c6508b..3a656cc724 100644 --- a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml +++ b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml @@ -6,7 +6,6 @@ test_bicycle_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [rear_wheel_joint] front_wheels_names: [steering_axis_joint] diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml index 39ffeed878..e1b9c1ab72 100644 --- a/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml +++ b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml @@ -5,7 +5,6 @@ test_bicycle_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [pid_controller/rear_wheel_joint] front_wheels_names: [pid_controller/steering_axis_joint] rear_wheels_state_names: [rear_wheel_joint] diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 5e21ff228c..390447c25f 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -251,7 +251,6 @@ class BicycleSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_wheel_joint"}; std::vector front_wheels_names_ = {"steering_axis_joint"}; std::vector joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]}; diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index b560e2a782..c11d90dc6f 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -98,7 +98,6 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; rclcpp::Subscription::SharedPtr ref_subscriber_ackermann_ = nullptr; - rclcpp::Subscription::SharedPtr ref_subscriber_unstamped_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms @@ -143,7 +142,6 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // callback for topic interface STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( const std::shared_ptr msg); - void reference_callback_unstamped(const std::shared_ptr msg); }; } // namespace steering_controllers_library diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 9bf9fa51d6..5880000d17 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -114,22 +114,9 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( // Reference Subscriber ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); - if (params_.use_stamped_vel) - { - RCLCPP_WARN( - get_node()->get_logger(), - "[Deprecated] Using geometry_msgs::msg::Twist instead of TwistStamped is deprecated."); - ref_subscriber_twist_ = get_node()->create_subscription( - "~/reference", subscribers_qos, - std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); - } - else - { - ref_subscriber_unstamped_ = get_node()->create_subscription( - "~/reference_unstamped", subscribers_qos, - std::bind( - &SteeringControllersLibrary::reference_callback_unstamped, this, std::placeholders::_1)); - } + ref_subscriber_twist_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); std::shared_ptr msg = std::make_shared(); @@ -244,42 +231,6 @@ void SteeringControllersLibrary::reference_callback( } } -void SteeringControllersLibrary::reference_callback_unstamped( - const std::shared_ptr msg) -{ - RCLCPP_WARN( - get_node()->get_logger(), - "Use of Twist message without stamped is deprecated and it will be removed in ROS 2 J-Turtle " - "version. Use '~/reference' topic with 'geometry_msgs::msg::TwistStamped' message type in the " - "future."); - auto twist_stamped = *(input_ref_.readFromNonRT()); - twist_stamped->header.stamp = get_node()->now(); - // if no timestamp provided use current time for command timestamp - if (twist_stamped->header.stamp.sec == 0 && twist_stamped->header.stamp.nanosec == 0u) - { - RCLCPP_WARN( - get_node()->get_logger(), - "Timestamp in header is missing, using current time as command timestamp."); - twist_stamped->header.stamp = get_node()->now(); - } - - const auto age_of_last_command = get_node()->now() - twist_stamped->header.stamp; - - if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) - { - twist_stamped->twist = *msg; - } - else - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " - "(%.4f).", - rclcpp::Time(twist_stamped->header.stamp).seconds(), age_of_last_command.seconds(), - ref_timeout_.seconds()); - } -} - controller_interface::InterfaceConfiguration SteeringControllersLibrary::command_interface_configuration() const { diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index b18cac5ae1..711a780458 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -112,11 +112,3 @@ steering_controllers_library: position_feedback is true then ``HW_IF_POSITION`` is taken as interface type", read_only: false, } - - use_stamped_vel: { - type: bool, - default_value: false, - description: "(Deprecated) Choice of vel type, if use_stamped_vel is false then ``geometry_msgs::msg::Twist`` is taken as vel msg type, if - use_stamped_vel is true then ``geometry_msgs::msg::TwistStamped`` is taken as vel msg type", - read_only: false, - } diff --git a/steering_controllers_library/test/steering_controllers_library_params.yaml b/steering_controllers_library/test/steering_controllers_library_params.yaml index d200d34961..01bfb02302 100644 --- a/steering_controllers_library/test/steering_controllers_library_params.yaml +++ b/steering_controllers_library/test/steering_controllers_library_params.yaml @@ -6,7 +6,6 @@ test_steering_controllers_library: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] front_wheels_names: [front_right_steering_joint, front_left_steering_joint] diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 1284b72096..05ac17b454 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -301,7 +301,6 @@ class SteeringControllersLibraryFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; std::vector front_wheels_names_ = { "front_right_steering_joint", "front_left_steering_joint"}; diff --git a/tricycle_controller/config/tricycle_drive_controller.yaml b/tricycle_controller/config/tricycle_drive_controller.yaml index fba6f34bb2..a7853954d7 100644 --- a/tricycle_controller/config/tricycle_drive_controller.yaml +++ b/tricycle_controller/config/tricycle_drive_controller.yaml @@ -36,7 +36,6 @@ tricycle_controller: # 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/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 24aaf89de9..b6f9aae417 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -140,8 +140,6 @@ class TricycleController : public controller_interface::ControllerInterface 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}; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 66c6ed1b0f..94ff63e659 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -239,7 +239,6 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & cmd_vel_timeout_ = std::chrono::milliseconds{params_.cmd_vel_timeout}; params_.publish_ackermann_command = get_node()->get_parameter("publish_ackermann_command").as_bool(); - params_.use_stamped_vel = get_node()->get_parameter("use_stamped_vel").as_bool(); try { @@ -291,52 +290,25 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } // initialize command subscriber - if (params_.use_stamped_vel) - { - velocity_command_subscriber_ = get_node()->create_subscription( - DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + velocity_command_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) { - 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 - { - RCLCPP_WARN( - get_node()->get_logger(), - "[Deprecated] Using geometry_msgs::msg::Twist instead of TwistStamped is deprecated."); - velocity_command_unstamped_subscriber_ = get_node()->create_subscription( - DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + 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)) { - 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(); - }); - } + 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)); + }); // initialize odometry publisher and message odometry_publisher_ = get_node()->create_publisher( @@ -460,7 +432,6 @@ bool TricycleController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - velocity_command_unstamped_subscriber_.reset(); received_velocity_msg_ptr_.set(nullptr); is_halted = false; diff --git a/tricycle_controller/src/tricycle_controller_parameter.yaml b/tricycle_controller/src/tricycle_controller_parameter.yaml index b6d1fdf964..cf661eddc1 100644 --- a/tricycle_controller/src/tricycle_controller_parameter.yaml +++ b/tricycle_controller/src/tricycle_controller_parameter.yaml @@ -84,11 +84,6 @@ tricycle_controller: gt<>: [0] } } - use_stamped_vel: { - type: bool, - default_value: true, - description: "(deprecated) Use stamp from input velocity message to calculate how old the command actually is.", - } traction: # "The positive limit will be applied to both directions. Setting different limits for positive " # "and negative directions is not supported. Actuators are " diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 6a516691b8..1807ab59a8 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -265,7 +265,6 @@ class TricycleSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; std::vector front_wheels_names_ = {"steering_axis_joint"}; std::vector joint_names_ = { diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml index 6bfb87a892..cc640e1503 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml @@ -6,7 +6,6 @@ test_tricycle_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] front_wheels_names: [steering_axis_joint] diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml index ea8b88002e..7cb2e4906f 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml @@ -5,7 +5,6 @@ test_tricycle_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] front_wheels_names: [pid_controller/steering_axis_joint] rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] From b24515538cd31b6c2b64268465dd479f464b151d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 5 Jun 2024 21:50:09 +0200 Subject: [PATCH 07/97] Fix steering controllers library code documentation and naming (#1149) * Update documentation and consolidate variable names * Simplify private methods and further update docs * Rename methods * Rename method and variables * Rename convert method * Rename variables and improve doc * Rename local variables * Use std::isfinite instead of !isnan Co-authored-by: Sai Kishor Kothakota * Use a lowercase theta for heading Co-authored-by: Sai Kishor Kothakota * Make some temporary variables const * Let update_from_position call update_from_velocity * Explicitly set variables with 0 in constructor * Fix docstring * Apply consistent variable naming Co-authored-by: Quique Llorente --------- Co-authored-by: Sai Kishor Kothakota Co-authored-by: Quique Llorente --- .../src/ackermann_steering_controller.cpp | 23 +-- .../src/bicycle_steering_controller.cpp | 10 +- .../steering_odometry.hpp | 63 +++---- .../src/steering_odometry.cpp | 175 ++++++++---------- .../src/tricycle_steering_controller.cpp | 18 +- 5 files changed, 139 insertions(+), 150 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index c3a7539c5a..d9d95bf8b5 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -62,28 +62,29 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio } else { - const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); - const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); - const double front_right_steer_position = - state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(); - const double front_left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(); + const double traction_right_wheel_value = + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); + const double traction_left_wheel_value = + state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); + const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(); + const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(); if ( - !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && - !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position)) + std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && + std::isfinite(steering_right_position) && std::isfinite(steering_left_position)) { if (params_.position_feedback) { // Estimate linear and angular velocity using joint information odometry_.update_from_position( - rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, - front_left_steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steering_right_position, + steering_left_position, period.seconds()); } else { // Estimate linear and angular velocity using joint information odometry_.update_from_velocity( - rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, - front_left_steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steering_right_position, + steering_left_position, period.seconds()); } } } diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index 5f013d7d7a..95eaf1965c 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -60,19 +60,19 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) } else { - const double rear_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value(); - const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value(); - if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) + const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value(); + const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value(); + if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position)) { if (params_.position_feedback) { // Estimate linear and angular velocity using joint information - odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); + odometry_.update_from_position(traction_wheel_value, steering_position, period.seconds()); } else { // Estimate linear and angular velocity using joint information - odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); + odometry_.update_from_velocity(traction_wheel_value, steering_position, period.seconds()); } } } diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index e4a22f6d3b..0104b011c7 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -61,7 +61,7 @@ class SteeringOdometry /** * \brief Updates the odometry class with latest wheels position * \param traction_wheel_pos traction wheel position [rad] - * \param steer_pos Front Steer position [rad] + * \param steer_pos Steer wheel position [rad] * \param dt time difference to last call * \return true if the odometry is actually updated */ @@ -72,7 +72,7 @@ class SteeringOdometry * \brief Updates the odometry class with latest wheels position * \param right_traction_wheel_pos Right traction wheel velocity [rad] * \param left_traction_wheel_pos Left traction wheel velocity [rad] - * \param front_steer_pos Steer wheel position [rad] + * \param steer_pos Steer wheel position [rad] * \param dt time difference to last call * \return true if the odometry is actually updated */ @@ -96,7 +96,7 @@ class SteeringOdometry /** * \brief Updates the odometry class with latest wheels position * \param traction_wheel_vel Traction wheel velocity [rad/s] - * \param front_steer_pos Steer wheel position [rad] + * \param steer_pos Steer wheel position [rad] * \param dt time difference to last call * \return true if the odometry is actually updated */ @@ -107,7 +107,7 @@ class SteeringOdometry * \brief Updates the odometry class with latest wheels position * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] - * \param front_steer_pos Steer wheel position [rad] + * \param steer_pos Steer wheel position [rad] * \param dt time difference to last call * \return true if the odometry is actually updated */ @@ -130,11 +130,11 @@ class SteeringOdometry /** * \brief Updates the odometry class with latest velocity command - * \param linear Linear velocity [m/s] - * \param angular Angular velocity [rad/s] - * \param time Current time + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call */ - void update_open_loop(const double linear, const double angular, const double dt); + void update_open_loop(const double v_bx, const double omega_bz, const double dt); /** * \brief Set odometry type @@ -175,22 +175,23 @@ class SteeringOdometry /** * \brief Sets the wheel parameters: radius, separation and wheelbase */ - void set_wheel_params(double wheel_radius, double wheelbase = 0.0, double wheel_track = 0.0); + void set_wheel_params( + const double wheel_radius, const double wheelbase = 0.0, const double wheel_track = 0.0); /** * \brief Velocity rolling window size setter * \param velocity_rolling_window_size Velocity rolling window size */ - void set_velocity_rolling_window_size(size_t velocity_rolling_window_size); + void set_velocity_rolling_window_size(const size_t velocity_rolling_window_size); /** * \brief Calculates inverse kinematics for the desired linear and angular velocities - * \param Vx Desired linear velocity [m/s] - * \param theta_dot Desired angular velocity [rad/s] + * \param v_bx Desired linear velocity of the robot in x_b-axis direction + * \param omega_bz Desired angular velocity of the robot around x_z-axis * \return Tuple of velocity commands and steering commands */ std::tuple, std::vector> get_commands( - const double Vx, const double theta_dot); + const double v_bx, const double omega_bz); /** * \brief Reset poses, heading, and accumulators @@ -199,35 +200,35 @@ class SteeringOdometry private: /** - * \brief Uses precomputed linear and angular velocities to compute dometry and update - * accumulators \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) - * computed by previous odometry method \param angular Angular velocity [rad] (angular - * displacement, i.e. m/s * dt) computed by previous odometry method + * \brief Uses precomputed linear and angular velocities to compute odometry + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call */ - bool update_odometry(const double linear_velocity, const double angular, const double dt); + bool update_odometry(const double v_bx, const double omega_bz, const double dt); /** * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta - * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by - * encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed - * by encoders + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call */ - void integrate_runge_kutta_2(double linear, double angular); + void integrate_runge_kutta_2(const double v_bx, const double omega_bz, const double dt); /** - * \brief Integrates the velocities (linear and angular) using exact method - * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by - * encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed - * by encoders + * \brief Integrates the velocities (linear and angular) + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call */ - void integrate_exact(double linear, double angular); + void integrate_fk(const double v_bx, const double omega_bz, const double dt); /** - * \brief Calculates steering angle from the desired translational and rotational velocity - * \param Vx Linear velocity [m] - * \param theta_dot Angular velocity [rad] + * \brief Calculates steering angle from the desired twist + * \param v_bx Linear velocity of the robot in x_b-axis direction + * \param omega_bz Angular velocity of the robot around x_z-axis */ - double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot); + double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); /** * \brief Reset linear and angular accumulators diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 6fb20478a4..32cdb69454 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -35,6 +35,8 @@ SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size) wheelbase_(0.0), wheel_radius_(0.0), traction_wheel_old_pos_(0.0), + traction_right_wheel_old_pos_(0.0), + traction_left_wheel_old_pos_(0.0), velocity_rolling_window_size_(velocity_rolling_window_size), linear_acc_(velocity_rolling_window_size), angular_acc_(velocity_rolling_window_size) @@ -49,10 +51,10 @@ void SteeringOdometry::init(const rclcpp::Time & time) } bool SteeringOdometry::update_odometry( - const double linear_velocity, const double angular, const double dt) + const double linear_velocity, const double angular_velocity, const double dt) { /// Integrate odometry: - SteeringOdometry::integrate_exact(linear_velocity * dt, angular * dt); + integrate_fk(linear_velocity, angular_velocity, dt); /// We cannot estimate the speed with very small time intervals: if (dt < 0.0001) @@ -62,7 +64,7 @@ bool SteeringOdometry::update_odometry( /// Estimate speeds using a rolling mean to filter them out: linear_acc_.accumulate(linear_velocity); - angular_acc_.accumulate(angular); + angular_acc_.accumulate(angular_velocity); linear_ = linear_acc_.getRollingMean(); angular_ = angular_acc_.getRollingMean(); @@ -73,70 +75,47 @@ bool SteeringOdometry::update_odometry( bool SteeringOdometry::update_from_position( const double traction_wheel_pos, const double steer_pos, const double dt) { - /// Get current wheel joint positions: - const double traction_wheel_cur_pos = traction_wheel_pos * wheel_radius_; - const double traction_wheel_est_pos_diff = traction_wheel_cur_pos - traction_wheel_old_pos_; + const double traction_wheel_est_pos_diff = traction_wheel_pos - traction_wheel_old_pos_; /// Update old position with current: - traction_wheel_old_pos_ = traction_wheel_cur_pos; + traction_wheel_old_pos_ = traction_wheel_pos; - /// Compute linear and angular diff: - const double linear_velocity = traction_wheel_est_pos_diff / dt; - steer_pos_ = steer_pos; - const double angular = tan(steer_pos) * linear_velocity / wheelbase_; - - return update_odometry(linear_velocity, angular, dt); + return update_from_velocity(traction_wheel_est_pos_diff / dt, steer_pos, dt); } bool SteeringOdometry::update_from_position( const double traction_right_wheel_pos, const double traction_left_wheel_pos, const double steer_pos, const double dt) { - /// Get current wheel joint positions: - const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_; - const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_; - const double traction_right_wheel_est_pos_diff = - traction_right_wheel_cur_pos - traction_right_wheel_old_pos_; + traction_right_wheel_pos - traction_right_wheel_old_pos_; const double traction_left_wheel_est_pos_diff = - traction_left_wheel_cur_pos - traction_left_wheel_old_pos_; + traction_left_wheel_pos - traction_left_wheel_old_pos_; /// Update old position with current: - traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos; - traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos; - - const double linear_velocity = - (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt; - steer_pos_ = steer_pos; - const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; + traction_right_wheel_old_pos_ = traction_right_wheel_pos; + traction_left_wheel_old_pos_ = traction_left_wheel_pos; - return update_odometry(linear_velocity, angular, dt); + return update_from_velocity( + traction_right_wheel_est_pos_diff / dt, traction_left_wheel_est_pos_diff / dt, steer_pos, dt); } bool SteeringOdometry::update_from_position( const double traction_right_wheel_pos, const double traction_left_wheel_pos, const double right_steer_pos, const double left_steer_pos, const double dt) { - /// Get current wheel joint positions: - const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_; - const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_; - const double traction_right_wheel_est_pos_diff = - traction_right_wheel_cur_pos - traction_right_wheel_old_pos_; + traction_right_wheel_pos - traction_right_wheel_old_pos_; const double traction_left_wheel_est_pos_diff = - traction_left_wheel_cur_pos - traction_left_wheel_old_pos_; + traction_left_wheel_pos - traction_left_wheel_old_pos_; /// Update old position with current: - traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos; - traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos; - - /// Compute linear and angular diff: - const double linear_velocity = - (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt; - steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; - const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; + traction_right_wheel_old_pos_ = traction_right_wheel_pos; + traction_left_wheel_old_pos_ = traction_left_wheel_pos; - return update_odometry(linear_velocity, angular, dt); + return update_from_velocity( + traction_right_wheel_est_pos_diff / dt, traction_left_wheel_est_pos_diff / dt, right_steer_pos, + left_steer_pos, dt); } bool SteeringOdometry::update_from_velocity( @@ -144,9 +123,9 @@ bool SteeringOdometry::update_from_velocity( { steer_pos_ = steer_pos; double linear_velocity = traction_wheel_vel * wheel_radius_; - const double angular = tan(steer_pos) * linear_velocity / wheelbase_; + const double angular_velocity = tan(steer_pos) * linear_velocity / wheelbase_; - return update_odometry(linear_velocity, angular, dt); + return update_odometry(linear_velocity, angular_velocity, dt); } bool SteeringOdometry::update_from_velocity( @@ -157,9 +136,9 @@ bool SteeringOdometry::update_from_velocity( (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; steer_pos_ = steer_pos; - const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; + const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_; - return update_odometry(linear_velocity, angular, dt); + return update_odometry(linear_velocity, angular_velocity, dt); } bool SteeringOdometry::update_from_velocity( @@ -169,19 +148,19 @@ bool SteeringOdometry::update_from_velocity( steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; double linear_velocity = (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; - const double angular = steer_pos_ * linear_velocity / wheelbase_; + const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_; - return update_odometry(linear_velocity, angular, dt); + return update_odometry(linear_velocity, angular_velocity, dt); } -void SteeringOdometry::update_open_loop(const double linear, const double angular, const double dt) +void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt) { /// Save last linear and angular velocity: - linear_ = linear; - angular_ = angular; + linear_ = v_bx; + angular_ = omega_bz; /// Integrate odometry: - SteeringOdometry::integrate_exact(linear * dt, angular * dt); + integrate_fk(v_bx, omega_bz, dt); } void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track) @@ -200,36 +179,37 @@ void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_ void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ = type; } -double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot) +double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) { - if (theta_dot == 0 || Vx == 0) + if (omega_bz == 0 || v_bx == 0) { return 0; } - return std::atan(theta_dot * wheelbase_ / Vx); + return std::atan(omega_bz * wheelbase_ / v_bx); } std::tuple, std::vector> SteeringOdometry::get_commands( - const double Vx, const double theta_dot) + const double v_bx, const double omega_bz) { - // desired velocity and steering angle of the middle of traction and steering axis - double Ws, alpha; + // desired wheel speed and steering angle of the middle of traction and steering axis + double Ws, phi; - if (Vx == 0 && theta_dot != 0) + if (v_bx == 0 && omega_bz != 0) { - alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; - Ws = abs(theta_dot) * wheelbase_ / wheel_radius_; + // TODO(anyone) would be only valid if traction is on the steering axis -> tricycle_controller + phi = omega_bz > 0 ? M_PI_2 : -M_PI_2; + Ws = abs(omega_bz) * wheelbase_ / wheel_radius_; } else { - alpha = SteeringOdometry::convert_trans_rot_vel_to_steering_angle(Vx, theta_dot); - Ws = Vx / (wheel_radius_ * std::cos(steer_pos_)); + phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); + Ws = v_bx / (wheel_radius_ * std::cos(steer_pos_)); } if (config_type_ == BICYCLE_CONFIG) { std::vector traction_commands = {Ws}; - std::vector steering_commands = {alpha}; + std::vector steering_commands = {phi}; return std::make_tuple(traction_commands, steering_commands); } else if (config_type_ == TRICYCLE_CONFIG) @@ -242,12 +222,12 @@ std::tuple, std::vector> SteeringOdometry::get_comma } else { - double turning_radius = wheelbase_ / std::tan(steer_pos_); - double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; - double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + const double turning_radius = wheelbase_ / std::tan(steer_pos_); + const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; } - steering_commands = {alpha}; + steering_commands = {phi}; return std::make_tuple(traction_commands, steering_commands); } else if (config_type_ == ACKERMANN_CONFIG) @@ -257,21 +237,23 @@ std::tuple, std::vector> SteeringOdometry::get_comma if (fabs(steer_pos_) < 1e-6) { traction_commands = {Ws, Ws}; - steering_commands = {alpha, alpha}; + steering_commands = {phi, phi}; } else { - double turning_radius = wheelbase_ / std::tan(steer_pos_); - double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; - double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + const double turning_radius = wheelbase_ / std::tan(steer_pos_); + const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; - double numerator = 2 * wheelbase_ * std::sin(alpha); - double denominator_first_member = 2 * wheelbase_ * std::cos(alpha); - double denominator_second_member = wheel_track_ * std::sin(alpha); + const double numerator = 2 * wheelbase_ * std::sin(phi); + const double denominator_first_member = 2 * wheelbase_ * std::cos(phi); + const double denominator_second_member = wheel_track_ * std::sin(phi); - double alpha_r = std::atan2(numerator, denominator_first_member + denominator_second_member); - double alpha_l = std::atan2(numerator, denominator_first_member - denominator_second_member); + const double alpha_r = + std::atan2(numerator, denominator_first_member + denominator_second_member); + const double alpha_l = + std::atan2(numerator, denominator_first_member - denominator_second_member); steering_commands = {alpha_r, alpha_l}; } return std::make_tuple(traction_commands, steering_commands); @@ -290,35 +272,36 @@ void SteeringOdometry::reset_odometry() reset_accumulators(); } -void SteeringOdometry::integrate_runge_kutta_2(double linear, double angular) +void SteeringOdometry::integrate_runge_kutta_2( + const double v_bx, const double omega_bz, const double dt) { - const double direction = heading_ + angular * 0.5; + // Compute intermediate value of the heading + const double theta_mid = heading_ + omega_bz * 0.5 * dt; - /// Runge-Kutta 2nd order integration: - x_ += linear * cos(direction); - y_ += linear * sin(direction); - heading_ += angular; + // Use the intermediate values to update the state + x_ += v_bx * cos(theta_mid) * dt; + y_ += v_bx * sin(theta_mid) * dt; + heading_ += omega_bz * dt; } -/** - * \brief Other possible integration method provided by the class - * \param linear - * \param angular - */ -void SteeringOdometry::integrate_exact(double linear, double angular) +void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, const double dt) { - if (fabs(angular) < 1e-6) + const double delta_x_b = v_bx * dt; + const double delta_theta = omega_bz * dt; + + if (fabs(delta_theta) < 1e-6) { - integrate_runge_kutta_2(linear, angular); + /// Runge-Kutta 2nd Order (should solve problems when omega_bz is zero): + integrate_runge_kutta_2(v_bx, omega_bz, dt); } else { - /// Exact integration (should solve problems when angular is zero): + /// Exact integration 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)); + const double R = delta_x_b / delta_theta; + heading_ += delta_theta; + x_ += R * (sin(heading_) - sin(heading_old)); + y_ += -R * (cos(heading_) - cos(heading_old)); } } diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index f89d78a52c..03be6b88f0 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -60,24 +60,28 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period } else { - const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); - const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); - const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value(); + const double traction_right_wheel_value = + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); + const double traction_left_wheel_value = + state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); + const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value(); if ( - !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && - !std::isnan(steer_position)) + std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && + std::isfinite(steering_position)) { if (params_.position_feedback) { // Estimate linear and angular velocity using joint information odometry_.update_from_position( - rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steering_position, + period.seconds()); } else { // Estimate linear and angular velocity using joint information odometry_.update_from_velocity( - rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steering_position, + period.seconds()); } } } From 615524831a3bd3e0fc4e4678c6eb9afd7f39f326 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 18 Jun 2024 10:21:17 +0200 Subject: [PATCH 08/97] [Steering controllers library] Reference interfaces are body twist (#1168) --- .../test/test_ackermann_steering_controller.cpp | 2 +- .../test/test_ackermann_steering_controller.hpp | 2 +- .../test/test_bicycle_steering_controller.cpp | 2 +- .../test/test_bicycle_steering_controller.hpp | 2 +- steering_controllers_library/doc/userdoc.rst | 4 +++- .../steering_controllers_library.hpp | 2 +- .../src/steering_controllers_library.cpp | 4 ++-- .../test/test_steering_controllers_library.cpp | 2 +- .../test/test_steering_controllers_library.hpp | 2 +- .../test/test_tricycle_steering_controller.hpp | 2 +- 10 files changed, 13 insertions(+), 11 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index ef5454a16c..38cca0cac9 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -84,7 +84,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME + // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 49331ae8a2..363db793d5 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -302,7 +302,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 3dcdc0b1db..fd8565853f 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -68,7 +68,7 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME + // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 390447c25f..dcdb7ed169 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -266,7 +266,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {3.3, 0.5}; std::array joint_command_values_ = {1.1, 2.2}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index a52e34c120..44b180162e 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -56,7 +56,9 @@ References (from a preceding controller) Used when controller is in chained mode (``in_chained_mode == true``). - ``/linear/velocity`` double, in m/s -- ``/angular/position`` double, in rad +- ``/angular/velocity`` double, in rad/s + +representing the body twist. Command interfaces ,,,,,,,,,,,,,,,,,,, diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index c11d90dc6f..e417b9fcd1 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -131,7 +131,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // name constants for reference interfaces size_t nr_ref_itfs_; - // store last velocity + // last velocity commands for open loop odometry double last_linear_velocity_ = 0.0; double last_angular_velocity_ = 0.0; diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 5880000d17..5445196a6e 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -324,7 +324,7 @@ SteeringControllersLibrary::on_export_reference_interfaces() &reference_interfaces_[0])); reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_POSITION, + get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY, &reference_interfaces_[1])); return reference_interfaces; @@ -396,7 +396,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { - // store and set commands + // store (for open loop odometry) and set commands last_linear_velocity_ = reference_interfaces_[0]; last_angular_velocity_ = reference_interfaces_[1]; diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 0217567a26..98ab97fdc3 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -66,7 +66,7 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME + // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 05ac17b454..b6fe1770c1 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -324,7 +324,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 1807ab59a8..b4d58a95c0 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -285,7 +285,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {0.5, 0.5, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; From b81be48fb1b6101a3c601c0ea11c5393e5a7026e Mon Sep 17 00:00:00 2001 From: Enrique Llorente Pastora Date: Wed, 19 Jun 2024 19:31:22 +0200 Subject: [PATCH 09/97] [STEERING] Add missing `tan` call for ackermann (#1117) --- steering_controllers_library/src/steering_odometry.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 32cdb69454..0d12518a3d 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -123,7 +123,7 @@ bool SteeringOdometry::update_from_velocity( { steer_pos_ = steer_pos; double linear_velocity = traction_wheel_vel * wheel_radius_; - const double angular_velocity = tan(steer_pos) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -136,7 +136,7 @@ bool SteeringOdometry::update_from_velocity( (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; steer_pos_ = steer_pos; - const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -148,7 +148,7 @@ bool SteeringOdometry::update_from_velocity( steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; double linear_velocity = (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; - const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } From dc928985b34c8bf439bc92f210d26209d09f262c Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 1 Jul 2024 13:52:30 +0200 Subject: [PATCH 10/97] Bump version of pre-commit hooks (#1185) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index c75acbd76d..29862f70e4 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -37,7 +37,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.15.2 + rev: v3.16.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -56,14 +56,14 @@ repos: args: ["--line-length=99"] - repo: https://github.com/pycqa/flake8 - rev: 7.0.0 + rev: 7.1.0 hooks: - id: flake8 args: ["--extend-ignore=E501"] # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.5 + rev: v18.1.8 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.28.4 + rev: 0.28.6 hooks: - id: check-github-workflows args: ["--verbose"] From 6439a0ded6d0072af0be710cd371a6897e5924a9 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 1 Jul 2024 19:03:41 +0100 Subject: [PATCH 11/97] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 7 +++++++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_controller/CHANGELOG.rst | 7 +++++++ 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 | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ pid_controller/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 8 ++++++++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 7 +++++++ velocity_controllers/CHANGELOG.rst | 3 +++ 21 files changed, 84 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 5cfdefb83b..165573dc84 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Quique Llorente + 4.9.0 (2024-06-05) ------------------ diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 1a2dd8ed96..ce1aa256f1 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 8c3d27e24f..52c41f549c 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Quique Llorente + 4.9.0 (2024-06-05) ------------------ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index a4473f0f83..6be80a2e54 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 63b44cd218..eb619ca1ba 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index deca56a197..005a23c0e4 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 +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 3f1917dcad..1db9c16874 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index e0b5a0a311..e7a8105cd2 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index cebfc8d78c..96141e51e1 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index ec9b79726f..8582c56d4f 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 544cf500ef..4d7071701b 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove manual angle-wraparound parameter (`#1152 `_) +* Contributors: Christoph Fröhlich + 4.9.0 (2024-06-05) ------------------ * JTC trajectory end time validation fix (`#1090 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 4b884dc386..58bdcd3691 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index f454aadc4e..85c48140ff 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 7dd295970d..a689b4fda5 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index af872ec8a3..5fcde17af9 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ * Add custom rosdoc2 config for ros2_controllers metapackage (`#1100 `_) diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 19ead3093f..99553224b7 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 +----------- + 4.9.0 (2024-06-05) ------------------ diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 121a5066c2..96cab6e11c 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 +----------- + 4.9.0 (2024-06-05) ------------------ * [RQT-JTC] limits from jtc controlled joints (`#1146 `_) diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index e1cdc069df..8a16a89fc4 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [STEERING] Add missing `tan` call for ackermann (`#1117 `_) +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich, Enrique Llorente Pastora, Quique Llorente + 4.9.0 (2024-06-05) ------------------ * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 8f36bae23c..231774c823 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich + 4.9.0 (2024-06-05) ------------------ * Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index f6423146a0..901ce56631 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Quique Llorente + 4.9.0 (2024-06-05) ------------------ diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 64a09e4309..ca954bb079 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.9.0 (2024-06-05) ------------------ From b441ced4c19dba79067239e7a28751adad298640 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 1 Jul 2024 19:04:06 +0100 Subject: [PATCH 12/97] 4.10.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_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 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/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 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 165573dc84..e27cac2205 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) * Fix steering controllers library code documentation and naming (`#1149 `_) * Remove unstamped twist subscribers + parameters (`#1151 `_) diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 7b66dd765b..77555c1f01 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.9.0 + 4.10.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index ce1aa256f1..a3faf30ab1 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 2add40baf7..9abdb5cf82 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.9.0 + 4.10.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 52c41f549c..a8956560b5 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) * Fix steering controllers library code documentation and naming (`#1149 `_) * Remove unstamped twist subscribers + parameters (`#1151 `_) diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index bcad405abe..b49b9f1a23 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.9.0 + 4.10.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 6be80a2e54..5b9275359d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 8765d2506f..9cfdee9a29 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.9.0 + 4.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 eb619ca1ba..9b1240263b 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 6519a07b07..faf31ada51 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.9.0 + 4.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 005a23c0e4..8f978cfd08 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 ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 4576bbb8b0..ae0c89dcf2 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.9.0 + 4.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 1db9c16874..75926feff6 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index b4f34fce8c..1e24ca0201 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.9.0 + 4.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index e7a8105cd2..adf9f8e208 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index f4ee00591a..5a9ff6ed87 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.9.0 + 4.10.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 96141e51e1..6649777076 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 032e5addc1..383c90d89a 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.9.0 + 4.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 8582c56d4f..e4f73e5cc6 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index a34c05b269..9ca30f4e52 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.9.0 + 4.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 4d7071701b..2c044b28b0 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * Remove manual angle-wraparound parameter (`#1152 `_) * Contributors: Christoph Fröhlich diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 2c04d7fa23..733059736a 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.9.0 + 4.10.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 58bdcd3691..101d507f56 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/pid_controller/package.xml b/pid_controller/package.xml index be598f94ae..3bd87c1875 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.9.0 + 4.10.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 85c48140ff..0582a40895 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 37bcdbe108..013b60c790 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.9.0 + 4.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index a689b4fda5..7c671ae3d3 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 31e8b1755b..da74b2d62f 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.9.0 + 4.10.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 5fcde17af9..1db9ae1c05 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 278ecb0b1a..14f686cce4 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.9.0 + 4.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 99553224b7..c483ee7c2f 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 ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index c321c9703d..2c26e5cfe0 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.9.0 + 4.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 e0dfb4d65e..bb9ff7b303 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.9.0", + version="4.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 96cab6e11c..bd2b00a1fe 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 ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 68a173217a..4d42c2d00d 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 - 4.9.0 + 4.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 e9a19034c6..b966635801 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.9.0", + version="4.10.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 8a16a89fc4..ee21fed403 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * [STEERING] Add missing `tan` call for ackermann (`#1117 `_) * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) * Fix steering controllers library code documentation and naming (`#1149 `_) diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 08d252d794..5b55f7f9de 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.9.0 + 4.10.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 231774c823..4034036e08 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * Remove unstamped twist subscribers + parameters (`#1151 `_) * Contributors: Christoph Fröhlich diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 51fe867106..cdb62117fd 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.9.0 + 4.10.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 901ce56631..c80b90ff0f 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) * Fix steering controllers library code documentation and naming (`#1149 `_) * Remove unstamped twist subscribers + parameters (`#1151 `_) diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 6e1ed66edd..f2408123df 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.9.0 + 4.10.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index ca954bb079..c4d146a21d 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.10.0 (2024-07-01) +------------------- 4.9.0 (2024-06-05) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 05f6245686..75c05fab94 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.9.0 + 4.10.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From df0449263f1568c4340a4e712ffb8d8412e06a0b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 3 Jul 2024 19:43:19 +0200 Subject: [PATCH 13/97] Fix steering controllers library kinematics (#1150) --- .../test_ackermann_steering_controller.cpp | 6 +- .../test/test_bicycle_steering_controller.cpp | 10 +- .../steering_odometry.hpp | 17 ++- .../src/steering_controllers_library.cpp | 2 +- .../src/steering_odometry.cpp | 82 ++++++++--- .../test/test_steering_odometry.cpp | 127 +++++++++++++++--- .../test_tricycle_steering_controller.cpp | 3 + 7 files changed, 197 insertions(+), 50 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 38cca0cac9..718e6f5856 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -173,6 +173,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -211,8 +212,9 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NEAR( + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands + EXPECT_NEAR( controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( @@ -261,6 +263,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -276,6 +279,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat subscribe_and_get_messages(msg); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( msg.linear_velocity_command[CMD_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index fd8565853f..9904f791b6 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -158,7 +158,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); @@ -190,7 +190,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); @@ -237,7 +237,7 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status EXPECT_EQ(msg.linear_velocity_command[0], 1.1); EXPECT_EQ(msg.steering_angle_command[0], 2.2); - publish_commands(); + publish_commands(0.1, 0.2); ASSERT_TRUE(controller_->wait_for_commands(executor)); ASSERT_EQ( @@ -245,14 +245,14 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); - EXPECT_NEAR(msg.linear_velocity_command[0], 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR(msg.linear_velocity_command[0], 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); } diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 0104b011c7..dadc5730d4 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -18,6 +18,7 @@ #ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ #define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ +#include #include #include @@ -36,6 +37,9 @@ namespace steering_odometry const unsigned int BICYCLE_CONFIG = 0; const unsigned int TRICYCLE_CONFIG = 1; const unsigned int ACKERMANN_CONFIG = 2; + +inline bool is_close_to_zero(double val) { return std::fabs(val) < 1e-6; } + /** * \brief The Odometry class handles odometry readings * (2D pose and velocity with related timestamp) @@ -188,10 +192,11 @@ class SteeringOdometry * \brief Calculates inverse kinematics for the desired linear and angular velocities * \param v_bx Desired linear velocity of the robot in x_b-axis direction * \param omega_bz Desired angular velocity of the robot around x_z-axis + * \param open_loop If false, the IK will be calculated using measured steering angle * \return Tuple of velocity commands and steering commands */ std::tuple, std::vector> get_commands( - const double v_bx, const double omega_bz); + const double v_bx, const double omega_bz, const bool open_loop = true); /** * \brief Reset poses, heading, and accumulators @@ -230,6 +235,16 @@ class SteeringOdometry */ double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); + /** + * \brief Calculates linear velocity of a robot with double traction axle + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param steer_pos Steer wheel position [rad] + */ + double get_linear_velocity_double_traction_axle( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos); + /** * \brief Reset linear and angular accumulators */ diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 5445196a6e..1aef556212 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -401,7 +401,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c last_angular_velocity_ = reference_interfaces_[1]; auto [traction_commands, steering_commands] = - odometry_.get_commands(last_linear_velocity_, last_angular_velocity_); + odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop); if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 0d12518a3d..f8b42771e2 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -21,6 +21,7 @@ #include #include +#include namespace steering_odometry { @@ -128,13 +129,26 @@ bool SteeringOdometry::update_from_velocity( return update_odometry(linear_velocity, angular_velocity, dt); } +double SteeringOdometry::get_linear_velocity_double_traction_axle( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos) +{ + double turning_radius = wheelbase_ / std::tan(steer_pos); + // overdetermined, we take the average + double vel_r = right_traction_wheel_vel * wheel_radius_ * turning_radius / + (turning_radius + wheel_track_ * 0.5); + double vel_l = left_traction_wheel_vel * wheel_radius_ * turning_radius / + (turning_radius - wheel_track_ * 0.5); + return (vel_r + vel_l) * 0.5; +} + bool SteeringOdometry::update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double steer_pos, const double dt) { - double linear_velocity = - (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; steer_pos_ = steer_pos; + double linear_velocity = get_linear_velocity_double_traction_axle( + right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; @@ -145,10 +159,18 @@ bool SteeringOdometry::update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double right_steer_pos, const double left_steer_pos, const double dt) { - steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; - double linear_velocity = - (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; - const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; + // overdetermined, we take the average + const double right_steer_pos_est = std::atan( + wheelbase_ * std::tan(right_steer_pos) / + (wheelbase_ - wheel_track_ / 2 * std::tan(right_steer_pos))); + const double left_steer_pos_est = std::atan( + wheelbase_ * std::tan(left_steer_pos) / + (wheelbase_ + wheel_track_ / 2 * std::tan(left_steer_pos))); + steer_pos_ = (right_steer_pos_est + left_steer_pos_est) * 0.5; + + double linear_velocity = get_linear_velocity_double_traction_axle( + right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); + const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -181,30 +203,41 @@ void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) { - if (omega_bz == 0 || v_bx == 0) + if (fabs(v_bx) < std::numeric_limits::epsilon()) { - return 0; + // avoid division by zero + return 0.; } return std::atan(omega_bz * wheelbase_ / v_bx); } std::tuple, std::vector> SteeringOdometry::get_commands( - const double v_bx, const double omega_bz) + const double v_bx, const double omega_bz, const bool open_loop) { // desired wheel speed and steering angle of the middle of traction and steering axis - double Ws, phi; + double Ws, phi, phi_IK = steer_pos_; +#if 0 if (v_bx == 0 && omega_bz != 0) { - // TODO(anyone) would be only valid if traction is on the steering axis -> tricycle_controller + // TODO(anyone) this would be only possible if traction is on the steering axis phi = omega_bz > 0 ? M_PI_2 : -M_PI_2; Ws = abs(omega_bz) * wheelbase_ / wheel_radius_; } else { - phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); - Ws = v_bx / (wheel_radius_ * std::cos(steer_pos_)); + // TODO(anyone) this would be valid only if traction is on the steering axis + Ws = v_bx / (wheel_radius_ * std::cos(phi_IK)); // using the measured steering angle + } +#endif + // steering angle + phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); + if (open_loop) + { + phi_IK = phi; } + // wheel speed + Ws = v_bx / wheel_radius_; if (config_type_ == BICYCLE_CONFIG) { @@ -216,17 +249,20 @@ std::tuple, std::vector> SteeringOdometry::get_comma { std::vector traction_commands; std::vector steering_commands; - if (fabs(steer_pos_) < 1e-6) + // double-traction axle + if (is_close_to_zero(phi_IK)) { + // avoid division by zero traction_commands = {Ws, Ws}; } else { - const double turning_radius = wheelbase_ / std::tan(steer_pos_); + const double turning_radius = wheelbase_ / std::tan(phi_IK); const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; } + // simple steering steering_commands = {phi}; return std::make_tuple(traction_commands, steering_commands); } @@ -234,14 +270,16 @@ std::tuple, std::vector> SteeringOdometry::get_comma { std::vector traction_commands; std::vector steering_commands; - if (fabs(steer_pos_) < 1e-6) + if (is_close_to_zero(phi_IK)) { + // avoid division by zero traction_commands = {Ws, Ws}; + // shortcut, no steering steering_commands = {phi, phi}; } else { - const double turning_radius = wheelbase_ / std::tan(steer_pos_); + const double turning_radius = wheelbase_ / std::tan(phi_IK); const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; @@ -279,8 +317,8 @@ void SteeringOdometry::integrate_runge_kutta_2( const double theta_mid = heading_ + omega_bz * 0.5 * dt; // Use the intermediate values to update the state - x_ += v_bx * cos(theta_mid) * dt; - y_ += v_bx * sin(theta_mid) * dt; + x_ += v_bx * std::cos(theta_mid) * dt; + y_ += v_bx * std::sin(theta_mid) * dt; heading_ += omega_bz * dt; } @@ -289,7 +327,7 @@ void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, co const double delta_x_b = v_bx * dt; const double delta_theta = omega_bz * dt; - if (fabs(delta_theta) < 1e-6) + if (is_close_to_zero(delta_theta)) { /// Runge-Kutta 2nd Order (should solve problems when omega_bz is zero): integrate_runge_kutta_2(v_bx, omega_bz, dt); @@ -300,8 +338,8 @@ void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, co const double heading_old = heading_; const double R = delta_x_b / delta_theta; heading_ += delta_theta; - x_ += R * (sin(heading_) - sin(heading_old)); - y_ += -R * (cos(heading_) - cos(heading_old)); + x_ += R * (sin(heading_) - std::sin(heading_old)); + y_ += -R * (cos(heading_) - std::cos(heading_old)); } } diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 3e4adc59fe..d93e29eca5 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -28,7 +28,21 @@ TEST(TestSteeringOdometry, initialize) EXPECT_DOUBLE_EQ(odom.get_y(), 0.); } -TEST(TestSteeringOdometry, ackermann_fwd_kin_linear) +// ----------------- Ackermann ----------------- + +TEST(TestSteeringOdometry, ackermann_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} + +TEST(TestSteeringOdometry, ackermann_odometry_openloop_linear) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -39,7 +53,7 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_linear) EXPECT_DOUBLE_EQ(odom.get_y(), 0.); } -TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left) +TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_left) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -52,7 +66,7 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left) EXPECT_GT(odom.get_y(), 0); // pos y, ie. left } -TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right) +TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_right) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -64,13 +78,13 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right) EXPECT_LT(odom.get_y(), 0); // neg y ie. right } -TEST(TestSteeringOdometry, ackermann_back_kin_linear) +TEST(TestSteeringOdometry, ackermann_IK_linear) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); odom.update_open_loop(1., 0., 1.); - auto cmd = odom.get_commands(1., 0.); + auto cmd = odom.get_commands(1., 0., true); auto cmd0 = std::get<0>(cmd); // vel EXPECT_EQ(cmd0[0], cmd0[1]); // linear EXPECT_GT(cmd0[0], 0); @@ -79,13 +93,13 @@ TEST(TestSteeringOdometry, ackermann_back_kin_linear) EXPECT_EQ(cmd1[0], 0); } -TEST(TestSteeringOdometry, ackermann_back_kin_left) +TEST(TestSteeringOdometry, ackermann_IK_left) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); odom.update_from_position(0., 0.2, 1.); // assume already turn - auto cmd = odom.get_commands(1., 0.1); + auto cmd = odom.get_commands(1., 0.1, false); auto cmd0 = std::get<0>(cmd); // vel EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) EXPECT_GT(cmd0[0], 0); @@ -94,13 +108,13 @@ TEST(TestSteeringOdometry, ackermann_back_kin_left) EXPECT_GT(cmd1[0], 0); } -TEST(TestSteeringOdometry, ackermann_back_kin_right) +TEST(TestSteeringOdometry, ackermann_IK_right) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); odom.update_from_position(0., -0.2, 1.); // assume already turn - auto cmd = odom.get_commands(1., -0.1); + auto cmd = odom.get_commands(1., -0.1, false); auto cmd0 = std::get<0>(cmd); // vel EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left outer) EXPECT_GT(cmd0[0], 0); @@ -109,6 +123,47 @@ TEST(TestSteeringOdometry, ackermann_back_kin_right) EXPECT_LT(cmd1[0], 0); } +// ----------------- bicycle ----------------- + +TEST(TestSteeringOdometry, bicycle_IK_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_DOUBLE_EQ(cmd1[0], 0); // no steering +} + +TEST(TestSteeringOdometry, bicycle_IK_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(cmd1[0], 0); // right steering +} + +TEST(TestSteeringOdometry, bicycle_IK_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); // left steering +} + TEST(TestSteeringOdometry, bicycle_odometry) { steering_odometry::SteeringOdometry odom(1); @@ -121,25 +176,57 @@ TEST(TestSteeringOdometry, bicycle_odometry) EXPECT_NEAR(odom.get_heading(), .01, 1e-3); } -TEST(TestSteeringOdometry, tricycle_odometry) +// ----------------- tricycle ----------------- + +TEST(TestSteeringOdometry, tricycle_IK_linear) { steering_odometry::SteeringOdometry odom(1); - odom.set_wheel_params(1., 1., 1.); + odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); - ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); - EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); - EXPECT_NEAR(odom.get_angular(), .1, 1e-3); - EXPECT_NEAR(odom.get_x(), .1, 1e-3); - EXPECT_NEAR(odom.get_heading(), .01, 1e-3); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_EQ(cmd0[0], cmd0[1]); // linear + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_EQ(cmd1[0], 0); // no steering } -TEST(TestSteeringOdometry, ackermann_odometry) +TEST(TestSteeringOdometry, tricycle_IK_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(cmd1[0], 0); // left steering +} + +TEST(TestSteeringOdometry, tricycle_IK_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left outer) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); // right steering +} + +TEST(TestSteeringOdometry, tricycle_odometry) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 1., 1.); - odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); - ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1)); - EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3); EXPECT_NEAR(odom.get_angular(), .1, 1e-3); EXPECT_NEAR(odom.get_x(), .1, 1e-3); EXPECT_NEAR(odom.get_heading(), .01, 1e-3); diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index c555de53de..328f5e4d6a 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -201,6 +201,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -246,6 +247,7 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -258,6 +260,7 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu subscribe_and_get_messages(msg); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( From 07061f96f21fd4436a1f48b29e74beb21be8709a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 3 Jul 2024 19:56:47 +0200 Subject: [PATCH 14/97] [JTC] Process tolerances sent with action goal (#716) --- doc/migration/Jazzy.rst | 1 + doc/release_notes/Jazzy.rst | 14 + joint_trajectory_controller/CMakeLists.txt | 4 + joint_trajectory_controller/doc/userdoc.rst | 16 +- .../joint_trajectory_controller.hpp | 3 + .../tolerances.hpp | 219 ++++++++- .../src/joint_trajectory_controller.cpp | 55 ++- .../test/test_tolerances.cpp | 424 ++++++++++++++++++ .../test/test_trajectory_actions.cpp | 178 +++++++- .../test/test_trajectory_controller_utils.hpp | 41 +- 10 files changed, 921 insertions(+), 34 deletions(-) create mode 100644 joint_trajectory_controller/test/test_tolerances.cpp diff --git a/doc/migration/Jazzy.rst b/doc/migration/Jazzy.rst index 72e6a52e6d..2114436098 100644 --- a/doc/migration/Jazzy.rst +++ b/doc/migration/Jazzy.rst @@ -18,3 +18,4 @@ joint_trajectory_controller * Empty trajectory messages are discarded (`#902 `_). * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). Remove the ``angle_wraparound`` parameter from the configuration and set continuous joint type in the URDF of the respective joint. +* Tolerances sent with the action goal were not used before, but are now processed and used for the upcoming action. (`#716 `_). Adaptions to the action goal might be necessary. diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes/Jazzy.rst index 4c5db18f3e..4793fd957d 100644 --- a/doc/release_notes/Jazzy.rst +++ b/doc/release_notes/Jazzy.rst @@ -33,6 +33,20 @@ joint_trajectory_controller * Action field ``error_string`` is now filled with meaningful strings (`#887 `_). * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). ``angle_wraparound`` parameter was completely removed. +* Tolerances sent with the action goal are now processed and used for the action. (`#716 `_). For details, see the `JointTolerance message `_: + + .. code-block:: markdown + + The tolerances specify the amount the position, velocity, and + accelerations can vary from the setpoints. For example, in the case + of trajectory control, when the actual position varies beyond + (desired position + position tolerance), the trajectory goal may + abort. + + There are two special values for tolerances: + * 0 - The tolerance is unspecified and will remain at whatever the default is + * -1 - The tolerance is "erased". If there was a default, the joint will be + allowed to move without restriction. pid_controller ************************ diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 4b0ca82df8..46c245be9c 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -61,6 +61,10 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory joint_trajectory_controller) target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES) + ament_add_gmock(test_tolerances test/test_tolerances.cpp) + target_link_libraries(test_tolerances joint_trajectory_controller) + target_compile_definitions(test_tolerances PRIVATE _USE_MATH_DEFINES) + ament_add_gmock(test_trajectory_controller test/test_trajectory_controller.cpp) set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index af495ad14d..4dcb71a064 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -152,7 +152,21 @@ Actions [#f1]_ The primary way to send trajectories is through the action interface, and should be favored when execution monitoring is desired. -Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. +Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. For details, see the `JointTolerance message `_: + +.. code-block:: markdown + + The tolerances specify the amount the position, velocity, and + accelerations can vary from the setpoints. For example, in the case + of trajectory control, when the actual position varies beyond + (desired position + position tolerance), the trajectory goal may + abort. + + There are two special values for tolerances: + * 0 - The tolerance is unspecified and will remain at whatever the default is + * -1 - The tolerance is "erased". If there was a default, the joint will be + allowed to move without restriction. + When no tolerances are specified, the defaults given in the parameter interface are used (see :ref:`parameters`). If tolerances are violated during trajectory execution, the action goal is aborted, the client is notified, and the current position is held. 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 ae370df0a6..c3fbb58456 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 @@ -244,7 +244,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa size_t joint_names_size, const std::vector & vector_field, const std::string & string_for_vector_field, size_t i, bool allow_empty) const; + // the tolerances from the node parameter SegmentTolerances default_tolerances_; + // the tolerances used for the current goal + realtime_tools::RealtimeBuffer active_tolerances_; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void preempt_active_goal(); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index c46b1c297f..1998930182 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -30,12 +30,15 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ +#include +#include #include #include "control_msgs/action/follow_joint_trajectory.hpp" #include "joint_trajectory_controller_parameters.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/time.hpp" namespace joint_trajectory_controller { @@ -85,16 +88,19 @@ struct SegmentTolerances * goal: 0.01 * \endcode * + * \param jtc_logger The logger to use for output * \param params The ROS Parameters * \return Trajectory segment tolerances. */ -SegmentTolerances get_segment_tolerances(Params const & params) +SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Params & params) { auto const & constraints = params.constraints; auto const n_joints = params.joints.size(); SegmentTolerances tolerances; tolerances.goal_time_tolerance = constraints.goal_time; + static auto logger = jtc_logger.get_child("tolerance"); + RCLCPP_DEBUG(logger, "goal_time %f", constraints.goal_time); // State and goal state tolerances tolerances.state_tolerance.resize(n_joints); @@ -106,16 +112,221 @@ SegmentTolerances get_segment_tolerances(Params const & params) 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", (joint + ".trajectory").c_str(), tolerances.state_tolerance[i].position); + logger, "%s %f", (joint + ".trajectory.position").c_str(), + tolerances.state_tolerance[i].position); RCLCPP_DEBUG( - logger, "%s %f", (joint + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); + logger, "%s %f", (joint + ".goal.position").c_str(), + tolerances.goal_state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal.velocity").c_str(), + tolerances.goal_state_tolerance[i].velocity); } return tolerances; } +/** + * \brief Populate trajectory segment tolerances using data from an action goal. + * + * \param jtc_logger The logger to use for output + * \param default_tolerances The default tolerances to use if the action goal does not specify any. + * \param goal The new action goal + * \param joints The joints configured by ROS parameters + * \return Trajectory segment tolerances. + */ +SegmentTolerances get_segment_tolerances( + rclcpp::Logger & jtc_logger, const SegmentTolerances & default_tolerances, + const control_msgs::action::FollowJointTrajectory::Goal & goal, + const std::vector & joints) +{ + SegmentTolerances active_tolerances(default_tolerances); + + active_tolerances.goal_time_tolerance = rclcpp::Duration(goal.goal_time_tolerance).seconds(); + static auto logger = jtc_logger.get_child("tolerance"); + RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); + + // from + // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg + // There are two special values for tolerances: + // * 0 - The tolerance is unspecified and will remain at whatever the default is + // * -1 - The tolerance is "erased". + // If there was a default, the joint will be allowed to move without restriction. + constexpr double ERASE_VALUE = -1.0; + auto is_erase_value = [](double value) + { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; + + // State and goal state tolerances + for (auto joint_tol : goal.path_tolerance) + { + auto const joint = joint_tol.name; + // map joint names from goal to active_tolerances + auto it = std::find(joints.begin(), joints.end(), joint); + if (it == joints.end()) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.path_tolerance does not exist. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + auto i = std::distance(joints.cbegin(), it); + if (joint_tol.position > 0.0) + { + active_tolerances.state_tolerance[i].position = joint_tol.position; + } + else if (is_erase_value(joint_tol.position)) + { + active_tolerances.state_tolerance[i].position = 0.0; + } + else if (joint_tol.position < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.path_tolerance has a invalid position tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + if (joint_tol.velocity > 0.0) + { + active_tolerances.state_tolerance[i].velocity = joint_tol.velocity; + } + else if (is_erase_value(joint_tol.velocity)) + { + active_tolerances.state_tolerance[i].velocity = 0.0; + } + else if (joint_tol.velocity < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.path_tolerance has a invalid velocity tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + if (joint_tol.acceleration > 0.0) + { + active_tolerances.state_tolerance[i].acceleration = joint_tol.acceleration; + } + else if (is_erase_value(joint_tol.acceleration)) + { + active_tolerances.state_tolerance[i].acceleration = 0.0; + } + else if (joint_tol.acceleration < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.path_tolerance has a invalid acceleration tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.position").c_str(), + active_tolerances.state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.velocity").c_str(), + active_tolerances.state_tolerance[i].velocity); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.acceleration").c_str(), + active_tolerances.state_tolerance[i].acceleration); + } + for (auto goal_tol : goal.goal_tolerance) + { + auto const joint = goal_tol.name; + // map joint names from goal to active_tolerances + auto it = std::find(joints.begin(), joints.end(), joint); + if (it == joints.end()) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.goal_tolerance does not exist. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + auto i = std::distance(joints.cbegin(), it); + if (goal_tol.position > 0.0) + { + active_tolerances.goal_state_tolerance[i].position = goal_tol.position; + } + else if (is_erase_value(goal_tol.position)) + { + active_tolerances.goal_state_tolerance[i].position = 0.0; + } + else if (goal_tol.position < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.goal_tolerance has a invalid position tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + if (goal_tol.velocity > 0.0) + { + active_tolerances.goal_state_tolerance[i].velocity = goal_tol.velocity; + } + else if (is_erase_value(goal_tol.velocity)) + { + active_tolerances.goal_state_tolerance[i].velocity = 0.0; + } + else if (goal_tol.velocity < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.goal_tolerance has a invalid velocity tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + if (goal_tol.acceleration > 0.0) + { + active_tolerances.goal_state_tolerance[i].acceleration = goal_tol.acceleration; + } + else if (is_erase_value(goal_tol.acceleration)) + { + active_tolerances.goal_state_tolerance[i].acceleration = 0.0; + } + else if (goal_tol.acceleration < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.goal_tolerance has a invalid acceleration tolerance. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.position").c_str(), + active_tolerances.goal_state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.velocity").c_str(), + active_tolerances.goal_state_tolerance[i].velocity); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.acceleration").c_str(), + active_tolerances.goal_state_tolerance[i].acceleration); + } + + return active_tolerances; +} + /** * \param state_error State error to check. * \param joint_idx Joint index for the state error diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index b80d1b7908..31598bb813 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -126,11 +126,12 @@ controller_interface::return_type JointTrajectoryController::update( { return controller_interface::return_type::OK; } + auto logger = this->get_node()->get_logger(); // update dynamic parameters if (param_listener_->is_old(params_)) { params_ = param_listener_->get_params(); - default_tolerances_ = get_segment_tolerances(params_); + default_tolerances_ = get_segment_tolerances(logger, params_); // update the PID gains // variable use_closed_loop_pid_adapter_ is updated in on_configure only if (use_closed_loop_pid_adapter_) @@ -200,6 +201,7 @@ controller_interface::return_type JointTrajectoryController::update( bool outside_goal_tolerance = false; bool within_goal_time = true; const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end(); + auto active_tol = active_tolerances_.readFromRT(); // have we reached the end, are not holding position, and is a timeout configured? // Check independently of other tolerances @@ -207,7 +209,7 @@ controller_interface::return_type JointTrajectoryController::update( !before_last_point && *(rt_is_holding_.readFromRT()) == false && cmd_timeout_ > 0.0 && time_difference > cmd_timeout_) { - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout"); + RCLCPP_WARN(logger, "Aborted due to command timeout"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -224,8 +226,7 @@ controller_interface::return_type JointTrajectoryController::update( if ( (before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.state_tolerance[index], - true /* show_errors */)) + state_error_, index, active_tol->state_tolerance[index], true /* show_errors */)) { tolerance_violated_while_moving = true; } @@ -233,14 +234,14 @@ controller_interface::return_type JointTrajectoryController::update( if ( !before_last_point && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.goal_state_tolerance[index], - false /* show_errors */)) + state_error_, index, active_tol->goal_state_tolerance[index], false /* show_errors */)) { outside_goal_tolerance = true; - if (default_tolerances_.goal_time_tolerance != 0.0) + if (active_tol->goal_time_tolerance != 0.0) { - if (time_difference > default_tolerances_.goal_time_tolerance) + // if we exceed goal_time_tolerance set it to aborted + if (time_difference > active_tol->goal_time_tolerance) { within_goal_time = false; // print once, goal will be aborted afterwards @@ -320,7 +321,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); + RCLCPP_WARN(logger, "Aborted due to state tolerance violation"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -339,7 +340,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); + RCLCPP_INFO(logger, "Goal reached, success!"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); @@ -358,7 +359,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN(get_node()->get_logger(), error_string.c_str()); + RCLCPP_WARN(logger, error_string.c_str()); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -368,7 +369,7 @@ controller_interface::return_type JointTrajectoryController::update( else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) { // we need to ensure that there is no pending goal -> we get a race condition otherwise - RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); + RCLCPP_ERROR(logger, "Holding position due to state tolerance violation"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -376,7 +377,7 @@ controller_interface::return_type JointTrajectoryController::update( else if ( !before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) { - RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position..."); + RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position..."); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -620,11 +621,11 @@ void JointTrajectoryController::query_state_service( controller_interface::CallbackReturn JointTrajectoryController::on_configure( const rclcpp_lifecycle::State &) { - const auto logger = get_node()->get_logger(); + auto logger = get_node()->get_logger(); if (!param_listener_) { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + RCLCPP_ERROR(logger, "Error encountered during init"); return controller_interface::CallbackReturn::ERROR; } @@ -782,6 +783,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_interface_list(params_.state_interfaces).c_str()); // parse remaining parameters + default_tolerances_ = get_segment_tolerances(logger, params_); + active_tolerances_.initRT(default_tolerances_); const std::string interpolation_string = get_node()->get_parameter("interpolation_method").as_string(); interpolation_method_ = interpolation_methods::from_string(interpolation_string); @@ -873,6 +876,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( controller_interface::CallbackReturn JointTrajectoryController::on_activate( const rclcpp_lifecycle::State &) { + auto logger = get_node()->get_logger(); + // update the dynamic map parameters param_listener_->refresh_dynamic_parameters(); @@ -880,7 +885,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( params_ = param_listener_->get_params(); // parse remaining parameters - default_tolerances_ = get_segment_tolerances(params_); + default_tolerances_ = get_segment_tolerances(logger, params_); // order all joints in the storage for (const auto & interface : params_.command_interfaces) @@ -892,8 +897,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, - interface.c_str(), joint_command_interface_[index].size()); + logger, "Expected %zu '%s' command interfaces, got %zu.", dof_, interface.c_str(), + joint_command_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -906,8 +911,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( state_interfaces_, params_.joints, interface, joint_state_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_, - interface.c_str(), joint_state_interface_[index].size()); + logger, "Expected %zu '%s' state interfaces, got %zu.", dof_, interface.c_str(), + joint_state_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -949,9 +954,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { // deactivate timeout RCLCPP_WARN( - get_node()->get_logger(), - "Command timeout must be higher than goal_time tolerance (%f vs. %f)", params_.cmd_timeout, - default_tolerances_.goal_time_tolerance); + logger, "Command timeout must be higher than goal_time tolerance (%f vs. %f)", + params_.cmd_timeout, default_tolerances_.goal_time_tolerance); cmd_timeout_ = 0.0; } } @@ -1173,6 +1177,11 @@ void JointTrajectoryController::goal_accepted_callback( rt_goal->execute(); rt_active_goal_.writeFromNonRT(rt_goal); + // Update tolerances if specified in the goal + auto logger = this->get_node()->get_logger(); + active_tolerances_.writeFromNonRT(get_segment_tolerances( + logger, default_tolerances_, *(goal_handle->get_goal()), params_.joints)); + // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list goal_handle_timer_.reset(); diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp new file mode 100644 index 0000000000..66914b6da4 --- /dev/null +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -0,0 +1,424 @@ +// Copyright 2024 Austrian Institute of Technology +// +// 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 "gmock/gmock.h" +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +#include "joint_trajectory_controller/tolerances.hpp" +#include "test_trajectory_controller_utils.hpp" + +using joint_trajectory_controller::SegmentTolerances; +using trajectory_msgs::msg::JointTrajectoryPoint; + +std::vector joint_names_ = {"joint1", "joint2", "joint3"}; + +control_msgs::action::FollowJointTrajectory_Goal prepareGoalMsg( + const std::vector & points, double timeout, + const std::vector path_tolerance = + std::vector(), + const std::vector goal_tolerance = + std::vector()) +{ + control_msgs::action::FollowJointTrajectory_Goal goal_msg; + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.goal_tolerance = goal_tolerance; + goal_msg.path_tolerance = path_tolerance; + goal_msg.trajectory.joint_names = joint_names_; + goal_msg.trajectory.points = points; + + return goal_msg; +} +class TestTolerancesFixture : public ::testing::Test +{ +protected: + SegmentTolerances default_tolerances; + joint_trajectory_controller::Params params; + std::vector joint_names_; + rclcpp::Logger logger = rclcpp::get_logger("TestTolerancesFixture"); + + void SetUp() override + { + // Initialize joint_names_ with some test data + joint_names_ = {"joint1", "joint2", "joint3"}; + + // Initialize default_tolerances and params with common setup for all tests + // TODO(anyone) fill params and use + // SegmentTolerances get_segment_tolerances(Params const & params) instead + default_tolerances.goal_time_tolerance = default_goal_time; + default_tolerances.state_tolerance.resize(joint_names_.size()); + default_tolerances.goal_state_tolerance.resize(joint_names_.size()); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + default_tolerances.state_tolerance.at(i).position = 0.1; + default_tolerances.goal_state_tolerance.at(i).position = 0.1; + default_tolerances.goal_state_tolerance.at(i).velocity = stopped_velocity_tolerance; + } + params.joints = joint_names_; + } + + void TearDown() override + { + // Cleanup code if necessary + } +}; + +TEST_F(TestTolerancesFixture, test_get_segment_tolerances) +{ + // send goal with nonzero tolerances, are they accepted? + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = 0.2; + tolerance.velocity = 0.3; + tolerance.acceleration = 0.4; + path_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + std::vector goal_tolerance; + // add different tolerances in jumbled order + tolerance.name = "joint2"; + tolerance.position = 1.2; + tolerance.velocity = 2.2; + tolerance.acceleration = 3.2; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + tolerance.position = 1.3; + tolerance.velocity = 2.3; + tolerance.acceleration = 3.3; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint1"; + tolerance.position = 1.1; + tolerance.velocity = 2.1; + tolerance.acceleration = 3.1; + goal_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 2.0, path_tolerance, goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 2.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.4); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 1.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 2.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 3.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 1.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 2.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 3.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 1.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 2.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 3.3); +} + +// send goal with deactivated tolerances (-1) +TEST_F(TestTolerancesFixture, test_deactivate_tolerances) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = -1.0; + tolerance.velocity = -1.0; + tolerance.acceleration = -1.0; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 0.0, path_tolerance, goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 0.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); +} + +// send goal with invalid tolerances, are the default ones used? +TEST_F(TestTolerancesFixture, test_invalid_tolerances) +{ + { + SCOPED_TRACE("negative path position tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = -123.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative path velocity tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = -123.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative path acceleration tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = -123.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal position tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = -123.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal velocity tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = -123.0; + tolerance.acceleration = 0.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal acceleration tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = -123.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } +} +TEST_F(TestTolerancesFixture, test_invalid_joints_path_tolerance) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint123"; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); +} +TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint123"; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); +} diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 332a30c53a..fb749ac250 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -31,7 +31,6 @@ #include "controller_interface/controller_interface.hpp" #include "gtest/gtest.h" #include "hardware_interface/resource_manager.hpp" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" @@ -44,10 +43,12 @@ #include "rclcpp_action/client_goal_handle.hpp" #include "rclcpp_action/create_client.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "test_trajectory_controller_utils.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "test_trajectory_controller_utils.hpp" + using std::placeholders::_1; using std::placeholders::_2; using test_trajectory_controllers::TestableJointTrajectoryController; @@ -152,10 +153,16 @@ class TestTrajectoryActions : public TrajectoryControllerTest using GoalOptions = rclcpp_action::Client::SendGoalOptions; std::shared_future sendActionGoal( - const std::vector & points, double timeout, const GoalOptions & opt) + const std::vector & points, double timeout, const GoalOptions & opt, + const std::vector path_tolerance = + std::vector(), + const std::vector goal_tolerance = + std::vector()) { control_msgs::action::FollowJointTrajectory_Goal goal_msg; goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.goal_tolerance = goal_tolerance; + goal_msg.path_tolerance = path_tolerance; goal_msg.trajectory.joint_names = joint_names_; goal_msg.trajectory.points = points; @@ -488,6 +495,165 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) expectCommandPoint(points_positions.at(1)); } +/** + * No need for parameterized tests + */ +TEST_F(TestTrajectoryActions, test_tolerances_via_actions) +{ + // set tolerance parameters + std::vector params = { + rclcpp::Parameter("constraints.joint1.goal", 0.1), + rclcpp::Parameter("constraints.joint2.goal", 0.1), + rclcpp::Parameter("constraints.joint3.goal", 0.1), + rclcpp::Parameter("constraints.goal_time", default_goal_time), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.1), + rclcpp::Parameter("constraints.joint1.trajectory", 0.1), + rclcpp::Parameter("constraints.joint2.trajectory", 0.1), + rclcpp::Parameter("constraints.joint3.trajectory", 0.1)}; + + SetUpExecutor(params); + + { + SCOPED_TRACE("Check default values"); + SetUpControllerHardware(); + std::shared_future gh_future; + // send goal + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); + expectDefaultTolerances(active_tolerances); + } + + // send goal with nonzero tolerances, are they accepted? + { + SetUpControllerHardware(); + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = 0.2; + tolerance.velocity = 0.3; + tolerance.acceleration = 0.4; + path_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + std::vector goal_tolerance; + // add different tolerances in jumbled order + tolerance.name = "joint2"; + tolerance.position = 1.2; + tolerance.velocity = 2.2; + tolerance.acceleration = 3.2; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + tolerance.position = 1.3; + tolerance.velocity = 2.3; + tolerance.acceleration = 3.3; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint1"; + tolerance.position = 1.1; + tolerance.velocity = 2.1; + tolerance.acceleration = 3.1; + goal_tolerance.push_back(tolerance); + + gh_future = sendActionGoal(points, 2.0, goal_options_, path_tolerance, goal_tolerance); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 2.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.4); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 1.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 2.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 3.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 1.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 2.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 3.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 1.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 2.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 3.3); + } + + // send goal without tolerances again, are the default ones used? + { + SetUpControllerHardware(); + + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); + expectDefaultTolerances(active_tolerances); + } +} + TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) { // set joint tolerance parameters @@ -682,7 +848,8 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true) { std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; SetUpExecutor(params); SetUpControllerHardware(); @@ -732,7 +899,8 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_tr TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_false) { std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", false)}; + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", false), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; SetUpExecutor(params); SetUpControllerHardware(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index f049bc65a9..693e19e67a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -25,6 +25,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "joint_trajectory_controller/tolerances.hpp" namespace { @@ -38,11 +39,44 @@ const std::vector INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0}; +const double default_goal_time = 0.1; +const double stopped_velocity_tolerance = 0.1; + +[[maybe_unused]] void expectDefaultTolerances( + joint_trajectory_controller::SegmentTolerances active_tolerances) +{ + // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(0).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(1).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(2).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); +} + bool is_same_sign_or_zero(double val1, double val2) { return val1 * val2 > 0.0 || (val1 == 0.0 && val2 == 0.0); } - } // namespace namespace test_trajectory_controllers @@ -138,6 +172,11 @@ class TestableJointTrajectoryController bool is_open_loop() const { return params_.open_loop_control; } + joint_trajectory_controller::SegmentTolerances get_active_tolerances() + { + return *(active_tolerances_.readFromRT()); + } + std::vector get_pids() const { return pids_; } joint_trajectory_controller::SegmentTolerances get_tolerances() const From 991ef48b79a12974a1b37763e818842906e3553b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 8 Jul 2024 23:08:20 +0200 Subject: [PATCH 15/97] added changes corresponding to the logger and clock propagation in ResourceManager (#1184) --- .../test/test_load_ackermann_steering_controller.cpp | 4 +--- .../test/test_load_admittance_controller.cpp | 4 +--- .../test/test_load_bicycle_steering_controller.cpp | 4 +--- .../test/test_load_diff_drive_controller.cpp | 3 +-- .../test/test_load_joint_group_effort_controller.cpp | 4 +--- .../test/test_load_force_torque_sensor_broadcaster.cpp | 4 +--- .../test/test_load_forward_command_controller.cpp | 4 +--- .../test_load_multi_interface_forward_command_controller.cpp | 4 +--- .../test/test_load_gripper_action_controllers.cpp | 4 +--- .../test/test_load_imu_sensor_broadcaster.cpp | 4 +--- .../test/test_load_joint_state_broadcaster.cpp | 4 +--- .../test/test_load_joint_trajectory_controller.cpp | 4 +--- pid_controller/test/test_load_pid_controller.cpp | 5 ++--- .../test/test_load_joint_group_position_controller.cpp | 4 +--- .../test/test_load_range_sensor_broadcaster.cpp | 4 +--- tricycle_controller/test/test_load_tricycle_controller.cpp | 4 +--- .../test/test_load_tricycle_steering_controller.cpp | 4 +--- .../test/test_load_joint_group_velocity_controller.cpp | 4 +--- 18 files changed, 19 insertions(+), 53 deletions(-) diff --git a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp index 0a8cd7b80c..101ecce8ff 100644 --- a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -28,9 +28,7 @@ TEST(TestLoadAckermannSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp index 23be1f23f5..69808f3505 100644 --- a/admittance_controller/test/test_load_admittance_controller.cpp +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -29,9 +29,7 @@ TEST(TestLoadAdmittanceController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_EQ( cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController"), diff --git a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp index 955feb33c5..f3828660a5 100644 --- a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp @@ -28,9 +28,7 @@ TEST(TestLoadBicycleSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 4c9d2f984f..0c65532c38 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -28,8 +28,7 @@ TEST(TestLoadDiffDriveController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique(ros2_control_test_assets::diffbot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::diffbot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp index 52f1f9934a..4008ac8534 100644 --- a/effort_controllers/test/test_load_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_load_joint_group_effort_controller.cpp @@ -30,9 +30,7 @@ TEST(TestLoadJointGroupVelocityController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp index 0c269d6a31..2e87505003 100644 --- a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp @@ -32,9 +32,7 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/forward_command_controller/test/test_load_forward_command_controller.cpp b/forward_command_controller/test/test_load_forward_command_controller.cpp index b493e52b2a..c192f1eb5f 100644 --- a/forward_command_controller/test/test_load_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_forward_command_controller.cpp @@ -30,9 +30,7 @@ TEST(TestLoadForwardCommandController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp index 41a9b74698..52b63bdae8 100644 --- a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp @@ -31,9 +31,7 @@ TEST(TestLoadMultiInterfaceForwardController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/gripper_controllers/test/test_load_gripper_action_controllers.cpp b/gripper_controllers/test/test_load_gripper_action_controllers.cpp index 0ef5f0bcb2..5641e1b4b3 100644 --- a/gripper_controllers/test/test_load_gripper_action_controllers.cpp +++ b/gripper_controllers/test/test_load_gripper_action_controllers.cpp @@ -30,9 +30,7 @@ TEST(TestLoadGripperActionControllers, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp index f4e6105ed6..a477a731ff 100644 --- a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp @@ -32,9 +32,7 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp index 5efb587805..266e33c1c8 100644 --- a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp @@ -30,9 +30,7 @@ TEST(TestLoadJointStateBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( 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 eb1a3691e6..b3635efaca 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -31,9 +31,7 @@ TEST(TestLoadJointStateController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/pid_controller/test/test_load_pid_controller.cpp b/pid_controller/test/test_load_pid_controller.cpp index 3a75f6e170..ed645b872c 100644 --- a/pid_controller/test/test_load_pid_controller.cpp +++ b/pid_controller/test/test_load_pid_controller.cpp @@ -33,9 +33,8 @@ TEST(TestLoadPidController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NO_THROW(cm.load_controller("test_pid_controller", "pid_controller/PidController")); diff --git a/position_controllers/test/test_load_joint_group_position_controller.cpp b/position_controllers/test/test_load_joint_group_position_controller.cpp index bc27b5e629..950773351c 100644 --- a/position_controllers/test/test_load_joint_group_position_controller.cpp +++ b/position_controllers/test/test_load_joint_group_position_controller.cpp @@ -30,9 +30,7 @@ TEST(TestLoadJointGroupPositionController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp index 5c400bef91..1d8a55b932 100644 --- a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp @@ -32,9 +32,7 @@ TEST(TestLoadRangeSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index bd54459780..bb4194909c 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -32,9 +32,7 @@ TEST(TestLoadTricycleController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController"), diff --git a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp index 0d04afdf38..4fa3698409 100644 --- a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -28,9 +28,7 @@ TEST(TestLoadTricycleSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp index e426349f96..7070594639 100644 --- a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp @@ -30,9 +30,7 @@ TEST(TestLoadJointGroupVelocityController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( From 776c432dbc9a8db11119823803daad9525c63846 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Tue, 9 Jul 2024 09:08:28 +0200 Subject: [PATCH 16/97] [JTC] Make goal_time_tolerance overwrite default value only if explicitly set (#1192) --- .../tolerances.hpp | 186 +++++++----------- .../test/test_tolerances.cpp | 35 +++- .../test/test_trajectory_controller_utils.hpp | 1 + 3 files changed, 99 insertions(+), 123 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 1998930182..c4404920df 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -31,6 +31,7 @@ #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ #include +#include #include #include @@ -126,6 +127,33 @@ SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Para return tolerances; } +double resolve_tolerance_source(const double default_value, const double goal_value) +{ + // from + // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg + // There are two special values for tolerances: + // * 0 - The tolerance is unspecified and will remain at whatever the default is + // * -1 - The tolerance is "erased". + // If there was a default, the joint will be allowed to move without restriction. + constexpr double ERASE_VALUE = -1.0; + auto is_erase_value = [](double value) + { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; + + if (goal_value > 0.0) + { + return goal_value; + } + else if (is_erase_value(goal_value)) + { + return 0.0; + } + else if (goal_value < 0.0) + { + throw std::runtime_error("Illegal tolerance value."); + } + return default_value; +} + /** * \brief Populate trajectory segment tolerances using data from an action goal. * @@ -141,20 +169,22 @@ SegmentTolerances get_segment_tolerances( const std::vector & joints) { SegmentTolerances active_tolerances(default_tolerances); - - active_tolerances.goal_time_tolerance = rclcpp::Duration(goal.goal_time_tolerance).seconds(); static auto logger = jtc_logger.get_child("tolerance"); - RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); - // from - // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg - // There are two special values for tolerances: - // * 0 - The tolerance is unspecified and will remain at whatever the default is - // * -1 - The tolerance is "erased". - // If there was a default, the joint will be allowed to move without restriction. - constexpr double ERASE_VALUE = -1.0; - auto is_erase_value = [](double value) - { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; + try + { + active_tolerances.goal_time_tolerance = resolve_tolerance_source( + default_tolerances.goal_time_tolerance, rclcpp::Duration(goal.goal_time_tolerance).seconds()); + } + catch (const std::runtime_error & e) + { + RCLCPP_ERROR_STREAM( + logger, "Specified illegal goal_time_tolerance: " + << rclcpp::Duration(goal.goal_time_tolerance).seconds() + << ". Using default tolerances"); + return default_tolerances; + } + RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); // State and goal state tolerances for (auto joint_tol : goal.path_tolerance) @@ -173,60 +203,24 @@ SegmentTolerances get_segment_tolerances( return default_tolerances; } auto i = std::distance(joints.cbegin(), it); - if (joint_tol.position > 0.0) - { - active_tolerances.state_tolerance[i].position = joint_tol.position; - } - else if (is_erase_value(joint_tol.position)) + std::string interface = ""; + try { - active_tolerances.state_tolerance[i].position = 0.0; + interface = "position"; + active_tolerances.state_tolerance[i].position = resolve_tolerance_source( + default_tolerances.state_tolerance[i].position, joint_tol.position); + interface = "velocity"; + active_tolerances.state_tolerance[i].velocity = resolve_tolerance_source( + default_tolerances.state_tolerance[i].velocity, joint_tol.velocity); + interface = "acceleration"; + active_tolerances.state_tolerance[i].acceleration = resolve_tolerance_source( + default_tolerances.state_tolerance[i].acceleration, joint_tol.acceleration); } - else if (joint_tol.position < 0.0) + catch (const std::runtime_error & e) { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.path_tolerance has a invalid position tolerance. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - - if (joint_tol.velocity > 0.0) - { - active_tolerances.state_tolerance[i].velocity = joint_tol.velocity; - } - else if (is_erase_value(joint_tol.velocity)) - { - active_tolerances.state_tolerance[i].velocity = 0.0; - } - else if (joint_tol.velocity < 0.0) - { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.path_tolerance has a invalid velocity tolerance. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - - if (joint_tol.acceleration > 0.0) - { - active_tolerances.state_tolerance[i].acceleration = joint_tol.acceleration; - } - else if (is_erase_value(joint_tol.acceleration)) - { - active_tolerances.state_tolerance[i].acceleration = 0.0; - } - else if (joint_tol.acceleration < 0.0) - { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.path_tolerance has a invalid acceleration tolerance. " - "Using default tolerances.") - .c_str()); + RCLCPP_ERROR_STREAM( + logger, "joint '" << joint << "' specified in goal.path_tolerance has a invalid " + << interface << " tolerance. Using default tolerances."); return default_tolerances; } @@ -256,60 +250,24 @@ SegmentTolerances get_segment_tolerances( return default_tolerances; } auto i = std::distance(joints.cbegin(), it); - if (goal_tol.position > 0.0) - { - active_tolerances.goal_state_tolerance[i].position = goal_tol.position; - } - else if (is_erase_value(goal_tol.position)) - { - active_tolerances.goal_state_tolerance[i].position = 0.0; - } - else if (goal_tol.position < 0.0) - { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.goal_tolerance has a invalid position tolerance. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - - if (goal_tol.velocity > 0.0) - { - active_tolerances.goal_state_tolerance[i].velocity = goal_tol.velocity; - } - else if (is_erase_value(goal_tol.velocity)) + std::string interface = ""; + try { - active_tolerances.goal_state_tolerance[i].velocity = 0.0; + interface = "position"; + active_tolerances.goal_state_tolerance[i].position = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].position, goal_tol.position); + interface = "velocity"; + active_tolerances.goal_state_tolerance[i].velocity = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].velocity, goal_tol.velocity); + interface = "acceleration"; + active_tolerances.goal_state_tolerance[i].acceleration = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].acceleration, goal_tol.acceleration); } - else if (goal_tol.velocity < 0.0) + catch (const std::runtime_error & e) { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.goal_tolerance has a invalid velocity tolerance. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - - if (goal_tol.acceleration > 0.0) - { - active_tolerances.goal_state_tolerance[i].acceleration = goal_tol.acceleration; - } - else if (is_erase_value(goal_tol.acceleration)) - { - active_tolerances.goal_state_tolerance[i].acceleration = 0.0; - } - else if (goal_tol.acceleration < 0.0) - { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.goal_tolerance has a invalid acceleration tolerance. " - "Using default tolerances.") - .c_str()); + RCLCPP_ERROR_STREAM( + logger, "joint '" << joint << "' specified in goal.goal_tolerance has a invalid " + << interface << " tolerance. Using default tolerances."); return default_tolerances; } diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp index 66914b6da4..af5036b869 100644 --- a/joint_trajectory_controller/test/test_tolerances.cpp +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -183,7 +183,7 @@ TEST_F(TestTolerancesFixture, test_deactivate_tolerances) path_tolerance.push_back(tolerance); goal_tolerance.push_back(tolerance); - auto goal_msg = prepareGoalMsg(points, 0.0, path_tolerance, goal_tolerance); + auto goal_msg = prepareGoalMsg(points, -1.0, path_tolerance, goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); @@ -215,6 +215,31 @@ TEST_F(TestTolerancesFixture, test_deactivate_tolerances) // send goal with invalid tolerances, are the default ones used? TEST_F(TestTolerancesFixture, test_invalid_tolerances) { + { + SCOPED_TRACE("negative goal_time_tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, -123.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } { SCOPED_TRACE("negative path position tolerance"); std::vector points; @@ -238,7 +263,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -264,7 +288,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -290,7 +313,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -317,7 +339,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -344,7 +365,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -371,7 +391,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } } @@ -395,7 +414,6 @@ TEST_F(TestTolerancesFixture, test_invalid_joints_path_tolerance) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) @@ -419,6 +437,5 @@ TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 693e19e67a..bbe12251a1 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -45,6 +45,7 @@ const double stopped_velocity_tolerance = 0.1; [[maybe_unused]] void expectDefaultTolerances( joint_trajectory_controller::SegmentTolerances active_tolerances) { + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); From a0d844f053d22b19045f51c38a2fa09d98148c89 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 9 Jul 2024 08:16:14 +0100 Subject: [PATCH 17/97] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 6 ++++++ admittance_controller/CHANGELOG.rst | 5 +++++ bicycle_steering_controller/CHANGELOG.rst | 6 ++++++ diff_drive_controller/CHANGELOG.rst | 5 +++++ 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 | 7 +++++++ pid_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 5 +++++ range_sensor_broadcaster/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 5 +++++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 6 ++++++ velocity_controllers/CHANGELOG.rst | 5 +++++ 21 files changed, 104 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index e27cac2205..84ac53044c 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Fix steering controllers library kinematics (`#1150 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index a3faf30ab1..e75a71a8af 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index a8956560b5..b1e1bf832f 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Fix steering controllers library kinematics (`#1150 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 5b9275359d..c402e58344 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 9b1240263b..6f7c2771cb 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 8f978cfd08..a7392ad187 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 +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 75926feff6..d37dd5f30d 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index adf9f8e208..acd4c2534c 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 6649777076..46ae9e4d27 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index e4f73e5cc6..f20ca59557 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 2c044b28b0..23838b7e3f 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Make goal_time_tolerance overwrite default value only if explicitly set (`#1192 `_) +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* [JTC] Process tolerances sent with action goal (`#716 `_) +* Contributors: Christoph Fröhlich, Felix Exner (fexner), Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- * Remove manual angle-wraparound parameter (`#1152 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 101d507f56..d5d140339b 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 0582a40895..d5f8b52cbb 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 7c671ae3d3..6be8a70b9d 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 1db9ae1c05..39c00e3c85 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.10.0 (2024-07-01) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index c483ee7c2f..d529a740b3 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 +----------- + 4.10.0 (2024-07-01) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index bd2b00a1fe..2ffbb95666 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 +----------- + 4.10.0 (2024-07-01) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index ee21fed403..7ea9c5d893 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix steering controllers library kinematics (`#1150 `_) +* Contributors: Christoph Fröhlich + 4.10.0 (2024-07-01) ------------------- * [STEERING] Add missing `tan` call for ackermann (`#1117 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 4034036e08..feb9b105cb 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- * Remove unstamped twist subscribers + parameters (`#1151 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index c80b90ff0f..a34e991d2a 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Fix steering controllers library kinematics (`#1150 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- * [Steering controllers library] Reference interfaces are body twist (`#1168 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index c4d146a21d..e749d6f8a0 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + 4.10.0 (2024-07-01) ------------------- From 18ce11a42d1549678b418794d283ea80914c4a6b Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 9 Jul 2024 08:16:31 +0100 Subject: [PATCH 18/97] 4.11.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_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 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/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 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 44 files changed, 65 insertions(+), 65 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 84ac53044c..21840061d4 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Fix steering controllers library kinematics (`#1150 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 77555c1f01..dfa733515f 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.10.0 + 4.11.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index e75a71a8af..4ae5ea39f8 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 9abdb5cf82..d6e13b0634 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.10.0 + 4.11.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index b1e1bf832f..831aa5a2b3 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Fix steering controllers library kinematics (`#1150 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index b49b9f1a23..7afcf53706 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.10.0 + 4.11.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index c402e58344..00f777df67 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 9cfdee9a29..d7fa06b762 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.10.0 + 4.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 6f7c2771cb..70cfaad021 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index faf31ada51..a7744927a1 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.10.0 + 4.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 a7392ad187..eac8c9959a 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 ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index ae0c89dcf2..90fcb3a684 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.10.0 + 4.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 d37dd5f30d..17196fed65 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 1e24ca0201..f4c02d8de7 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.10.0 + 4.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index acd4c2534c..968a1088c2 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 5a9ff6ed87..72d2ec48f2 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.10.0 + 4.11.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 46ae9e4d27..5bacde755e 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 383c90d89a..5f0e981605 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.10.0 + 4.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 f20ca59557..cc01373289 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9ca30f4e52..77bbe2ca00 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.10.0 + 4.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 23838b7e3f..047d232155 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * [JTC] Make goal_time_tolerance overwrite default value only if explicitly set (`#1192 `_) * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * [JTC] Process tolerances sent with action goal (`#716 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 733059736a..d32d4e3daf 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.10.0 + 4.11.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index d5d140339b..8e45f042f0 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 3bd87c1875..2a44c9ad1b 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.10.0 + 4.11.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index d5f8b52cbb..c9e2c749b5 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 013b60c790..33f0f0c15e 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.10.0 + 4.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 6be8a70b9d..31458c07d6 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index da74b2d62f..e4525f53fd 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.10.0 + 4.11.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 39c00e3c85..a54d39b85d 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- 4.10.0 (2024-07-01) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 14f686cce4..24e4429ade 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.10.0 + 4.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 d529a740b3..78e18dffaa 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 ------------ +4.11.0 (2024-07-09) +------------------- 4.10.0 (2024-07-01) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 2c26e5cfe0..87af6e6695 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.10.0 + 4.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 bb9ff7b303..91db299739 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.10.0", + version="4.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 2ffbb95666..20450f3ca3 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 ------------ +4.11.0 (2024-07-09) +------------------- 4.10.0 (2024-07-01) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 4d42c2d00d..85c44c58fe 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 - 4.10.0 + 4.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 b966635801..2588a47cc8 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.10.0", + version="4.11.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 7ea9c5d893..d4f62b806c 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * Fix steering controllers library kinematics (`#1150 `_) * Contributors: Christoph Fröhlich diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 5b55f7f9de..7d172d45a8 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.10.0 + 4.11.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index feb9b105cb..fdde4481e2 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index cdb62117fd..20310e6b9a 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.10.0 + 4.11.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index a34e991d2a..6f4ad5d8c1 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Fix steering controllers library kinematics (`#1150 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index f2408123df..4aeb8a086f 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.10.0 + 4.11.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index e749d6f8a0..614e023984 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.11.0 (2024-07-09) +------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) * Contributors: Sai Kishor Kothakota diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 75c05fab94..d128f2b45b 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.10.0 + 4.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 91ebb73b23872f60e09a0d3826b57d0f8b3bf5b7 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Thu, 11 Jul 2024 07:38:47 -0700 Subject: [PATCH 19/97] Add parallel_gripper_controller, configure gripper speed and effort with hardware interface (#1002) --- doc/controllers_index.rst | 1 + .../gripper_action_controller_impl.hpp | 5 + parallel_gripper_controller/CMakeLists.txt | 84 ++++ parallel_gripper_controller/doc/userdoc.rst | 19 + .../parallel_gripper_action_controller.hpp | 174 ++++++++ ...arallel_gripper_action_controller_impl.hpp | 396 ++++++++++++++++++ .../visibility_control.hpp | 56 +++ parallel_gripper_controller/package.xml | 35 ++ .../ros_control_plugins.xml | 10 + .../gripper_action_controller_parameters.yaml | 69 +++ .../parallel_gripper_action_controller.cpp | 25 ++ ...oad_parallel_gripper_action_controller.cpp | 45 ++ .../test/test_parallel_gripper_controller.cpp | 169 ++++++++ .../test/test_parallel_gripper_controller.hpp | 66 +++ 14 files changed, 1154 insertions(+) create mode 100644 parallel_gripper_controller/CMakeLists.txt create mode 100644 parallel_gripper_controller/doc/userdoc.rst create mode 100644 parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp create mode 100644 parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp create mode 100644 parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp create mode 100644 parallel_gripper_controller/package.xml create mode 100644 parallel_gripper_controller/ros_control_plugins.xml create mode 100644 parallel_gripper_controller/src/gripper_action_controller_parameters.yaml create mode 100644 parallel_gripper_controller/src/parallel_gripper_action_controller.cpp create mode 100644 parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp create mode 100644 parallel_gripper_controller/test/test_parallel_gripper_controller.cpp create mode 100644 parallel_gripper_controller/test/test_parallel_gripper_controller.hpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 2cb55150ce..78abe5d21f 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -52,6 +52,7 @@ The controllers are using `common hardware interface definitions`_, and may use Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Gripper Controller <../gripper_controllers/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> + Parallel Gripper Controller <../parallel_gripper_controller/doc/userdoc.rst> PID Controller <../pid_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> 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 77598591ae..2c115091af 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -40,6 +40,11 @@ void GripperActionController::preempt_active_goal() template controller_interface::CallbackReturn GripperActionController::on_init() { + RCLCPP_WARN( + get_node()->get_logger(), + "[Deprecated]: the `position_controllers/GripperActionController` and " + "`effort_controllers::GripperActionController` controllers are replaced by " + "'parallel_gripper_controllers/GripperActionController' controller"); try { param_listener_ = std::make_shared(get_node()); diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt new file mode 100644 index 0000000000..56aa623a6f --- /dev/null +++ b/parallel_gripper_controller/CMakeLists.txt @@ -0,0 +1,84 @@ +cmake_minimum_required(VERSION 3.16) +project(parallel_gripper_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_action + realtime_tools +) + +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(parallel_gripper_action_controller_parameters + src/gripper_action_controller_parameters.yaml +) + +add_library(parallel_gripper_action_controller SHARED + src/parallel_gripper_action_controller.cpp +) +target_compile_features(parallel_gripper_action_controller PUBLIC cxx_std_17) +target_include_directories(parallel_gripper_action_controller PUBLIC + $ + $ +) +target_link_libraries(parallel_gripper_action_controller PUBLIC + parallel_gripper_action_controller_parameters +) +ament_target_dependencies(parallel_gripper_action_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) + +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_parallel_gripper_action_controllers + test/test_load_parallel_gripper_action_controller.cpp + ) + ament_target_dependencies(test_load_parallel_gripper_action_controllers + controller_manager + ros2_control_test_assets + ) + + ament_add_gmock(test_parallel_gripper_controller + test/test_parallel_gripper_controller.cpp + ) + target_link_libraries(test_parallel_gripper_controller + parallel_gripper_action_controller + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/parallel_gripper_action_controller +) +install( + TARGETS + parallel_gripper_action_controller + parallel_gripper_action_controller_parameters + EXPORT export_parallel_gripper_action_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + +ament_export_targets(export_parallel_gripper_action_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/parallel_gripper_controller/doc/userdoc.rst b/parallel_gripper_controller/doc/userdoc.rst new file mode 100644 index 0000000000..5c04eb5f90 --- /dev/null +++ b/parallel_gripper_controller/doc/userdoc.rst @@ -0,0 +1,19 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/parallel_gripper_controller/doc/userdoc.rst + +.. _parallel_gripper_controller_userdoc: + +Parallel Gripper Action Controller +----------------------------------- + +Controller for executing a ``ParallelGripperCommand`` action for simple parallel grippers. +This controller supports grippers that offer position only control as well as grippers that allow configuring the velocity and effort. +By default, the controller will only claim the ``{joint}/position`` interface for control. +The velocity and effort interfaces can be optionally claimed by setting the ``max_velocity_interface`` and ``max_effort_interface`` parameter, respectively. +By default, the controller will try to claim position and velocity state interfaces. +The claimed state interfaces can be configured by setting the ``state_interfaces`` parameter. + +Parameters +^^^^^^^^^^^ +This controller uses the `generate_parameter_library `_ to handle its parameters. + +.. generate_parameter_library_details:: ../src/gripper_action_controller_parameters.yaml diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp new file mode 100644 index 0000000000..d303990b89 --- /dev/null +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -0,0 +1,174 @@ +// Copyright 2014, SRI International +// +// 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 Sachin Chitta, Adolfo Rodriguez Tsouroukdissian + +#ifndef PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ + +// C++ standard +#include +#include +#include +#include + +// ROS +#include "rclcpp/rclcpp.hpp" + +// ROS messages +#include "control_msgs/action/parallel_gripper_command.hpp" + +// rclcpp_action +#include "rclcpp_action/create_server.hpp" + +// ros_controls +#include "controller_interface/controller_interface.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "parallel_gripper_controller/visibility_control.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_server_goal_handle.h" + +// Project +#include "parallel_gripper_action_controller_parameters.hpp" + +namespace parallel_gripper_action_controller +{ +/** + * \brief Controller for executing a `ParallelGripperCommand` action. + * + * \tparam HardwareInterface Controller hardware interface. Currently \p + * hardware_interface::HW_IF_POSITION and \p + * hardware_interface::HW_IF_EFFORT are supported out-of-the-box. + */ + +class GripperActionController : public controller_interface::ControllerInterface +{ +public: + /** + * \brief Store position and max effort in struct to allow easier realtime + * buffer usage + */ + struct Commands + { + double position_cmd_; // Commanded position + double max_velocity_; // Desired max gripper velocity + double max_effort_; // Desired max allowed effort + }; + + GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController(); + + /** + * @brief command_interface_configuration This controller requires the + * position command interfaces for the controlled joints + */ + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + /** + * @brief command_interface_configuration This controller requires the + * position and velocity state interfaces for the controlled joints + */ + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + realtime_tools::RealtimeBuffer command_; + // pre-allocated memory that is re-used to set the realtime buffer + Commands command_struct_, command_struct_rt_; + +protected: + using GripperCommandAction = control_msgs::action::ParallelGripperCommand; + using ActionServer = rclcpp_action::Server; + using ActionServerPtr = ActionServer::SharedPtr; + using GoalHandle = rclcpp_action::ServerGoalHandle; + using RealtimeGoalHandle = + realtime_tools::RealtimeServerGoalHandle; + using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + + bool update_hold_position_; + + bool verbose_ = false; ///< Hard coded verbose flag to help in debugging + std::string name_; ///< Controller name. + std::optional> + joint_command_interface_; + std::optional> + effort_interface_; + std::optional> + speed_interface_; + std::optional> + joint_position_state_interface_; + std::optional> + joint_velocity_state_interface_; + + std::shared_ptr param_listener_; + Params params_; + + RealtimeGoalHandleBuffer + rt_active_goal_; ///< Container for the currently active action goal, if any. + control_msgs::action::ParallelGripperCommand::Result::SharedPtr pre_alloc_result_; + + rclcpp::Duration action_monitor_period_; + + // ROS API + ActionServerPtr action_server_; + + rclcpp::TimerBase::SharedPtr goal_handle_timer_; + + rclcpp_action::GoalResponse goal_callback( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + + rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle); + + void accepted_callback(std::shared_ptr goal_handle); + + void preempt_active_goal(); + + void set_hold_position(); + + rclcpp::Time last_movement_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); ///< Store stall time + double computed_command_; ///< Computed command + + /** + * \brief Check for success and publish appropriate result and feedback. + **/ + void check_for_success( + const rclcpp::Time & time, double error_position, double current_position, + double current_velocity); +}; + +} // namespace parallel_gripper_action_controller + +#include "parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp" + +#endif // PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp new file mode 100644 index 0000000000..b6fccf1bbf --- /dev/null +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -0,0 +1,396 @@ +// Copyright 2014, SRI International +// +// 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 Sachin Chitta, Adolfo Rodriguez Tsouroukdissian, Stu Glaser + +#ifndef PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ + +#include +#include +#include + +#include +#include "parallel_gripper_controller/parallel_gripper_action_controller.hpp" + +namespace parallel_gripper_action_controller +{ + +void GripperActionController::preempt_active_goal() +{ + // Cancels the currently active goal + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) + { + // Marks the current goal as canceled + active_goal->setCanceled(std::make_shared()); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } +} + +controller_interface::CallbackReturn GripperActionController::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type GripperActionController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + command_struct_rt_ = *(command_.readFromRT()); + + const double current_position = joint_position_state_interface_->get().get_value(); + const double current_velocity = joint_velocity_state_interface_->get().get_value(); + const double error_position = command_struct_rt_.position_cmd_ - current_position; + + check_for_success(get_node()->now(), error_position, current_position, current_velocity); + + joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_); + if (speed_interface_.has_value()) + { + speed_interface_->get().set_value(command_struct_rt_.max_velocity_); + } + if (effort_interface_.has_value()) + { + effort_interface_->get().set_value(command_struct_rt_.max_effort_); + } + + return controller_interface::return_type::OK; +} + +rclcpp_action::GoalResponse GripperActionController::goal_callback( + const rclcpp_action::GoalUUID &, std::shared_ptr goal_handle) +{ + if (goal_handle->command.position.size() != 1) + { + pre_alloc_result_ = std::make_shared(); + pre_alloc_result_->state.position.resize(1); + pre_alloc_result_->state.effort.resize(1); + RCLCPP_ERROR( + get_node()->get_logger(), + "Received action goal with wrong number of position values, expects 1, got %zu", + goal_handle->command.position.size()); + return rclcpp_action::GoalResponse::REJECT; + } + + RCLCPP_INFO(get_node()->get_logger(), "Received & accepted new action goal"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +void GripperActionController::accepted_callback( + std::shared_ptr goal_handle) // Try to update goal +{ + auto rt_goal = std::make_shared(goal_handle); + + // Accept new goal + preempt_active_goal(); + + // This is the non-realtime command_struct + // We use command_ for sharing + command_struct_.position_cmd_ = goal_handle->get_goal()->command.position[0]; + if (!params_.max_velocity_interface.empty() && !goal_handle->get_goal()->command.velocity.empty()) + { + command_struct_.max_velocity_ = goal_handle->get_goal()->command.velocity[0]; + } + else + { + command_struct_.max_velocity_ = params_.max_velocity; + } + if (!params_.max_effort_interface.empty() && !goal_handle->get_goal()->command.effort.empty()) + { + command_struct_.max_effort_ = goal_handle->get_goal()->command.effort[0]; + } + else + { + command_struct_.max_effort_ = params_.max_effort; + } + command_.writeFromNonRT(command_struct_); + + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = false; + + 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(); + + // Setup goal status checking timer + goal_handle_timer_ = get_node()->create_wall_timer( + action_monitor_period_.to_chrono(), + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); +} + +rclcpp_action::CancelResponse GripperActionController::cancel_callback( + const std::shared_ptr goal_handle) +{ + RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); + + // Check that cancel request refers to currently active goal (if any) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) + { + // Enter hold current position mode + set_hold_position(); + + RCLCPP_INFO( + get_node()->get_logger(), "Canceling active action goal because cancel callback received."); + + // Mark the current goal as canceled + auto action_res = std::make_shared(); + active_goal->setCanceled(action_res); + // Reset current goal + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + return rclcpp_action::CancelResponse::ACCEPT; +} + +void GripperActionController::set_hold_position() +{ + command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + command_struct_.max_effort_ = params_.max_effort; + command_struct_.max_velocity_ = params_.max_velocity; + command_.writeFromNonRT(command_struct_); +} + +void GripperActionController::check_for_success( + const rclcpp::Time & time, double error_position, double current_position, + double current_velocity) +{ + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (!active_goal) + { + return; + } + + if (fabs(error_position) < params_.goal_tolerance) + { + pre_alloc_result_->state.effort[0] = computed_command_; + pre_alloc_result_->state.position[0] = current_position; + pre_alloc_result_->reached_goal = true; + pre_alloc_result_->stalled = false; + RCLCPP_DEBUG(get_node()->get_logger(), "Successfully moved to goal."); + active_goal->setSucceeded(pre_alloc_result_); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + else + { + if (fabs(current_velocity) > params_.stall_velocity_threshold) + { + last_movement_time_ = time; + } + else if ((time - last_movement_time_).seconds() > params_.stall_timeout) + { + pre_alloc_result_->state.effort[0] = computed_command_; + pre_alloc_result_->state.position[0] = current_position; + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = true; + + if (params_.allow_stalling) + { + RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Returning success."); + active_goal->setSucceeded(pre_alloc_result_); + } + else + { + RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Aborting action!"); + active_goal->setAborted(pre_alloc_result_); + } + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + } +} + +controller_interface::CallbackReturn GripperActionController::on_configure( + 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 + action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / params_.action_monitor_rate); + RCLCPP_INFO_STREAM( + logger, "Action status changes will be monitored at " << params_.action_monitor_rate << "Hz."); + + // Controlled joint + if (params_.joint.empty()) + { + RCLCPP_ERROR(logger, "Joint name cannot be empty"); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GripperActionController::on_activate( + const rclcpp_lifecycle::State &) +{ + auto 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; }); + if (command_interface_it == command_interfaces_.end()) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Expected 1 " << hardware_interface::HW_IF_POSITION << " command interface"); + return controller_interface::CallbackReturn::ERROR; + } + if (command_interface_it->get_prefix_name() != params_.joint) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Command interface is different than joint name `" + << command_interface_it->get_prefix_name() << "` != `" + << params_.joint << "`"); + return controller_interface::CallbackReturn::ERROR; + } + 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; }); + if (position_state_interface_it == state_interfaces_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position state interface"); + return controller_interface::CallbackReturn::ERROR; + } + 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() << "` != `" + << params_.joint << "`"); + return controller_interface::CallbackReturn::ERROR; + } + 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; }); + if (velocity_state_interface_it == state_interfaces_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 velocity state interface"); + return controller_interface::CallbackReturn::ERROR; + } + 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() << "` != `" + << params_.joint << "`"); + return controller_interface::CallbackReturn::ERROR; + } + + joint_command_interface_ = *command_interface_it; + joint_position_state_interface_ = *position_state_interface_it; + joint_velocity_state_interface_ = *velocity_state_interface_it; + + for (auto & interface : command_interfaces_) + { + if (interface.get_interface_name() == "set_gripper_max_effort") + { + effort_interface_ = interface; + } + else if (interface.get_interface_name() == "set_gripper_max_velocity") + { + speed_interface_ = interface; + } + } + + + // Command - non RT version + command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + command_struct_.max_effort_ = params_.max_effort; + command_struct_.max_velocity_ = params_.max_velocity; + command_.initRT(command_struct_); + + // Result + pre_alloc_result_ = std::make_shared(); + pre_alloc_result_->state.position.resize(1); + pre_alloc_result_->state.effort.resize(1); + pre_alloc_result_->state.position[0] = command_struct_.position_cmd_; + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = false; + + // Action interface + action_server_ = rclcpp_action::create_server( + get_node(), "~/gripper_cmd", + std::bind( + &GripperActionController::goal_callback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&GripperActionController::cancel_callback, this, std::placeholders::_1), + std::bind(&GripperActionController::accepted_callback, this, std::placeholders::_1)); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GripperActionController::on_deactivate( + const rclcpp_lifecycle::State &) +{ + joint_command_interface_ = std::nullopt; + joint_position_state_interface_ = std::nullopt; + joint_velocity_state_interface_ = std::nullopt; + release_interfaces(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +GripperActionController::command_interface_configuration() const +{ + std::vector names = {params_.joint + "/" + hardware_interface::HW_IF_POSITION}; + if (!params_.max_effort_interface.empty()) + { + names.push_back({params_.max_effort_interface}); + } + if (!params_.max_velocity_interface.empty()) + { + names.push_back({params_.max_velocity_interface}); + } + + return {controller_interface::interface_configuration_type::INDIVIDUAL, names}; +} + +controller_interface::InterfaceConfiguration +GripperActionController::state_interface_configuration() const +{ + std::vector interface_names; + for (const auto & interface : params_.state_interfaces) + { + interface_names.push_back(params_.joint + "/" + interface); + } + return {controller_interface::interface_configuration_type::INDIVIDUAL, interface_names}; +} + +GripperActionController::GripperActionController() +: controller_interface::ControllerInterface(), + action_monitor_period_(rclcpp::Duration::from_seconds(0)) +{ +} + +} // namespace parallel_gripper_action_controller + +#endif // PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp new file mode 100644 index 0000000000..63e5c8e1c7 --- /dev/null +++ b/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ + +// 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 GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((dllexport)) +#define GRIPPER_ACTION_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define GRIPPER_ACTION_CONTROLLER_EXPORT __declspec(dllexport) +#define GRIPPER_ACTION_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef GRIPPER_ACTION_CONTROLLER_BUILDING_DLL +#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_EXPORT +#else +#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_IMPORT +#endif +#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE GRIPPER_ACTION_CONTROLLER_PUBLIC +#define GRIPPER_ACTION_CONTROLLER_LOCAL +#else +#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define GRIPPER_ACTION_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define GRIPPER_ACTION_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define GRIPPER_ACTION_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define GRIPPER_ACTION_CONTROLLER_PUBLIC +#define GRIPPER_ACTION_CONTROLLER_LOCAL +#endif +#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml new file mode 100644 index 0000000000..f189071617 --- /dev/null +++ b/parallel_gripper_controller/package.xml @@ -0,0 +1,35 @@ + + + + parallel_gripper_controller + 2.32.0 + The parallel_gripper_controller package + Bence Magyar + + Apache License 2.0 + + Sachin Chitta + + ament_cmake + + backward_ros + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_action + realtime_tools + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/parallel_gripper_controller/ros_control_plugins.xml b/parallel_gripper_controller/ros_control_plugins.xml new file mode 100644 index 0000000000..dcd9609ba7 --- /dev/null +++ b/parallel_gripper_controller/ros_control_plugins.xml @@ -0,0 +1,10 @@ + + + + + + + + diff --git a/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml new file mode 100644 index 0000000000..9b0dbb8a05 --- /dev/null +++ b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml @@ -0,0 +1,69 @@ +parallel_gripper_action_controller: + action_monitor_rate: { + type: double, + default_value: 20.0, + description: "Hz", + read_only: true, + validation: { + gt_eq: [ 0.1 ] + }, + } + joint: { + type: string, + read_only: true, + } + state_interfaces: { + type: string_array, + default_value: [position, velocity], + } + goal_tolerance: { + type: double, + default_value: 0.01, + validation: { + gt_eq: [ 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, + validation: { + gt_eq: [ 0.0 ] + }, + } + max_effort_interface: { + type: string, + description: "Controller will claim the specified effort interface, e.g. robotiq_85_left_knuckle_joint/max_effort_interface.", + default_value: "", + } + max_effort: { + type: double, + default_value: 10.0, + description: "Default effort used for grasping (Newtons)", + validation: { + gt_eq: [ 0.0 ] + }, + } + max_velocity_interface: { + type: string, + description: "Controller will claim the speed specified interface, e.g. robotiq_85_left_knuckle_joint/max_effort_interface.", + default_value: "", + } + max_velocity: { + type: double, + default_value: 0.1, + description: "Default target velocity used for grasping (meters/second)", + validation: { + gt_eq: [ 0.0 ] + }, + } diff --git a/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp b/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp new file mode 100644 index 0000000000..3a6d028ddf --- /dev/null +++ b/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp @@ -0,0 +1,25 @@ +// Copyright 2014, SRI International +// +// 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 Sachin Chitta + +// Project +#include +#include + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + parallel_gripper_action_controller::GripperActionController, + controller_interface::ControllerInterface) diff --git a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp new file mode 100644 index 0000000000..130b12e0bb --- /dev/null +++ b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp @@ -0,0 +1,45 @@ +// 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 "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(TestLoadGripperActionControllers, 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_NE( + cm.load_controller( + "test_gripper_action_position_controller", "position_controllers/GripperActionController"), + nullptr); + ASSERT_NE( + cm.load_controller( + "test_gripper_action_effort_controller", "effort_controllers/GripperActionController"), + nullptr); + + rclcpp::shutdown(); +} diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp new file mode 100644 index 0000000000..d9a5ae958b --- /dev/null +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp @@ -0,0 +1,169 @@ +// 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_parallel_gripper_controller.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::ParallelGripperCommand; +using GoalHandle = rclcpp_action::ServerGoalHandle; +using testing::SizeIs; +using testing::UnorderedElementsAre; + +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", "", 0, "", controller_->define_custom_node_options()); + ASSERT_EQ(result, controller_interface::return_type::OK); + + std::vector command_ifs; + command_ifs.emplace_back(this->joint_1_cmd_); + std::vector state_ifs; + state_ifs.emplace_back(this->joint_1_pos_state_); + state_ifs.emplace_back(this->joint_1_vel_state_); + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); +} + +TEST_F(GripperControllerTest, ParametersNotSet) +{ + this->SetUpController(); + + // configure failed, 'joints' parameter not set + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, JointParameterIsEmpty) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", ""}); + + // configure failed, 'joints' is empty + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, ConfigureParamsSuccess) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "joint_1"}); + + // configure successful + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = this->controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(1lu)); + ASSERT_THAT( + cmd_if_conf.names, + UnorderedElementsAre(std::string("joint_1/") + hardware_interface::HW_IF_POSITION)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = this->controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); + ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint_1/position", "joint_1/velocity")); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); +} + +TEST_F(GripperControllerTest, ActivateWithWrongJointsNamesFails) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "unicorn_joint"}); + + // activate failed, 'joint4' is not a valid joint name for the hardware + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, ActivateSuccess) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "joint1"}); + + // activate successful + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(GripperControllerTest, ActivateDeactivateActivateSuccess) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "joint1"}); + + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_deactivate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // re-assign interfaces + std::vector command_ifs; + command_ifs.emplace_back(this->joint_1_cmd_); + std::vector state_ifs; + state_ifs.emplace_back(this->joint_1_pos_state_); + state_ifs.emplace_back(this->joint_1_vel_state_); + this->controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp new file mode 100644 index 0000000000..8bc6ab954c --- /dev/null +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp @@ -0,0 +1,66 @@ +// 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_PARALLEL_GRIPPER_CONTROLLER_HPP_ +#define TEST_PARALLEL_GRIPPER_CONTROLLER_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "parallel_gripper_controller/parallel_gripper_action_controller.hpp" + +namespace +{ +// subclassing and friending so we can access member variables +class FriendGripperController : public parallel_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}; + + hardware_interface::StateInterface joint_1_pos_state_{ + joint_name_, hardware_interface::HW_IF_POSITION, &joint_states_[0]}; + hardware_interface::StateInterface joint_1_vel_state_{ + joint_name_, hardware_interface::HW_IF_VELOCITY, &joint_states_[1]}; + hardware_interface::CommandInterface joint_1_cmd_{ + joint_name_, hardware_interface::HW_IF_POSITION, &joint_commands_[0]}; +}; + +} // anonymous namespace + +#endif // TEST_PARALLEL_GRIPPER_CONTROLLER_HPP_ From ebca6c0a3f29d75521a7a85683206b32073bd8ac Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 12 Jul 2024 12:02:19 +0200 Subject: [PATCH 20/97] Fix parallel gripper controller CI (#1202) --- .../test/test_load_ackermann_steering_controller.cpp | 2 +- .../test/test_load_admittance_controller.cpp | 2 +- .../test/test_load_bicycle_steering_controller.cpp | 2 +- .../test/test_load_diff_drive_controller.cpp | 2 +- .../test/test_load_joint_group_effort_controller.cpp | 2 +- .../test/test_load_force_torque_sensor_broadcaster.cpp | 2 +- .../test_load_multi_interface_forward_command_controller.cpp | 2 +- .../test/test_load_gripper_action_controllers.cpp | 2 +- .../test/test_load_imu_sensor_broadcaster.cpp | 2 +- .../test/test_load_joint_state_broadcaster.cpp | 2 +- .../test/test_load_joint_trajectory_controller.cpp | 2 +- .../parallel_gripper_action_controller_impl.hpp | 1 - .../test/test_load_parallel_gripper_action_controller.cpp | 4 +--- pid_controller/test/test_load_pid_controller.cpp | 3 +-- .../test/test_load_joint_group_position_controller.cpp | 2 +- .../test/test_load_range_sensor_broadcaster.cpp | 2 +- tricycle_controller/test/test_load_tricycle_controller.cpp | 2 +- .../test/test_load_tricycle_steering_controller.cpp | 2 +- .../test/test_load_joint_group_velocity_controller.cpp | 2 +- 19 files changed, 18 insertions(+), 22 deletions(-) diff --git a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp index 101ecce8ff..8e81643835 100644 --- a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -28,7 +28,7 @@ TEST(TestLoadAckermannSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp index 69808f3505..ea4c14caac 100644 --- a/admittance_controller/test/test_load_admittance_controller.cpp +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -29,7 +29,7 @@ TEST(TestLoadAdmittanceController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_EQ( cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController"), diff --git a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp index f3828660a5..7be5e2dd1d 100644 --- a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp @@ -28,7 +28,7 @@ TEST(TestLoadBicycleSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 0c65532c38..bcd334631f 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -28,7 +28,7 @@ TEST(TestLoadDiffDriveController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::diffbot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::diffbot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp index 4008ac8534..0ac1053d18 100644 --- a/effort_controllers/test/test_load_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_load_joint_group_effort_controller.cpp @@ -30,7 +30,7 @@ TEST(TestLoadJointGroupVelocityController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp index 2e87505003..f8b367d341 100644 --- a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp @@ -32,7 +32,7 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp index 52b63bdae8..49bc277824 100644 --- a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp @@ -31,7 +31,7 @@ TEST(TestLoadMultiInterfaceForwardController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/gripper_controllers/test/test_load_gripper_action_controllers.cpp b/gripper_controllers/test/test_load_gripper_action_controllers.cpp index 5641e1b4b3..b607fb6fe7 100644 --- a/gripper_controllers/test/test_load_gripper_action_controllers.cpp +++ b/gripper_controllers/test/test_load_gripper_action_controllers.cpp @@ -30,7 +30,7 @@ TEST(TestLoadGripperActionControllers, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp index a477a731ff..fa2afab200 100644 --- a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp @@ -32,7 +32,7 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp index 266e33c1c8..b3cf778063 100644 --- a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp @@ -30,7 +30,7 @@ TEST(TestLoadJointStateBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( 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 b3635efaca..5b36afc4e8 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -31,7 +31,7 @@ TEST(TestLoadJointStateController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index b6fccf1bbf..bcfbb3b5c5 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -322,7 +322,6 @@ controller_interface::CallbackReturn GripperActionController::on_activate( } } - // Command - non RT version command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = params_.max_effort; diff --git a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp index 130b12e0bb..6ba99ac248 100644 --- a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp +++ b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp @@ -28,9 +28,7 @@ TEST(TestLoadGripperActionControllers, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/pid_controller/test/test_load_pid_controller.cpp b/pid_controller/test/test_load_pid_controller.cpp index ed645b872c..5cfb04e860 100644 --- a/pid_controller/test/test_load_pid_controller.cpp +++ b/pid_controller/test/test_load_pid_controller.cpp @@ -33,8 +33,7 @@ TEST(TestLoadPidController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NO_THROW(cm.load_controller("test_pid_controller", "pid_controller/PidController")); diff --git a/position_controllers/test/test_load_joint_group_position_controller.cpp b/position_controllers/test/test_load_joint_group_position_controller.cpp index 950773351c..f71474bd4a 100644 --- a/position_controllers/test/test_load_joint_group_position_controller.cpp +++ b/position_controllers/test/test_load_joint_group_position_controller.cpp @@ -30,7 +30,7 @@ TEST(TestLoadJointGroupPositionController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp index 1d8a55b932..b416ac4ef9 100644 --- a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp @@ -32,7 +32,7 @@ TEST(TestLoadRangeSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index bb4194909c..486c04515b 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -32,7 +32,7 @@ TEST(TestLoadTricycleController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController"), diff --git a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp index 4fa3698409..210153eef8 100644 --- a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -28,7 +28,7 @@ TEST(TestLoadTricycleSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp index 7070594639..a7dc72f337 100644 --- a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp @@ -30,7 +30,7 @@ TEST(TestLoadJointGroupVelocityController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( From 362067bc269024faa7a866d13dfc17050b390ce8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 14 Jul 2024 23:56:03 +0200 Subject: [PATCH 21/97] Use a single file for migration + release notes (#1204) --- doc/{migration/Jazzy.rst => migration.rst} | 0 doc/{release_notes/Jazzy.rst => release_notes.rst} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename doc/{migration/Jazzy.rst => migration.rst} (100%) rename doc/{release_notes/Jazzy.rst => release_notes.rst} (100%) diff --git a/doc/migration/Jazzy.rst b/doc/migration.rst similarity index 100% rename from doc/migration/Jazzy.rst rename to doc/migration.rst diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes.rst similarity index 100% rename from doc/release_notes/Jazzy.rst rename to doc/release_notes.rst From 12b531082bcea252d1c4359dacf648afa3a9568d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 15 Jul 2024 11:47:00 +0200 Subject: [PATCH 22/97] [doc] Fix controller idx page (#1205) * Fix controller idx page * Fix gh links --- doc/controllers_index.rst | 4 ++-- doc/migration.rst | 4 ++-- doc/release_notes.rst | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 78abe5d21f..683f23c202 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -16,9 +16,9 @@ Guidelines and Best Practices .. toctree:: :titlesonly: - :glob: - * + mobile_robot_kinematics.rst + writing_new_controller.rst Controllers for Wheeled Mobile Robots diff --git a/doc/migration.rst b/doc/migration.rst index 2114436098..4c0e4608d6 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -1,6 +1,6 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/migration/Jazzy.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/migration.rst -Iron to Jazzy +Migration Guides: Iron to Jazzy ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This list summarizes important changes between Iron (previous) and Jazzy (current) releases, where changes to user code might be necessary. diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 4793fd957d..b55db861b3 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -1,6 +1,6 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/release_notes/Jazzy.rst +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/release_notes.rst -Iron to Jazzy +Release Notes: Iron to Jazzy ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This list summarizes the changes between Iron (previous) and Jazzy (current) releases. From 0bb88805713fb21cddc418bc4c502960d9fd9f39 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 16 Jul 2024 10:02:42 +0200 Subject: [PATCH 23/97] [JTC] Fix test_tolerances_via_actions (#1209) --- joint_trajectory_controller/test/test_tolerances.cpp | 4 ++-- .../test/test_trajectory_actions.cpp | 11 +++++------ .../test/test_trajectory_controller_utils.hpp | 1 + 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp index af5036b869..bd6304df29 100644 --- a/joint_trajectory_controller/test/test_tolerances.cpp +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -32,14 +32,14 @@ using trajectory_msgs::msg::JointTrajectoryPoint; std::vector joint_names_ = {"joint1", "joint2", "joint3"}; control_msgs::action::FollowJointTrajectory_Goal prepareGoalMsg( - const std::vector & points, double timeout, + const std::vector & points, double goal_time_tolerance, const std::vector path_tolerance = std::vector(), const std::vector goal_tolerance = std::vector()) { control_msgs::action::FollowJointTrajectory_Goal goal_msg; - goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(goal_time_tolerance); goal_msg.goal_tolerance = goal_tolerance; goal_msg.path_tolerance = path_tolerance; goal_msg.trajectory.joint_names = joint_names_; diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index fb749ac250..3e65016403 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -153,14 +153,15 @@ class TestTrajectoryActions : public TrajectoryControllerTest using GoalOptions = rclcpp_action::Client::SendGoalOptions; std::shared_future sendActionGoal( - const std::vector & points, double timeout, const GoalOptions & opt, + const std::vector & points, double goal_time_tolerance, + const GoalOptions & opt, const std::vector path_tolerance = std::vector(), const std::vector goal_tolerance = std::vector()) { control_msgs::action::FollowJointTrajectory_Goal goal_msg; - goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(goal_time_tolerance); goal_msg.goal_tolerance = goal_tolerance; goal_msg.path_tolerance = path_tolerance; goal_msg.trajectory.joint_names = joint_names_; @@ -529,7 +530,7 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) point.positions[2] = 3.0; points.push_back(point); - gh_future = sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 0.0, goal_options_); } controller_hw_thread_.join(); @@ -539,7 +540,6 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); auto active_tolerances = traj_controller_->get_active_tolerances(); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); expectDefaultTolerances(active_tolerances); } @@ -639,7 +639,7 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) point.positions[2] = 3.0; points.push_back(point); - gh_future = sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 0.0, goal_options_); } controller_hw_thread_.join(); @@ -649,7 +649,6 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); auto active_tolerances = traj_controller_->get_active_tolerances(); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); expectDefaultTolerances(active_tolerances); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index bbe12251a1..1750f7d30d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -46,6 +46,7 @@ const double stopped_velocity_tolerance = 0.1; joint_trajectory_controller::SegmentTolerances active_tolerances) { EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); From b958da39e04ac9a560b68f4361807dcdc0c77ec6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jul 2024 11:24:08 +0200 Subject: [PATCH 24/97] [GripperController] Fix failing tests (#1210) --- parallel_gripper_controller/CMakeLists.txt | 14 ++--- ...arallel_gripper_action_controller_impl.hpp | 2 +- .../gripper_action_controller_parameters.yaml | 3 ++ .../gripper_action_controller_params.yaml | 7 +++ ...oad_parallel_gripper_action_controller.cpp | 16 +++--- .../test/test_parallel_gripper_controller.cpp | 54 +++++++++---------- .../test/test_parallel_gripper_controller.hpp | 3 +- 7 files changed, 57 insertions(+), 42 deletions(-) create mode 100644 parallel_gripper_controller/test/gripper_action_controller_params.yaml diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt index 56aa623a6f..75380a953f 100644 --- a/parallel_gripper_controller/CMakeLists.txt +++ b/parallel_gripper_controller/CMakeLists.txt @@ -48,17 +48,19 @@ if(BUILD_TESTING) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock(test_load_parallel_gripper_action_controllers - test/test_load_parallel_gripper_action_controller.cpp - ) + add_rostest_with_parameters_gmock(test_load_parallel_gripper_action_controllers + test/test_load_parallel_gripper_action_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/gripper_action_controller_params.yaml) + target_include_directories(test_load_parallel_gripper_action_controllers PRIVATE include) ament_target_dependencies(test_load_parallel_gripper_action_controllers controller_manager ros2_control_test_assets ) - ament_add_gmock(test_parallel_gripper_controller - test/test_parallel_gripper_controller.cpp - ) + add_rostest_with_parameters_gmock(test_parallel_gripper_controller + test/test_parallel_gripper_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/gripper_action_controller_params.yaml) + target_include_directories(test_parallel_gripper_controller PRIVATE include) target_link_libraries(test_parallel_gripper_controller parallel_gripper_action_controller ) diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index bcfbb3b5c5..d37cb87fc8 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -44,7 +44,6 @@ controller_interface::CallbackReturn GripperActionController::on_init() try { param_listener_ = std::make_shared(get_node()); - params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -245,6 +244,7 @@ controller_interface::CallbackReturn GripperActionController::on_configure( RCLCPP_ERROR(logger, "Joint name cannot be empty"); return controller_interface::CallbackReturn::ERROR; } + RCLCPP_INFO(logger, "Joint name is : %s", params_.joint.c_str()); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml index 9b0dbb8a05..10b0df0958 100644 --- a/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml +++ b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml @@ -11,6 +11,9 @@ parallel_gripper_action_controller: joint: { type: string, read_only: true, + validation: { + not_empty<>: null + } } state_interfaces: { type: string_array, diff --git a/parallel_gripper_controller/test/gripper_action_controller_params.yaml b/parallel_gripper_controller/test/gripper_action_controller_params.yaml new file mode 100644 index 0000000000..521960e18c --- /dev/null +++ b/parallel_gripper_controller/test/gripper_action_controller_params.yaml @@ -0,0 +1,7 @@ +test_gripper_action_position_controller: + ros__parameters: + joint: "joint1" + +test_gripper_action_position_controller_empty_joint: + ros__parameters: + joint: "" diff --git a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp index 6ba99ac248..35a10a8c31 100644 --- a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp +++ b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp @@ -22,8 +22,6 @@ TEST(TestLoadGripperActionControllers, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -32,12 +30,16 @@ TEST(TestLoadGripperActionControllers, load_controller) ASSERT_NE( cm.load_controller( - "test_gripper_action_position_controller", "position_controllers/GripperActionController"), - nullptr); - ASSERT_NE( - cm.load_controller( - "test_gripper_action_effort_controller", "effort_controllers/GripperActionController"), + "test_gripper_action_position_controller", + "parallel_gripper_action_controller/GripperActionController"), nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); rclcpp::shutdown(); + return result; } diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp index d9a5ae958b..417d87d40f 100644 --- a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp @@ -34,9 +34,9 @@ using GoalHandle = rclcpp_action::ServerGoalHandle; using testing::SizeIs; using testing::UnorderedElementsAre; -void GripperControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } +void GripperControllerTest::SetUpTestCase() {} -void GripperControllerTest::TearDownTestCase() { rclcpp::shutdown(); } +void GripperControllerTest::TearDownTestCase() {} void GripperControllerTest::SetUp() { @@ -46,11 +46,13 @@ void GripperControllerTest::SetUp() void GripperControllerTest::TearDown() { controller_.reset(nullptr); } -void GripperControllerTest::SetUpController() +void GripperControllerTest::SetUpController( + const std::string & controller_name = "test_gripper_action_position_controller", + controller_interface::return_type expected_result = controller_interface::return_type::OK) { const auto result = - controller_->init("gripper_controller", "", 0, "", controller_->define_custom_node_options()); - ASSERT_EQ(result, controller_interface::return_type::OK); + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()); + ASSERT_EQ(result, expected_result); std::vector command_ifs; command_ifs.emplace_back(this->joint_1_cmd_); @@ -62,7 +64,9 @@ void GripperControllerTest::SetUpController() TEST_F(GripperControllerTest, ParametersNotSet) { - this->SetUpController(); + this->SetUpController( + "test_gripper_action_position_controller_no_parameters", + controller_interface::return_type::ERROR); // configure failed, 'joints' parameter not set ASSERT_EQ( @@ -72,9 +76,9 @@ TEST_F(GripperControllerTest, ParametersNotSet) TEST_F(GripperControllerTest, JointParameterIsEmpty) { - this->SetUpController(); - - this->controller_->get_node()->set_parameter({"joint", ""}); + this->SetUpController( + "test_gripper_action_position_controller_empty_joint", + controller_interface::return_type::ERROR); // configure failed, 'joints' is empty ASSERT_EQ( @@ -86,7 +90,9 @@ TEST_F(GripperControllerTest, ConfigureParamsSuccess) { this->SetUpController(); - this->controller_->get_node()->set_parameter({"joint", "joint_1"}); + this->controller_->get_node()->set_parameter({"joint", "joint1"}); + + rclcpp::spin_some(this->controller_->get_node()->get_node_base_interface()); // configure successful ASSERT_EQ( @@ -98,29 +104,14 @@ TEST_F(GripperControllerTest, ConfigureParamsSuccess) ASSERT_THAT(cmd_if_conf.names, SizeIs(1lu)); ASSERT_THAT( cmd_if_conf.names, - UnorderedElementsAre(std::string("joint_1/") + hardware_interface::HW_IF_POSITION)); + UnorderedElementsAre(std::string("joint1/") + hardware_interface::HW_IF_POSITION)); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = this->controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); - ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint_1/position", "joint_1/velocity")); + ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint1/position", "joint1/velocity")); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } -TEST_F(GripperControllerTest, ActivateWithWrongJointsNamesFails) -{ - this->SetUpController(); - - this->controller_->get_node()->set_parameter({"joint", "unicorn_joint"}); - - // activate failed, 'joint4' is not a valid joint name for the hardware - ASSERT_EQ( - this->controller_->on_configure(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::SUCCESS); - ASSERT_EQ( - this->controller_->on_activate(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::ERROR); -} - TEST_F(GripperControllerTest, ActivateSuccess) { this->SetUpController(); @@ -167,3 +158,12 @@ TEST_F(GripperControllerTest, ActivateDeactivateActivateSuccess) this->controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); } + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp index 8bc6ab954c..d206097782 100644 --- a/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp @@ -42,7 +42,8 @@ class GripperControllerTest : public ::testing::Test void SetUp(); void TearDown(); - void SetUpController(); + void SetUpController( + const std::string & controller_name, controller_interface::return_type expected_result); void SetUpHandles(); protected: From 2674f6d2d1821231781d9528089c7d9d24e76a33 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jul 2024 14:09:30 +0200 Subject: [PATCH 25/97] Fix WaitSet issue in tests (#1206) --- .../test_ackermann_steering_controller.cpp | 2 +- .../test_ackermann_steering_controller.hpp | 49 +++++++--------- .../test/test_admittance_controller.cpp | 2 +- .../test/test_admittance_controller.hpp | 49 +++++++--------- .../test/test_bicycle_steering_controller.cpp | 2 +- .../test/test_bicycle_steering_controller.hpp | 48 ++++++++-------- .../test/test_diff_drive_controller.cpp | 15 ++--- .../test_joint_group_effort_controller.cpp | 23 +++----- .../test_joint_group_effort_controller.hpp | 2 + .../test_force_torque_sensor_broadcaster.cpp | 21 +++++-- .../test/test_forward_command_controller.cpp | 24 +++----- .../test/test_forward_command_controller.hpp | 2 + ...i_interface_forward_command_controller.cpp | 24 +++----- ...i_interface_forward_command_controller.hpp | 2 + .../test/test_imu_sensor_broadcaster.cpp | 20 +++++-- .../test/test_joint_state_broadcaster.cpp | 57 ++++++++++++------- .../test/test_trajectory_controller.cpp | 4 +- .../test/test_trajectory_controller_utils.hpp | 28 ++++----- pid_controller/test/test_pid_controller.cpp | 2 +- pid_controller/test/test_pid_controller.hpp | 52 ++++++++--------- .../test_joint_group_position_controller.cpp | 23 +++----- .../test_joint_group_position_controller.hpp | 2 + .../test/test_range_sensor_broadcaster.cpp | 20 +++++-- .../test_steering_controllers_library.hpp | 49 +++++++--------- .../test/test_tricycle_controller.cpp | 15 ++--- .../test_tricycle_steering_controller.cpp | 2 +- .../test_tricycle_steering_controller.hpp | 49 ++++++++-------- .../test_joint_group_velocity_controller.cpp | 23 +++----- .../test_joint_group_velocity_controller.hpp | 2 + 29 files changed, 294 insertions(+), 319 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 718e6f5856..de45accc0a 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -257,7 +257,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat EXPECT_EQ(msg.steering_angle_command[1], 4.4); publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 363db793d5..9379d956c9 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -76,14 +76,7 @@ class TestableAckermannSteeringController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = - ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } - return ret; + return ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -97,30 +90,25 @@ class TestableAckermannSteeringController * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -214,36 +202,43 @@ class AckermannSteeringControllerFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_ackermann_steering_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(const double linear = 0.1, const double angular = 0.2) diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 6b03249df8..256fe1e5fe 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -277,7 +277,7 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); broadcast_tfs(); ASSERT_EQ( diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 19908d7f9f..a464d050be 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -57,15 +57,6 @@ constexpr auto NODE_FAILURE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; 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 @@ -92,39 +83,27 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon 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; + return admittance_controller::AdmittanceController::on_configure(previous_state); } /** * @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( + void 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) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - 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; }; @@ -279,9 +258,12 @@ class AdmittanceControllerTest : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + ControllerStateMsg::SharedPtr received_msg; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node_->create_subscription( "/test_admittance_controller/status", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node_->get_node_base_interface()); // call update to publish the test value ASSERT_EQ( @@ -289,11 +271,18 @@ class AdmittanceControllerTest : public ::testing::Test controller_interface::return_type::OK); // wait for message to be passed - ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node_->get_clock()->now() + timeout; + while (!received_msg && test_subscription_node_->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands() diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 9904f791b6..a06aabbd20 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -238,7 +238,7 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status EXPECT_EQ(msg.steering_angle_command[0], 2.2); publish_commands(0.1, 0.2); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index dcdb7ed169..4643ee3ee9 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -74,13 +74,7 @@ class TestableBicycleSteeringController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = bicycle_steering_controller::BicycleSteeringController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } - return ret; + return bicycle_steering_controller::BicycleSteeringController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -94,30 +88,25 @@ class TestableBicycleSteeringController * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -187,34 +176,43 @@ class BicycleSteeringControllerFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_bicycle_steering_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on max_sub_check_loop_count - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(const double linear = 0.1, const double angular = 0.2) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index f3f99ac8d8..4976da4dfa 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -57,22 +57,17 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr * @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( + void 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) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); - return true; + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return false; } /** @@ -550,7 +545,7 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) const double angular = 0.0; publish(linear, angular); // wait for msg is be published to the system - ASSERT_TRUE(controller_->wait_for_twist(executor)); + controller_->wait_for_twist(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 200a1beda8..06bbc6fd22 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -30,17 +30,6 @@ using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; -namespace -{ -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 - void JointGroupEffortControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void JointGroupEffortControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -63,6 +52,7 @@ void JointGroupEffortControllerTest::SetUpController() command_ifs.emplace_back(joint_2_cmd_); command_ifs.emplace_back(joint_3_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(JointGroupEffortControllerTest, JointsParameterNotSet) @@ -204,10 +194,13 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp index 6ae9db4670..e871ac6c43 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ b/effort_controllers/test/test_joint_group_effort_controller.hpp @@ -24,6 +24,7 @@ #include "effort_controllers/joint_group_effort_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_EFFORT; @@ -57,6 +58,7 @@ class JointGroupEffortControllerTest : public ::testing::Test CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]}; CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]}; CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 2412361352..9528adbb0f 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -75,31 +75,40 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( geometry_msgs::msg::WrenchStamped & wrench_msg) { // create a new subscriber + geometry_msgs::msg::WrenchStamped::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const geometry_msgs::msg::WrenchStamped::SharedPtr) {}; + auto subs_callback = [&](const geometry_msgs::msg::WrenchStamped::SharedPtr msg) + { received_msg = msg; }; auto subscription = test_subscription_node.create_subscription( "/test_force_torque_sensor_broadcaster/wrench", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(wrench_msg, msg_info)); + wrench_msg = *received_msg; } TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index c31c8a964c..fcfa65ee1c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -30,24 +30,12 @@ #include "rclcpp/qos.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/wait_set.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; using testing::IsEmpty; using testing::SizeIs; -namespace -{ -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 - void ForwardCommandControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void ForwardCommandControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -71,6 +59,7 @@ void ForwardCommandControllerTest::SetUpController() command_ifs.emplace_back(joint_2_pos_cmd_); command_ifs.emplace_back(joint_3_pos_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(ForwardCommandControllerTest, JointsParameterNotSet) @@ -320,10 +309,13 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index 9c6bd2a352..2284d46d61 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -24,6 +24,7 @@ #include "forward_command_controller/forward_command_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_POSITION; @@ -69,6 +70,7 @@ class ForwardCommandControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 7879d5c1d7..c52f4f3ab6 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -32,24 +32,12 @@ #include "rclcpp/qos.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/wait_set.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; using testing::IsEmpty; using testing::SizeIs; -namespace -{ -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 - void MultiInterfaceForwardCommandControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void MultiInterfaceForwardCommandControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -74,6 +62,7 @@ void MultiInterfaceForwardCommandControllerTest::SetUpController(bool set_params command_ifs.emplace_back(joint_1_vel_cmd_); command_ifs.emplace_back(joint_1_eff_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); if (set_params_and_activate) { @@ -273,10 +262,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp index 62a4d4e981..78b244847b 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp @@ -26,6 +26,7 @@ #include "forward_command_controller/multi_interface_forward_command_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_EFFORT; @@ -77,6 +78,7 @@ class MultiInterfaceForwardCommandControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &pos_cmd_}; CommandInterface joint_1_vel_cmd_{joint_name_, HW_IF_VELOCITY, &vel_cmd_}; CommandInterface joint_1_eff_cmd_{joint_name_, HW_IF_EFFORT, &eff_cmd_}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 25a39a8b4d..e8aa571112 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -77,31 +77,39 @@ void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg) { // create a new subscriber + sensor_msgs::msg::Imu::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const sensor_msgs::msg::Imu::SharedPtr) {}; + auto subs_callback = [&](const sensor_msgs::msg::Imu::SharedPtr msg) { received_msg = msg; }; auto subscription = test_subscription_node.create_subscription( "/test_imu_sensor_broadcaster/imu", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(imu_msg, msg_info)); + imu_msg = *received_msg; } TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 2faa55f467..4e4c0631f0 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -664,31 +664,41 @@ void JointStateBroadcasterTest::activate_and_get_joint_state_message( node_state = state_broadcaster_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + sensor_msgs::msg::JointState::SharedPtr received_msg; rclcpp::Node test_node("test_node"); - auto subs_callback = [&](const sensor_msgs::msg::JointState::SharedPtr) {}; + auto subs_callback = [&](const sensor_msgs::msg::JointState::SharedPtr msg) + { received_msg = msg; }; auto subscription = test_node.create_subscription(topic, 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node.get_node_base_interface()); + // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_node.get_clock()->now() + timeout; + while (!received_msg && test_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(joint_state_msg, msg_info)); + joint_state_msg = *received_msg; } void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) @@ -731,51 +741,58 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( node_state = state_broadcaster_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + control_msgs::msg::DynamicJointState::SharedPtr dynamic_joint_state_msg; rclcpp::Node test_node("test_node"); - auto subs_callback = [&](const control_msgs::msg::DynamicJointState::SharedPtr) {}; + auto subs_callback = [&](const control_msgs::msg::DynamicJointState::SharedPtr msg) + { dynamic_joint_state_msg = msg; }; auto subscription = test_node.create_subscription(topic, 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node.get_node_base_interface()); + dynamic_joint_state_msg.reset(); + ASSERT_FALSE(dynamic_joint_state_msg); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_node.get_clock()->now() + timeout; + while (test_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (dynamic_joint_state_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; - - // take message from subscription - control_msgs::msg::DynamicJointState dynamic_joint_state_msg; - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(dynamic_joint_state_msg, msg_info)); + ASSERT_TRUE(dynamic_joint_state_msg); const size_t NUM_JOINTS = 3; 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)); + 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 // and that it is the same across the interfaces // for test purposes they are mapped to the same doubles - for (size_t i = 0; i < dynamic_joint_state_msg.joint_names.size(); ++i) + for (size_t i = 0; i < dynamic_joint_state_msg->joint_names.size(); ++i) { ASSERT_THAT( - dynamic_joint_state_msg.interface_values[i].interface_names, + dynamic_joint_state_msg->interface_values[i].interface_names, ElementsAreArray(INTERFACE_NAMES)); const auto index_in_original_order = std::distance( joint_names_.cbegin(), std::find( - joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg.joint_names[i])); + joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg->joint_names[i])); ASSERT_THAT( - dynamic_joint_state_msg.interface_values[i].values, + dynamic_joint_state_msg->interface_values[i].values, Each(joint_values_[index_in_original_order])); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 611c1d4c1b..21c5705505 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -253,7 +253,7 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}); - subscribeToState(); + subscribeToState(executor); updateController(); // Spin to receive latest state @@ -644,7 +644,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun SetUpAndActivateTrajectoryController( executor, params, true, k_p, 0.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_revolute); - subscribeToState(); + subscribeToState(executor); size_t n_joints = joint_names_.size(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 1750f7d30d..a15b2e5bc5 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -94,11 +94,6 @@ class TestableJointTrajectoryController const rclcpp_lifecycle::State & previous_state) override { auto ret = joint_trajectory_controller::JointTrajectoryController::on_configure(previous_state); - // this class can still be useful without the wait set - if (joint_command_subscriber_) - { - joint_cmd_sub_wait_set_.add_subscription(joint_command_subscriber_); - } return ret; } @@ -108,19 +103,17 @@ class TestableJointTrajectoryController * @brief wait_for_trajectory block until a new JointTrajectory is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function - * - * @return true if new JointTrajectory msg was received, false if timeout */ - bool wait_for_trajectory( + void wait_for_trajectory( rclcpp::Executor & executor, - const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{10}) { - bool success = joint_cmd_sub_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } void set_joint_names(const std::vector & joint_names) @@ -223,7 +216,6 @@ class TestableJointTrajectoryController } } - rclcpp::WaitSet joint_cmd_sub_wait_set_; rclcpp::NodeOptions node_options_; }; @@ -399,7 +391,7 @@ class TrajectoryControllerTest : public ::testing::Test static void TearDownTestCase() { rclcpp::shutdown(); } - void subscribeToState() + void subscribeToState(rclcpp::Executor & executor) { auto traj_lifecycle_node = traj_controller_->get_node(); @@ -416,6 +408,14 @@ class TrajectoryControllerTest : public ::testing::Test std::lock_guard guard(state_mutex_); state_msg_ = msg; }); + + const auto timeout = std::chrono::milliseconds{10}; + const auto until = traj_lifecycle_node->get_clock()->now() + timeout; + while (traj_lifecycle_node->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } } /// Publish trajectory msgs with multiple points diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index a44347f5f1..4e9601ae66 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -495,7 +495,7 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status) } publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) { diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 1c356263e7..8c36dca60f 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -65,13 +65,7 @@ class TestablePidController : public pid_controller::PidController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = pid_controller::PidController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_); - } - return ret; + return pid_controller::PidController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -85,30 +79,25 @@ class TestablePidController : public pid_controller::PidController * @brief wait_for_command 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_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -182,36 +171,43 @@ class PidControllerFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_pid_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands( diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 60bff556db..a4a288a8e4 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -30,17 +30,6 @@ using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; -namespace -{ -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 - void JointGroupPositionControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void JointGroupPositionControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -63,6 +52,7 @@ void JointGroupPositionControllerTest::SetUpController() command_ifs.emplace_back(joint_2_pos_cmd_); command_ifs.emplace_back(joint_3_pos_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(JointGroupPositionControllerTest, JointsParameterNotSet) @@ -204,10 +194,13 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/position_controllers/test/test_joint_group_position_controller.hpp b/position_controllers/test/test_joint_group_position_controller.hpp index 93149d8e19..c7d704db52 100644 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ b/position_controllers/test/test_joint_group_position_controller.hpp @@ -24,6 +24,7 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "position_controllers/joint_group_position_controller.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_POSITION; @@ -58,6 +59,7 @@ class JointGroupPositionControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index a23d5e3cde..ef9d5187a3 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -66,31 +66,39 @@ controller_interface::CallbackReturn RangeSensorBroadcasterTest::configure_broad void RangeSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Range & range_msg) { // create a new subscriber + sensor_msgs::msg::Range::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const sensor_msgs::msg::Range::SharedPtr) {}; + auto subs_callback = [&](const sensor_msgs::msg::Range::SharedPtr msg) { received_msg = msg; }; auto subscription = test_subscription_node.create_subscription( "/test_range_sensor_broadcaster/range", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { range_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(range_msg, msg_info)); + range_msg = *received_msg; } TEST_F(RangeSensorBroadcasterTest, Initialize_RangeBroadcaster_Exception) diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index b6fe1770c1..ae7c41a09b 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -80,14 +80,7 @@ class TestableSteeringControllersLibrary controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = - steering_controllers_library::SteeringControllersLibrary::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } - return ret; + return steering_controllers_library::SteeringControllersLibrary::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -101,26 +94,24 @@ class TestableSteeringControllersLibrary * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + wait_for_command(executor, timeout); } // implementing methods which are declared virtual in the steering_controllers_library.hpp @@ -139,9 +130,6 @@ class TestableSteeringControllersLibrary } bool update_odometry(const rclcpp::Duration & /*period*/) { return true; } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -235,36 +223,43 @@ class SteeringControllersLibraryFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_steering_controllers_library/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(const double linear = 0.1, const double angular = 0.2) diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 4be1680980..901597a8bc 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -62,22 +62,17 @@ class TestableTricycleController : public tricycle_controller::TricycleControlle * @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( + void 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) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); - return true; + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return false; } }; @@ -326,7 +321,7 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) const double angular = 0.0; publish(linear, angular); // wait for msg is be published to the system - ASSERT_TRUE(controller_->wait_for_twist(executor)); + controller_->wait_for_twist(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 328f5e4d6a..ad7f80607f 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -241,7 +241,7 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu EXPECT_EQ(msg.steering_angle_command[0], 2.2); publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index b4d58a95c0..8a3ea33925 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -75,14 +75,7 @@ class TestableTricycleSteeringController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = - tricycle_steering_controller::TricycleSteeringController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } - return ret; + return tricycle_steering_controller::TricycleSteeringController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -96,30 +89,25 @@ class TestableTricycleSteeringController * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + return wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -201,34 +189,43 @@ class TricycleSteeringControllerFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_tricycle_steering_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(const double linear = 0.1, const double angular = 0.2) diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index a99ffaeebf..f9aa666e30 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -30,17 +30,6 @@ using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; -namespace -{ -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 - void JointGroupVelocityControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void JointGroupVelocityControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -63,6 +52,7 @@ void JointGroupVelocityControllerTest::SetUpController() command_ifs.emplace_back(joint_2_cmd_); command_ifs.emplace_back(joint_3_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(JointGroupVelocityControllerTest, JointsParameterNotSet) @@ -204,10 +194,13 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.hpp b/velocity_controllers/test/test_joint_group_velocity_controller.hpp index e94f7f082d..3078359028 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.hpp @@ -23,6 +23,7 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" #include "velocity_controllers/joint_group_velocity_controller.hpp" using hardware_interface::CommandInterface; @@ -58,6 +59,7 @@ class JointGroupVelocityControllerTest : public ::testing::Test CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_VELOCITY, &joint_commands_[0]}; CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_VELOCITY, &joint_commands_[1]}; CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_VELOCITY, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ From 96a8d5798c1e2a80b104d2fb3312e1444dc142c5 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 16 Jul 2024 06:50:22 -0600 Subject: [PATCH 26/97] Unused header cleanup (#1199) --- .../test/test_ackermann_steering_controller.cpp | 2 -- .../test/test_ackermann_steering_controller.hpp | 8 ++------ ...st_ackermann_steering_controller_preceeding.cpp | 2 -- .../admittance_controller.hpp | 8 -------- .../admittance_controller/admittance_rule.hpp | 13 +++---------- .../admittance_controller/admittance_rule_impl.hpp | 5 +++-- .../src/admittance_controller.cpp | 4 ---- .../test/test_admittance_controller.cpp | 2 -- .../test/test_admittance_controller.hpp | 7 +++---- .../test/test_asset_6d_robot_description.hpp | 2 -- .../test/test_bicycle_steering_controller.cpp | 2 -- .../test/test_bicycle_steering_controller.hpp | 8 ++------ ...test_bicycle_steering_controller_preceeding.cpp | 2 -- .../diff_drive_controller.hpp | 4 ---- .../test/test_diff_drive_controller.cpp | 2 -- .../joint_group_effort_controller.hpp | 3 --- .../src/joint_group_effort_controller.cpp | 1 - .../test/test_joint_group_effort_controller.cpp | 5 ----- .../test/test_joint_group_effort_controller.hpp | 4 ++-- .../force_torque_sensor_broadcaster.hpp | 3 --- .../test/test_force_torque_sensor_broadcaster.cpp | 5 ----- .../test/test_force_torque_sensor_broadcaster.hpp | 4 ++-- .../forward_command_controller.hpp | 2 -- .../forward_controllers_base.hpp | 1 - .../multi_interface_forward_command_controller.hpp | 2 -- .../src/forward_command_controller.cpp | 3 --- .../src/forward_controllers_base.cpp | 2 -- .../hardware_interface_adapter.hpp | 1 - .../test/test_gripper_controllers.cpp | 4 ---- .../test/test_gripper_controllers.hpp | 4 ++-- .../imu_sensor_broadcaster.hpp | 3 --- .../test/test_imu_sensor_broadcaster.cpp | 5 ----- .../test/test_imu_sensor_broadcaster.hpp | 4 ++-- .../joint_state_broadcaster.hpp | 2 -- .../src/joint_state_broadcaster.cpp | 7 +------ .../test/test_joint_state_broadcaster.cpp | 5 +---- .../test/test_joint_state_broadcaster.hpp | 4 ++-- .../joint_trajectory_controller.hpp | 2 -- .../joint_trajectory_controller/tolerances.hpp | 3 --- .../src/joint_trajectory_controller.cpp | 3 --- joint_trajectory_controller/src/trajectory.cpp | 1 - joint_trajectory_controller/test/test_assets.hpp | 2 -- .../test/test_load_joint_trajectory_controller.cpp | 4 ++-- .../test/test_tolerances.cpp | 5 ++--- .../test/test_trajectory.cpp | 7 ++----- .../test/test_trajectory_actions.cpp | 8 -------- .../test/test_trajectory_controller.cpp | 14 +------------- .../include/pid_controller/pid_controller.hpp | 6 ------ pid_controller/src/pid_controller.cpp | 3 --- pid_controller/test/test_pid_controller.cpp | 1 - pid_controller/test/test_pid_controller.hpp | 7 ++----- .../test/test_pid_controller_preceding.cpp | 2 -- .../joint_group_position_controller.hpp | 3 --- .../src/joint_group_position_controller.cpp | 1 - .../test/test_joint_group_position_controller.cpp | 5 ----- .../test/test_joint_group_position_controller.hpp | 4 ++-- .../range_sensor_broadcaster.hpp | 3 --- .../steering_controllers_library.hpp | 6 ------ .../steering_odometry.hpp | 3 +-- .../src/steering_controllers_library.cpp | 4 ---- .../src/steering_odometry.cpp | 1 - .../test/test_steering_controllers_library.cpp | 3 --- .../test/test_steering_controllers_library.hpp | 8 ++------ .../test/test_steering_odometry.cpp | 2 +- .../include/tricycle_controller/odometry.hpp | 2 +- .../tricycle_controller/tricycle_controller.hpp | 3 --- .../test/test_tricycle_controller.cpp | 2 -- .../test/test_tricycle_steering_controller.cpp | 2 -- .../test/test_tricycle_steering_controller.hpp | 8 ++------ ...est_tricycle_steering_controller_preceeding.cpp | 2 -- .../joint_group_velocity_controller.hpp | 3 --- .../src/joint_group_velocity_controller.cpp | 1 - .../test/test_joint_group_velocity_controller.cpp | 5 ----- .../test/test_joint_group_velocity_controller.hpp | 4 ++-- 74 files changed, 45 insertions(+), 243 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index de45accc0a..c04e87be95 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -14,10 +14,8 @@ #include "test_ackermann_steering_controller.hpp" -#include #include #include -#include #include class AckermannSteeringControllerTest diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 9379d956c9..320a1a91a6 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -15,22 +15,18 @@ #ifndef TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ #define TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ +#include + #include -#include #include #include -#include #include #include #include "ackermann_steering_controller/ackermann_steering_controller.hpp" -#include "gmock/gmock.h" #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/time.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using ControllerStateMsg = diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index 1a16bed838..96dd20d80e 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -14,10 +14,8 @@ #include "test_ackermann_steering_controller.hpp" -#include #include #include -#include #include class AckermannSteeringControllerTest diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 6ff6cdae7a..9be6c3298c 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -17,7 +17,6 @@ #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ #define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ -#include #include #include #include @@ -29,21 +28,14 @@ #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; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 36c027491c..7223dbe9d1 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -18,23 +18,16 @@ #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ #include -#include -#include + #include #include #include +#include "admittance_controller_parameters.hpp" #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 "controller_interface/controller_interface_base.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 diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 9b03924882..77f1277b35 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -22,9 +22,10 @@ #include #include +#include +#include + #include "rclcpp/duration.hpp" -#include "rclcpp/utilities.hpp" -#include "tf2_ros/transform_listener.h" namespace admittance_controller { diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index c6a8168736..6e29b574a2 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -16,17 +16,13 @@ #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 diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 256fe1e5fe..5c4bbe9438 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -16,9 +16,7 @@ #include "test_admittance_controller.hpp" -#include #include -#include #include // Test on_init returns ERROR when a required parameter is missing diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index a464d050be..d2aef3df4b 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -17,6 +17,8 @@ #ifndef TEST_ADMITTANCE_CONTROLLER_HPP_ #define TEST_ADMITTANCE_CONTROLLER_HPP_ +#include + #include #include #include @@ -25,21 +27,18 @@ #include #include -#include "gmock/gmock.h" +#include "geometry_msgs/msg/pose_stamped.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 "test_asset_6d_robot_description.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; diff --git a/admittance_controller/test/test_asset_6d_robot_description.hpp b/admittance_controller/test/test_asset_6d_robot_description.hpp index 4d38df7c30..11412ebbab 100644 --- a/admittance_controller/test/test_asset_6d_robot_description.hpp +++ b/admittance_controller/test/test_asset_6d_robot_description.hpp @@ -15,8 +15,6 @@ #ifndef TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ #define TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ -#include - namespace ros2_control_test_assets { const auto valid_6d_robot_urdf = diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index a06aabbd20..573992b24e 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -14,10 +14,8 @@ #include "test_bicycle_steering_controller.hpp" -#include #include #include -#include #include class BicycleSteeringControllerTest diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 4643ee3ee9..a6b3f8ae40 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -15,22 +15,18 @@ #ifndef TEST_BICYCLE_STEERING_CONTROLLER_HPP_ #define TEST_BICYCLE_STEERING_CONTROLLER_HPP_ +#include + #include -#include #include #include -#include #include #include #include "bicycle_steering_controller/bicycle_steering_controller.hpp" -#include "gmock/gmock.h" #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/time.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using ControllerStateMsg = diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp index bc3a182753..0bc03f4886 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -14,10 +14,8 @@ #include "test_bicycle_steering_controller.hpp" -#include #include #include -#include #include class BicycleSteeringControllerTest 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 72b38f7d2d..ac34b81eca 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 @@ -30,15 +30,11 @@ #include "diff_drive_controller/odometry.hpp" #include "diff_drive_controller/speed_limiter.hpp" #include "diff_drive_controller/visibility_control.h" -#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 "odometry.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" diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 4976da4dfa..bc664f0711 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -14,7 +14,6 @@ #include -#include #include #include #include @@ -26,7 +25,6 @@ #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" using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::HW_IF_POSITION; diff --git a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp index 562bbea52f..b07e8a630c 100644 --- a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp +++ b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp @@ -15,11 +15,8 @@ #ifndef EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ #define EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ -#include - #include "effort_controllers/visibility_control.h" #include "forward_command_controller/forward_command_controller.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace effort_controllers { diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index c3748d493c..e7d0f274a5 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -17,7 +17,6 @@ #include "controller_interface/controller_interface.hpp" #include "effort_controllers/joint_group_effort_controller.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/logging.hpp" #include "rclcpp/parameter.hpp" namespace effort_controllers diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 06bbc6fd22..c19cbff9cf 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -12,19 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include #include #include #include #include #include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_effort_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp index e871ac6c43..5af880d792 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ b/effort_controllers/test/test_joint_group_effort_controller.hpp @@ -15,12 +15,12 @@ #ifndef TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ #define TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "effort_controllers/joint_group_effort_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" 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 754d1de9ba..bd477ed68a 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 @@ -20,15 +20,12 @@ #define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ #include -#include -#include #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" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/force_torque_sensor.hpp" diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 9528adbb0f..2872e8a60f 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -22,14 +22,9 @@ #include #include -#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedStateInterface; using testing::IsEmpty; diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index fe5b0ab3ba..fe7fb0c174 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -19,11 +19,11 @@ #ifndef TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ #define TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ +#include + #include #include -#include "gmock/gmock.h" - #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" // subclassing and friending so we can access member variables 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 91e3aae480..12f2a28b22 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 @@ -16,8 +16,6 @@ #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" diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index d60245f328..4858e5ab64 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -22,7 +22,6 @@ #include "controller_interface/controller_interface.hpp" #include "forward_command_controller/visibility_control.h" #include "rclcpp/subscription.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "std_msgs/msg/float64_multi_array.hpp" 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 fd7c0d480e..50476c62e3 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 @@ -16,8 +16,6 @@ #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" diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 0305a37e4e..78fe8c9191 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -14,14 +14,11 @@ #include "forward_command_controller/forward_command_controller.hpp" -#include #include #include -#include #include #include "rclcpp/logging.hpp" -#include "rclcpp/qos.hpp" namespace forward_command_controller { diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index e4ea46fcc5..331d3f9eea 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -14,10 +14,8 @@ #include "forward_command_controller/forward_controllers_base.hpp" -#include #include #include -#include #include #include "controller_interface/helpers.hpp" diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index b125ab12d0..2cc24794b8 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -22,7 +22,6 @@ #include #include #include -#include #include "control_toolbox/pid.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp index 9f7e024917..97fef24eaf 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include @@ -23,9 +22,6 @@ #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; diff --git a/gripper_controllers/test/test_gripper_controllers.hpp b/gripper_controllers/test/test_gripper_controllers.hpp index 4983c8102d..cb92bc9dcd 100644 --- a/gripper_controllers/test/test_gripper_controllers.hpp +++ b/gripper_controllers/test/test_gripper_controllers.hpp @@ -15,12 +15,12 @@ #ifndef TEST_GRIPPER_CONTROLLERS_HPP_ #define TEST_GRIPPER_CONTROLLERS_HPP_ +#include + #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" 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 cd2a44d32b..1ba0b032ac 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 @@ -20,14 +20,11 @@ #define IMU_SENSOR_BROADCASTER__IMU_SENSOR_BROADCASTER_HPP_ #include -#include -#include #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" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/imu_sensor.hpp" diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index e8aa571112..a2da6713bb 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -23,12 +23,7 @@ #include #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp" -#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "sensor_msgs/msg/imu.hpp" using hardware_interface::LoanedStateInterface; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 01423724b8..0f3286c302 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -19,11 +19,11 @@ #ifndef TEST_IMU_SENSOR_BROADCASTER_HPP_ #define TEST_IMU_SENSOR_BROADCASTER_HPP_ +#include + #include #include -#include "gmock/gmock.h" - #include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp" // subclassing and friending so we can access member variables 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 f1c532dce9..b3fa69f94c 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 @@ -25,8 +25,6 @@ #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" #include "realtime_tools/realtime_publisher.h" #include "sensor_msgs/msg/joint_state.hpp" diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index a53fe2b3c4..5eb0a9b9b6 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -14,21 +14,16 @@ #include "joint_state_broadcaster/joint_state_broadcaster.hpp" -#include +#include #include #include #include #include #include -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/clock.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rcpputils/split.hpp" -#include "rcutils/logging_macros.h" #include "std_msgs/msg/header.hpp" namespace rclcpp_lifecycle diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 4e4c0631f0..e479f344bf 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -23,12 +23,9 @@ #include "gmock/gmock.h" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "joint_state_broadcaster/joint_state_broadcaster.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_state_broadcaster.hpp" using hardware_interface::HW_IF_EFFORT; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index fa9d29c936..3e54116d5c 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -15,12 +15,12 @@ #ifndef TEST_JOINT_STATE_BROADCASTER_HPP_ #define TEST_JOINT_STATE_BROADCASTER_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "joint_state_broadcaster/joint_state_broadcaster.hpp" #include "hardware_interface/types/hardware_interface_type_values.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 c3fbb58456..e22dee8946 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 @@ -15,7 +15,6 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ -#include #include // for std::reference_wrapper #include #include @@ -36,7 +35,6 @@ #include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" #include "rclcpp_action/server.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" diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index c4404920df..8c556703ab 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -38,9 +38,6 @@ #include "control_msgs/action/follow_joint_trajectory.hpp" #include "joint_trajectory_controller_parameters.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/time.hpp" - namespace joint_trajectory_controller { /** diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 31598bb813..ddcf57f508 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -22,10 +22,7 @@ #include #include "angles/angles.h" -#include "builtin_interfaces/msg/duration.hpp" -#include "builtin_interfaces/msg/time.hpp" #include "controller_interface/helpers.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "lifecycle_msgs/msg/state.hpp" diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 0ed7f2ff13..54f785a070 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -20,7 +20,6 @@ #include "hardware_interface/macros.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" -#include "std_msgs/msg/header.hpp" namespace joint_trajectory_controller { diff --git a/joint_trajectory_controller/test/test_assets.hpp b/joint_trajectory_controller/test/test_assets.hpp index ccdbaf4c8a..df6f2a37c7 100644 --- a/joint_trajectory_controller/test/test_assets.hpp +++ b/joint_trajectory_controller/test/test_assets.hpp @@ -15,8 +15,6 @@ #ifndef TEST_ASSETS_HPP_ #define TEST_ASSETS_HPP_ -#include - namespace test_trajectory_controllers { const auto urdf_rrrbot_revolute = 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 5b36afc4e8..8d0fbb3e75 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,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include -#include "gmock/gmock.h" +#include #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp index bd6304df29..106a9a761f 100644 --- a/joint_trajectory_controller/test/test_tolerances.cpp +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -12,12 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include + #include -#include #include -#include "gmock/gmock.h" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index 6e0c53ac77..b0d7ad8e18 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -12,15 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include + #include #include #include -#include "gmock/gmock.h" - -#include "builtin_interfaces/msg/duration.hpp" -#include "builtin_interfaces/msg/time.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 3e65016403..164f7d0e11 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -15,38 +15,30 @@ #ifndef _MSC_VER #include #endif -#include #include #include #include #include -#include #include #include -#include #include #include #include "control_msgs/action/detail/follow_joint_trajectory__struct.hpp" -#include "controller_interface/controller_interface.hpp" #include "gtest/gtest.h" -#include "hardware_interface/resource_manager.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/logging.hpp" -#include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_action/client.hpp" #include "rclcpp_action/client_goal_handle.hpp" #include "rclcpp_action/create_client.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "test_trajectory_controller_utils.hpp" using std::placeholders::_1; diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 21c5705505..e83e74ef41 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -12,35 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include -#include #include -#include -#include #include -#include #include #include #include "builtin_interfaces/msg/duration.hpp" -#include "builtin_interfaces/msg/time.hpp" -#include "controller_interface/controller_interface.hpp" -#include "hardware_interface/resource_manager.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" -#include "rclcpp/publisher.hpp" -#include "rclcpp/qos.hpp" -#include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index f7b8cc4491..236b067929 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -28,17 +28,11 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "pid_controller/visibility_control.h" #include "pid_controller_parameters.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/set_bool.hpp" -#include "control_msgs/msg/joint_controller_state.hpp" - -#include "control_msgs/msg/pid_state.hpp" -#include "trajectory_msgs/msg/joint_trajectory_point.hpp" - namespace pid_controller { diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index b76926d5a0..c8e6cc0fe0 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -24,9 +24,6 @@ #include "angles/angles.h" #include "control_msgs/msg/single_dof_state.hpp" -#include "controller_interface/helpers.hpp" - -#include "rclcpp/rclcpp.hpp" #include "rclcpp/version.h" namespace diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 4e9601ae66..9b53dccb23 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include using pid_controller::feedforward_mode_type; diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 8c36dca60f..895c56c1a5 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -18,20 +18,17 @@ #ifndef TEST_PID_CONTROLLER_HPP_ #define TEST_PID_CONTROLLER_HPP_ +#include + #include -#include #include #include -#include #include #include -#include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "pid_controller/pid_controller.hpp" -#include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index 3e17e69286..498ca633da 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -17,10 +17,8 @@ #include "test_pid_controller.hpp" -#include #include #include -#include #include using pid_controller::feedforward_mode_type; diff --git a/position_controllers/include/position_controllers/joint_group_position_controller.hpp b/position_controllers/include/position_controllers/joint_group_position_controller.hpp index 47705b6a3d..4eaf9086e4 100644 --- a/position_controllers/include/position_controllers/joint_group_position_controller.hpp +++ b/position_controllers/include/position_controllers/joint_group_position_controller.hpp @@ -15,11 +15,8 @@ #ifndef POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ #define POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ -#include - #include "forward_command_controller/forward_command_controller.hpp" #include "position_controllers/visibility_control.h" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace position_controllers { diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp index 8335a200c4..1074ba7ef7 100644 --- a/position_controllers/src/joint_group_position_controller.cpp +++ b/position_controllers/src/joint_group_position_controller.cpp @@ -17,7 +17,6 @@ #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "position_controllers/joint_group_position_controller.hpp" -#include "rclcpp/logging.hpp" #include "rclcpp/parameter.hpp" namespace position_controllers diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index a4a288a8e4..96a5cead17 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -12,19 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include #include #include #include #include #include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_position_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; diff --git a/position_controllers/test/test_joint_group_position_controller.hpp b/position_controllers/test/test_joint_group_position_controller.hpp index c7d704db52..fefd2073e3 100644 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ b/position_controllers/test/test_joint_group_position_controller.hpp @@ -15,12 +15,12 @@ #ifndef TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ #define TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "position_controllers/joint_group_position_controller.hpp" diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp index b2e5fbfac0..5a93f95982 100644 --- a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp @@ -20,13 +20,10 @@ #define RANGE_SENSOR_BROADCASTER__RANGE_SENSOR_BROADCASTER_HPP_ #include -#include -#include #include "controller_interface/controller_interface.hpp" #include "range_sensor_broadcaster/visibility_control.h" #include "range_sensor_broadcaster_parameters.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/range_sensor.hpp" diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index e417b9fcd1..84a892d79e 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -15,21 +15,16 @@ #ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ #define STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ -#include #include #include -#include #include -#include #include #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" -#include "std_srvs/srv/set_bool.hpp" #include "steering_controllers_library/steering_odometry.hpp" #include "steering_controllers_library/visibility_control.h" #include "steering_controllers_library_parameters.hpp" @@ -37,7 +32,6 @@ // TODO(anyone): Replace with controller specific messages #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" #include "control_msgs/msg/steering_controller_status.hpp" -#include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index dadc5730d4..5b67797b79 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -22,8 +22,7 @@ #include #include -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include // \note The versions conditioning is added here to support the source-compatibility with Humble #if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 1aef556212..f193098f82 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -16,15 +16,11 @@ #include #include -#include #include #include #include -#include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "tf2/transform_datatypes.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index f8b42771e2..ba431faf33 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -20,7 +20,6 @@ #include "steering_controllers_library/steering_odometry.hpp" #include -#include #include namespace steering_odometry diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 98ab97fdc3..fca7d00946 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -17,11 +17,8 @@ #include #include #include -#include #include -#include "hardware_interface/types/hardware_interface_type_values.hpp" - class SteeringControllersLibraryTest : public SteeringControllersLibraryFixture { diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index ae7c41a09b..b257562dcd 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -15,21 +15,17 @@ #ifndef TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ #define TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ +#include + #include -#include #include #include -#include #include #include -#include "gmock/gmock.h" #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/time.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "steering_controllers_library/steering_controllers_library.hpp" diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index d93e29eca5..e3c8db6c15 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "gmock/gmock.h" +#include #include "steering_controllers_library/steering_odometry.hpp" diff --git a/tricycle_controller/include/tricycle_controller/odometry.hpp b/tricycle_controller/include/tricycle_controller/odometry.hpp index 13d65a980a..f3252df12f 100644 --- a/tricycle_controller/include/tricycle_controller/odometry.hpp +++ b/tricycle_controller/include/tricycle_controller/odometry.hpp @@ -21,7 +21,7 @@ #include -#include "rclcpp/time.hpp" +#include // \note The versions conditioning is added here to support the source-compatibility with Humble #if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 #include "rcpputils/rolling_mean_accumulator.hpp" diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index b6f9aae417..010a890f52 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -31,12 +31,9 @@ #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 "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" diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 901597a8bc..e56e3afacb 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -18,7 +18,6 @@ #include -#include #include #include #include @@ -29,7 +28,6 @@ #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; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index ad7f80607f..3f2589cb6c 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -14,10 +14,8 @@ #include "test_tricycle_steering_controller.hpp" -#include #include #include -#include #include class TricycleSteeringControllerTest diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 8a3ea33925..184439aa6f 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -15,21 +15,17 @@ #ifndef TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ #define TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ +#include + #include -#include #include #include -#include #include #include -#include "gmock/gmock.h" #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/time.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "tricycle_steering_controller/tricycle_steering_controller.hpp" diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp index 6f2913aeb8..2170659ee7 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -14,10 +14,8 @@ #include "test_tricycle_steering_controller.hpp" -#include #include #include -#include #include class TricycleSteeringControllerTest diff --git a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp index b5c36f433a..e443ba76cc 100644 --- a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp +++ b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp @@ -15,10 +15,7 @@ #ifndef VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ #define VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ -#include - #include "forward_command_controller/forward_command_controller.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "velocity_controllers/visibility_control.h" namespace velocity_controllers diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index eb55e052dc..97f6713d72 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -16,7 +16,6 @@ #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/logging.hpp" #include "rclcpp/parameter.hpp" #include "velocity_controllers/joint_group_velocity_controller.hpp" diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index f9aa666e30..d781b6ca5c 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -12,19 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include #include #include #include #include #include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_velocity_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.hpp b/velocity_controllers/test/test_joint_group_velocity_controller.hpp index 3078359028..0ebe68833c 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.hpp @@ -15,12 +15,12 @@ #ifndef TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ #define TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" From f7b1af1841c42bf6c1dc617b170e5abc0150c99c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Jul 2024 11:40:05 +0200 Subject: [PATCH 27/97] Change the subscription timeout in the tests to 5ms (#1219) --- .../test/test_ackermann_steering_controller.hpp | 2 +- admittance_controller/test/test_admittance_controller.hpp | 2 +- .../test/test_bicycle_steering_controller.hpp | 2 +- .../test/test_force_torque_sensor_broadcaster.cpp | 2 +- imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp | 2 +- joint_state_broadcaster/test/test_joint_state_broadcaster.cpp | 4 ++-- pid_controller/test/test_pid_controller.hpp | 2 +- .../test/test_range_sensor_broadcaster.cpp | 2 +- .../test/test_steering_controllers_library.hpp | 2 +- .../test/test_tricycle_steering_controller.hpp | 2 +- 10 files changed, 11 insertions(+), 11 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 320a1a91a6..b0b0944de2 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -216,7 +216,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index d2aef3df4b..b2a95c12fa 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -270,7 +270,7 @@ class AdmittanceControllerTest : public ::testing::Test controller_interface::return_type::OK); // wait for message to be passed - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node_->get_clock()->now() + timeout; while (!received_msg && test_subscription_node_->get_clock()->now() < until) { diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index a6b3f8ae40..bfcf9fa424 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -190,7 +190,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 2872e8a60f..1acddcb64a 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -85,7 +85,7 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( while (max_sub_check_loop_count--) { fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index a2da6713bb..132bf4006e 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -86,7 +86,7 @@ void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & while (max_sub_check_loop_count--) { imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index e479f344bf..71e65a978d 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -677,7 +677,7 @@ void JointStateBroadcasterTest::activate_and_get_joint_state_message( while (max_sub_check_loop_count--) { state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_node.get_clock()->now() + timeout; while (!received_msg && test_node.get_clock()->now() < until) { @@ -755,7 +755,7 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( while (max_sub_check_loop_count--) { state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_node.get_clock()->now() + timeout; while (test_node.get_clock()->now() < until) { diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 895c56c1a5..326393bac0 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -186,7 +186,7 @@ class PidControllerFixture : public ::testing::Test while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index ef9d5187a3..e8be5e3d0a 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -80,7 +80,7 @@ void RangeSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Ran while (max_sub_check_loop_count--) { range_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index b257562dcd..8c4955b7eb 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -237,7 +237,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 184439aa6f..0fbea25abf 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -203,7 +203,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); - const auto timeout = std::chrono::milliseconds{1}; + const auto timeout = std::chrono::milliseconds{5}; const auto until = test_subscription_node.get_clock()->now() + timeout; while (!received_msg && test_subscription_node.get_clock()->now() < until) { From 990bfb645425c166a923ddb53ddd98ec66ea42d8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Noel=20Jim=C3=A9nez=20Garc=C3=ADa?= Date: Thu, 18 Jul 2024 14:47:05 +0200 Subject: [PATCH 28/97] Remove duplicated call to rclcpp::shutdown in test (#1220) --- diff_drive_controller/test/test_load_diff_drive_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index bcd334631f..aa23f83ec9 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -41,6 +41,5 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); int result = RUN_ALL_TESTS(); rclcpp::shutdown(); - rclcpp::shutdown(); return result; } From a95364449b110056c3f3d389e1a1903cc23996ff Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 21 Jul 2024 11:03:44 +0200 Subject: [PATCH 29/97] Use the internal methods instead of using the variables directly (#1221) --- .../src/joint_trajectory_controller.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index ddcf57f508..faa2efce9b 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -43,9 +43,10 @@ JointTrajectoryController::JointTrajectoryController() controller_interface::CallbackReturn JointTrajectoryController::on_init() { - if (!urdf_.empty()) + const std::string & urdf = get_robot_description(); + if (!urdf.empty()) { - if (!model_.initString(urdf_)) + if (!model_.initString(urdf)) { RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse URDF file"); } @@ -701,7 +702,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( joints_angle_wraparound_.resize(dof_); for (size_t i = 0; i < dof_; ++i) { - if (!urdf_.empty()) + if (!get_robot_description().empty()) { auto urdf_joint = model_.getJoint(params_.joints[i]); if (urdf_joint && urdf_joint->type == urdf::Joint::CONTINUOUS) From abadffae4f8d4420e3cd36e52d502ed7d09b418c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 21 Jul 2024 19:28:44 +0200 Subject: [PATCH 30/97] Add missing includes (#1226) --- .../test/test_ackermann_steering_controller.hpp | 2 ++ .../test/test_bicycle_steering_controller.hpp | 2 ++ diff_drive_controller/test/test_diff_drive_controller.cpp | 2 ++ .../test/test_force_torque_sensor_broadcaster.cpp | 2 ++ imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp | 2 ++ joint_state_broadcaster/test/test_joint_state_broadcaster.cpp | 2 ++ pid_controller/test/test_pid_controller.hpp | 2 ++ range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp | 2 ++ .../test/test_steering_controllers_library.hpp | 2 ++ tricycle_controller/test/test_tricycle_controller.cpp | 2 ++ .../test/test_tricycle_steering_controller.hpp | 2 ++ 11 files changed, 22 insertions(+) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index b0b0944de2..59637a072f 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -26,6 +26,8 @@ #include "ackermann_steering_controller/ackermann_steering_controller.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index bfcf9fa424..65f1691a3b 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -26,6 +26,8 @@ #include "bicycle_steering_controller/bicycle_steering_controller.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index bc664f0711..72ae4dbfd7 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -25,6 +25,8 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::HW_IF_POSITION; diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 1acddcb64a..1ea25520cc 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -24,6 +24,8 @@ #include "geometry_msgs/msg/wrench_stamped.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" using hardware_interface::LoanedStateInterface; diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 132bf4006e..67cf31f8a5 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -23,6 +23,8 @@ #include #include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" #include "sensor_msgs/msg/imu.hpp" diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 71e65a978d..7400d6bbb9 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -25,6 +25,8 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" #include "test_joint_state_broadcaster.hpp" diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 326393bac0..158b5d9147 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -29,6 +29,8 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "pid_controller/pid_controller.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index e8be5e3d0a..59d27ebc0c 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -21,6 +21,8 @@ #include "test_range_sensor_broadcaster.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" using testing::IsEmpty; using testing::SizeIs; diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 8c4955b7eb..93ee823e0f 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -25,6 +25,8 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "steering_controllers_library/steering_controllers_library.hpp" diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index e56e3afacb..9d43c2590d 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -28,6 +28,8 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "tricycle_controller/tricycle_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 0fbea25abf..3b7a053937 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -25,6 +25,8 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "tricycle_steering_controller/tricycle_steering_controller.hpp" From 609b86763822e3c1276643de0b1f4f2aeb90ce90 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 21 Jul 2024 23:28:34 +0200 Subject: [PATCH 31/97] [JTC] Refactor URDF Model parsing (#1227) --- joint_trajectory_controller/CMakeLists.txt | 4 ++ .../joint_trajectory_controller.hpp | 3 - .../src/joint_trajectory_controller.cpp | 63 +++++++++---------- .../test/test_trajectory_controller.cpp | 7 +++ .../test/test_trajectory_controller_utils.hpp | 8 ++- 5 files changed, 47 insertions(+), 38 deletions(-) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 46c245be9c..438661f7bf 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -59,10 +59,12 @@ if(BUILD_TESTING) ament_add_gmock(test_trajectory test/test_trajectory.cpp) target_link_libraries(test_trajectory joint_trajectory_controller) + ament_target_dependencies(test_trajectory ros2_control_test_assets) target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES) ament_add_gmock(test_tolerances test/test_tolerances.cpp) target_link_libraries(test_tolerances joint_trajectory_controller) + ament_target_dependencies(test_tolerances ros2_control_test_assets) target_compile_definitions(test_tolerances PRIVATE _USE_MATH_DEFINES) ament_add_gmock(test_trajectory_controller @@ -71,6 +73,7 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory_controller joint_trajectory_controller ) + ament_target_dependencies(test_trajectory_controller ros2_control_test_assets) target_compile_definitions(joint_trajectory_controller PRIVATE _USE_MATH_DEFINES) ament_add_gmock(test_load_joint_trajectory_controller @@ -93,6 +96,7 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory_actions joint_trajectory_controller ) + ament_target_dependencies(test_trajectory_actions ros2_control_test_assets) endif() 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 e22dee8946..2f0a1f4df0 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 @@ -41,7 +41,6 @@ #include "realtime_tools/realtime_server_goal_handle.h" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" -#include "urdf/model.h" // auto-generated by generate_parameter_library #include "joint_trajectory_controller_parameters.hpp" @@ -300,8 +299,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void resize_joint_trajectory_point_command( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); - urdf::Model model_; - /** * @brief Assigns the values from a trajectory point interface to a joint interface. * diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index faa2efce9b..9563568ad5 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -43,15 +43,44 @@ JointTrajectoryController::JointTrajectoryController() controller_interface::CallbackReturn JointTrajectoryController::on_init() { + try + { + // 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) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + const std::string & urdf = get_robot_description(); if (!urdf.empty()) { - if (!model_.initString(urdf)) + urdf::Model model; + if (!model.initString(urdf)) { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse URDF file"); + RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse robot description!"); + return CallbackReturn::ERROR; } else { + /// initialize the URDF model and update the joint angles wraparound vector + // Configure joint position error normalization (angle_wraparound) + joints_angle_wraparound_.resize(params_.joints.size(), false); + for (size_t i = 0; i < params_.joints.size(); ++i) + { + auto urdf_joint = model.getJoint(params_.joints[i]); + if (urdf_joint && urdf_joint->type == urdf::Joint::CONTINUOUS) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "joint '%s' is of type continuous, use angle_wraparound.", + params_.joints[i].c_str()); + joints_angle_wraparound_[i] = true; + } + // do nothing if joint is not found in the URDF + } RCLCPP_DEBUG(get_node()->get_logger(), "Successfully parsed URDF file"); } } @@ -61,18 +90,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() RCLCPP_DEBUG(get_node()->get_logger(), "No URDF file given"); } - try - { - // 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) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - return CallbackReturn::SUCCESS; } @@ -698,24 +715,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( update_pids(); } - // Configure joint position error normalization (angle_wraparound) - joints_angle_wraparound_.resize(dof_); - for (size_t i = 0; i < dof_; ++i) - { - if (!get_robot_description().empty()) - { - auto urdf_joint = model_.getJoint(params_.joints[i]); - if (urdf_joint && urdf_joint->type == urdf::Joint::CONTINUOUS) - { - RCLCPP_DEBUG( - logger, "joint '%s' is of type continuous, use angle_wraparound.", - params_.joints[i].c_str()); - joints_angle_wraparound_[i] = true; - } - // do nothing if joint is not found in the URDF - } - } - if (params_.state_interfaces.empty()) { RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index e83e74ef41..f188c0f04b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -39,6 +39,13 @@ using lifecycle_msgs::msg::State; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; +TEST_P(TrajectoryControllerTestParameterized, invalid_robot_description) +{ + ASSERT_EQ( + controller_interface::return_type::ERROR, + SetUpTrajectoryControllerLocal({}, "")); +} + TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { rclcpp::executors::MultiThreadedExecutor executor; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index a15b2e5bc5..796503c036 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -26,6 +26,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "joint_trajectory_controller/tolerances.hpp" +#include "ros2_control_test_assets/descriptions.hpp" namespace { @@ -249,7 +250,7 @@ class TrajectoryControllerTest : public ::testing::Test void SetUpTrajectoryController( rclcpp::Executor & executor, const std::vector & parameters = {}, - const std::string & urdf = "") + const std::string & urdf = ros2_control_test_assets::minimal_robot_urdf) { auto ret = SetUpTrajectoryControllerLocal(parameters, urdf); if (ret != controller_interface::return_type::OK) @@ -260,7 +261,8 @@ class TrajectoryControllerTest : public ::testing::Test } controller_interface::return_type SetUpTrajectoryControllerLocal( - const std::vector & parameters = {}, const std::string & urdf = "") + const std::vector & parameters = {}, + const std::string & urdf = ros2_control_test_assets::minimal_robot_urdf) { traj_controller_ = std::make_shared(); @@ -302,7 +304,7 @@ class TrajectoryControllerTest : public ::testing::Test const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, const std::vector initial_eff_joints = INITIAL_EFF_JOINTS, - const std::string & urdf = "") + const std::string & urdf = ros2_control_test_assets::minimal_robot_urdf) { auto has_nonzero_vel_param = std::find_if( From adcb88e8fa2d98b82f2dd7dcd8b690c9c0cb1085 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 23 Jul 2024 17:27:35 +0100 Subject: [PATCH 32/97] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 9 + admittance_controller/CHANGELOG.rst | 8 + bicycle_steering_controller/CHANGELOG.rst | 9 + diff_drive_controller/CHANGELOG.rst | 9 + effort_controllers/CHANGELOG.rst | 7 + force_torque_sensor_broadcaster/CHANGELOG.rst | 9 + forward_command_controller/CHANGELOG.rst | 7 + gripper_controllers/CHANGELOG.rst | 7 + imu_sensor_broadcaster/CHANGELOG.rst | 9 + joint_state_broadcaster/CHANGELOG.rst | 9 + joint_trajectory_controller/CHANGELOG.rst | 10 + parallel_gripper_controller/CHANGELOG.rst | 196 ++++++++++++++++++ parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 9 + position_controllers/CHANGELOG.rst | 7 + range_sensor_broadcaster/CHANGELOG.rst | 9 + ros2_controllers/CHANGELOG.rst | 3 + ros2_controllers_test_nodes/CHANGELOG.rst | 3 + rqt_joint_trajectory_controller/CHANGELOG.rst | 3 + steering_controllers_library/CHANGELOG.rst | 8 + tricycle_controller/CHANGELOG.rst | 8 + tricycle_steering_controller/CHANGELOG.rst | 9 + velocity_controllers/CHANGELOG.rst | 7 + 23 files changed, 356 insertions(+), 1 deletion(-) create mode 100644 parallel_gripper_controller/CHANGELOG.rst diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 21840061d4..7ecc861d1a 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 4ae5ea39f8..b13ca47d31 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 831aa5a2b3..dc98a4cfe7 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 00f777df67..9a57056434 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Remove duplicated call to rclcpp::shutdown in test (`#1220 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Noel Jiménez García, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 70cfaad021..d6e77f5151 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index eac8c9959a..eff0763f9b 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 17196fed65..08bdeca044 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 968a1088c2..fcfd574be7 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Unused header cleanup (`#1199 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Add parallel_gripper_controller, configure gripper speed and effort with hardware interface (`#1002 `_) +* Contributors: Henry Moore, Paul Gesel, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 5bacde755e..e0d338fdae 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index cc01373289..f4e5891353 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 047d232155..7ad799d30b 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] Refactor URDF Model parsing (`#1227 `_) +* Use the internal methods instead of using the variables directly (`#1221 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* [JTC] Fix test_tolerances_via_actions (`#1209 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * [JTC] Make goal_time_tolerance overwrite default value only if explicitly set (`#1192 `_) diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst new file mode 100644 index 0000000000..2e828ac6f8 --- /dev/null +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -0,0 +1,196 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package parallel_gripper_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* [GripperController] Fix failing tests (`#1210 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Add parallel_gripper_controller, configure gripper speed and effort with hardware interface (`#1002 `_) +* Contributors: Paul Gesel, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-02-12) +------------------ + +4.5.0 (2024-01-31) +------------------ + +4.4.0 (2024-01-11) +------------------ + +4.3.0 (2024-01-08) +------------------ + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-12-01) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.17.0 (2023-10-31) +------------------- + +3.16.0 (2023-09-20) +------------------- + +3.15.0 (2023-09-11) +------------------- + +3.14.0 (2023-08-16) +------------------- + +3.13.0 (2023-08-04) +------------------- + +3.12.0 (2023-07-18) +------------------- + +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +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/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index f189071617..b180e07fc5 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 2.32.0 + 4.11.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 8e45f042f0..9736cdf480 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index c9e2c749b5..873982845b 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 31458c07d6..b427c11c7d 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index a54d39b85d..b1c335bd26 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.11.0 (2024-07-09) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 78e18dffaa..be2c3c0e35 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 +----------- + 4.11.0 (2024-07-09) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 20450f3ca3..55af17d40b 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 +----------- + 4.11.0 (2024-07-09) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index d4f62b806c..7ccb978487 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * Fix steering controllers library kinematics (`#1150 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index fdde4481e2..869801a7bc 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 6f4ad5d8c1..79e0a71af0 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 614e023984..f7f4103cef 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + 4.11.0 (2024-07-09) ------------------- * added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) From e4b7827c7545c03aa977bbde92149606cb73e6a9 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 23 Jul 2024 17:27:54 +0100 Subject: [PATCH 33/97] 4.12.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_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 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/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 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 46 files changed, 68 insertions(+), 68 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 7ecc861d1a..8ce11f3a9e 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index dfa733515f..62e9a5a27b 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.11.0 + 4.12.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index b13ca47d31..390eae4878 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) * Fix WaitSet issue in tests (`#1206 `_) diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index d6e13b0634..2e056c12a7 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.11.0 + 4.12.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index dc98a4cfe7..ee689b77ae 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 7afcf53706..37a552cd7d 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.11.0 + 4.12.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 9a57056434..6fcf3e272c 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Remove duplicated call to rclcpp::shutdown in test (`#1220 `_) * Unused header cleanup (`#1199 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index d7fa06b762..a819df01e0 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.11.0 + 4.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 d6e77f5151..0f4271ba91 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Unused header cleanup (`#1199 `_) * Fix WaitSet issue in tests (`#1206 `_) * Fix parallel gripper controller CI (`#1202 `_) diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index a7744927a1..6f3438c70e 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.11.0 + 4.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 eff0763f9b..134faec535 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 ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 90fcb3a684..063239659b 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.11.0 + 4.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 08bdeca044..bfabe442ac 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Unused header cleanup (`#1199 `_) * Fix WaitSet issue in tests (`#1206 `_) * Fix parallel gripper controller CI (`#1202 `_) diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f4c02d8de7..7ff4e85139 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.11.0 + 4.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index fcfd574be7..3334033fa9 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Unused header cleanup (`#1199 `_) * Fix parallel gripper controller CI (`#1202 `_) * Add parallel_gripper_controller, configure gripper speed and effort with hardware interface (`#1002 `_) diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 72d2ec48f2..a61a650c4d 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.11.0 + 4.12.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index e0d338fdae..d73046d7e3 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 5f0e981605..5c3d3b1196 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.11.0 + 4.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 f4e5891353..8ec080072f 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 77bbe2ca00..a6bee4983a 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.11.0 + 4.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 7ad799d30b..043f71623e 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * [JTC] Refactor URDF Model parsing (`#1227 `_) * Use the internal methods instead of using the variables directly (`#1221 `_) * Unused header cleanup (`#1199 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index d32d4e3daf..3da60aa2c5 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.11.0 + 4.12.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 2e828ac6f8..56f5b21426 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * [GripperController] Fix failing tests (`#1210 `_) * Fix parallel gripper controller CI (`#1202 `_) * Add parallel_gripper_controller, configure gripper speed and effort with hardware interface (`#1002 `_) diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index b180e07fc5..2bceb44069 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 4.11.0 + 4.12.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 9736cdf480..c7a23dcba0 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 2a44c9ad1b..335f48f91c 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.11.0 + 4.12.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 873982845b..95bba79c9d 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Unused header cleanup (`#1199 `_) * Fix WaitSet issue in tests (`#1206 `_) * Fix parallel gripper controller CI (`#1202 `_) diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 33f0f0c15e..b3a3fc6ce8 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.11.0 + 4.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index b427c11c7d..762ba806ba 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index e4525f53fd..8ad0290ff7 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.11.0 + 4.12.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index b1c335bd26..52d97ab7d2 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- 4.11.0 (2024-07-09) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 24e4429ade..a01eb0921e 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.11.0 + 4.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 be2c3c0e35..2cc1bb37dd 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 ------------ +4.12.0 (2024-07-23) +------------------- 4.11.0 (2024-07-09) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 87af6e6695..12388322bc 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.11.0 + 4.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 91db299739..394c11dab5 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.11.0", + version="4.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 55af17d40b..d6e70f3a47 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 ------------ +4.12.0 (2024-07-23) +------------------- 4.11.0 (2024-07-09) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 85c44c58fe..4f5674760d 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 - 4.11.0 + 4.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 2588a47cc8..60d3944d61 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.11.0", + version="4.12.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 7ccb978487..04f10c3158 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 7d172d45a8..3d300ae22e 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.11.0 + 4.12.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 869801a7bc..890bdbb026 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Unused header cleanup (`#1199 `_) * Fix WaitSet issue in tests (`#1206 `_) diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 20310e6b9a..e7d8c3438a 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.11.0 + 4.12.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 79e0a71af0..6966dce4dc 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Add missing includes (`#1226 `_) * Change the subscription timeout in the tests to 5ms (`#1219 `_) * Unused header cleanup (`#1199 `_) diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 4aeb8a086f..50340a466b 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.11.0 + 4.12.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index f7f4103cef..9d52d6fd25 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.0 (2024-07-23) +------------------- * Unused header cleanup (`#1199 `_) * Fix WaitSet issue in tests (`#1206 `_) * Fix parallel gripper controller CI (`#1202 `_) diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index d128f2b45b..8a3385b76e 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.11.0 + 4.12.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 33e35f0cf330d51290c13405e5af04544c795fe5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nikola=20Banovi=C4=87?= Date: Wed, 31 Jul 2024 20:04:46 +0200 Subject: [PATCH 34/97] Fix admittance controller interface read/write logic (#1232) --- admittance_controller/src/admittance_controller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 6e29b574a2..a4b56d739c 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -476,13 +476,13 @@ void AdmittanceController::read_state_from_hardware( 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_) + 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_) + if (has_acceleration_state_interface_) { state_current.accelerations[joint_ind] = state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value(); @@ -528,15 +528,15 @@ void AdmittanceController::write_state_to_hardware( command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value( state_commanded.positions[joint_ind]); } - else if (has_velocity_command_interface_) + if (has_velocity_command_interface_) { command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value( - state_commanded.positions[joint_ind]); + state_commanded.velocities[joint_ind]); } - else if (has_acceleration_command_interface_) + if (has_acceleration_command_interface_) { command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value( - state_commanded.positions[joint_ind]); + state_commanded.accelerations[joint_ind]); } } last_commanded_ = state_commanded; From 9ab820522aa0ea5c7d1a6c93c3325545641b4408 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Thu, 1 Aug 2024 10:28:18 +0200 Subject: [PATCH 35/97] Bump version of pre-commit hooks (#1236) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 29862f70e4..6fadbbdace 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -37,7 +37,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.16.0 + rev: v3.17.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.28.6 + rev: 0.29.1 hooks: - id: check-github-workflows args: ["--verbose"] From fa301f43e6b85d364b42247d8f1b6594cc5ce6c1 Mon Sep 17 00:00:00 2001 From: Mateus Menezes Date: Mon, 5 Aug 2024 03:45:17 -0300 Subject: [PATCH 36/97] Fix wrong method name in write new controller doc (#1240) Due to https://github.com/ros-controls/ros2_control/pull/1169 --- doc/writing_new_controller.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 1a9ffce714..5aa7068b98 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -42,7 +42,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 5. Add a constructor without parameters and the following public methods overriding the ``ControllerInterface`` definition: ``on_init``, ``command_interface_configuration``, ``state_interface_configuration``, ``on_configure``, ``on_activate``, ``on_deactivate``, ``update``. For exact definitions check the ``controller_interface/controller_interface.hpp`` header or one of the controllers from `ros2_controllers `_. - 6. (Optional) The NodeOptions of the LifecycleNode can be personalized by overriding the default method ``get_node_options``. + 6. (Optional) The NodeOptions of the LifecycleNode can be personalized by overriding the default method ``define_custom_node_options``. 7. (Optional) Often, controllers accept lists of joint names and interface names as parameters. If so, you can add two protected string vectors to store those values. From 0401dd4b4a4cc671d3a1f5bc61b44da2875f017e Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 14 Aug 2024 12:42:00 +0100 Subject: [PATCH 37/97] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 +++ admittance_controller/CHANGELOG.rst | 5 +++++ bicycle_steering_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 | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 3 +++ joint_state_broadcaster/CHANGELOG.rst | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 3 +++ parallel_gripper_controller/CHANGELOG.rst | 3 +++ pid_controller/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 3 +++ tricycle_steering_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 22 files changed, 68 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 8ce11f3a9e..e2e4427d0d 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 390eae4878..d716b154cc 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix admittance controller interface read/write logic (`#1232 `_) +* Contributors: Nikola Banović + 4.12.0 (2024-07-23) ------------------- * Change the subscription timeout in the tests to 5ms (`#1219 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index ee689b77ae..517a1451b7 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 6fcf3e272c..1bba757ac8 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 0f4271ba91..61e46afca4 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * Unused header cleanup (`#1199 `_) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 134faec535..2692b7936d 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 +----------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index bfabe442ac..b727c39b9e 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * Unused header cleanup (`#1199 `_) diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 3334033fa9..e02268991e 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * Unused header cleanup (`#1199 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index d73046d7e3..92b9ba0bbf 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 8ec080072f..c441096538 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 043f71623e..394756ab59 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * [JTC] Refactor URDF Model parsing (`#1227 `_) diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 56f5b21426..542bed69ca 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * [GripperController] Fix failing tests (`#1210 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index c7a23dcba0..2e9e795878 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 95bba79c9d..28369f72b5 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * Unused header cleanup (`#1199 `_) diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 762ba806ba..8308f75c7d 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 52d97ab7d2..8b4143c687 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 2cc1bb37dd..aaed2ebc31 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 +----------- + 4.12.0 (2024-07-23) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index d6e70f3a47..808c4c95f5 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 +----------- + 4.12.0 (2024-07-23) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 04f10c3158..df6da55e96 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 890bdbb026..e8b8d9b926 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 6966dce4dc..f6ded4c07a 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 9d52d6fd25..3f8c27d1d4 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.0 (2024-07-23) ------------------- * Unused header cleanup (`#1199 `_) From 298df4babc49f24825e9c8e350ff6345b9f85e45 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 14 Aug 2024 12:42:19 +0100 Subject: [PATCH 38/97] 4.12.1 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_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 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/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 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 46 files changed, 68 insertions(+), 68 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index e2e4427d0d..d156aac14a 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 62e9a5a27b..1ab37fa944 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.12.0 + 4.12.1 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index d716b154cc..27944504fa 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- * Fix admittance controller interface read/write logic (`#1232 `_) * Contributors: Nikola Banović diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 2e056c12a7..75a2827417 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.12.0 + 4.12.1 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 517a1451b7..ff5a8c13c7 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 37a552cd7d..819280a96b 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.12.0 + 4.12.1 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 1bba757ac8..a2a3a23ee2 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index a819df01e0..4f77e3a919 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.12.0 + 4.12.1 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 61e46afca4..23b1c687d8 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 6f3438c70e..0cea01d743 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.12.0 + 4.12.1 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 2692b7936d..5c8f0da1bc 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 ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 063239659b..3973cd91cd 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.12.0 + 4.12.1 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 b727c39b9e..24232cac8a 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 7ff4e85139..1b996a1b5e 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.12.0 + 4.12.1 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index e02268991e..add7f90438 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index a61a650c4d..47e809afae 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.12.0 + 4.12.1 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 92b9ba0bbf..a8d07e2caf 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 5c3d3b1196..6efaea8076 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.12.0 + 4.12.1 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 c441096538..802f2f8b90 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index a6bee4983a..336c04f2c7 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.12.0 + 4.12.1 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 394756ab59..aefe4f1a68 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 3da60aa2c5..edfef38b39 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.12.0 + 4.12.1 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 542bed69ca..96b3d90756 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 2bceb44069..98b9cd1b14 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 4.12.0 + 4.12.1 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 2e9e795878..05cfd65c65 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 335f48f91c..6148c43b94 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.12.0 + 4.12.1 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 28369f72b5..caa2319219 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index b3a3fc6ce8..5b05352a21 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.12.0 + 4.12.1 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 8308f75c7d..13b29cbd8f 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 8ad0290ff7..1271dd9db1 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.12.0 + 4.12.1 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 8b4143c687..5122cc1d98 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index a01eb0921e..c6791dd0a5 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.12.0 + 4.12.1 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 aaed2ebc31..bbd2aaf3d0 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 ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 12388322bc..6d7d362847 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.12.0 + 4.12.1 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 394c11dab5..9b2a14c367 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.12.0", + version="4.12.1", 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 808c4c95f5..c167335f3d 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 ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 4f5674760d..13f6136350 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 - 4.12.0 + 4.12.1 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 60d3944d61..3d10574eec 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.12.0", + version="4.12.1", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index df6da55e96..47cbebc6ce 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 3d300ae22e..73c2bf8198 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.12.0 + 4.12.1 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index e8b8d9b926..0424e7f4f5 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index e7d8c3438a..0e87370142 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.12.0 + 4.12.1 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index f6ded4c07a..ae42dff585 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 50340a466b..249d2041e7 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.12.0 + 4.12.1 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 3f8c27d1d4..be0f90301e 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.12.1 (2024-08-14) +------------------- 4.12.0 (2024-07-23) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 8a3385b76e..03283cbc14 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.12.0 + 4.12.1 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 4a6456fb6877c10494ddea78b57e23053142c38e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 15 Aug 2024 13:02:01 +0200 Subject: [PATCH 39/97] [Joint State Broadcaster] Publish the joint_states of joints present in the URDF (#1233) --- joint_state_broadcaster/CMakeLists.txt | 2 + joint_state_broadcaster/package.xml | 1 + .../src/joint_state_broadcaster.cpp | 19 +- .../joint_state_broadcaster_parameters.yaml | 7 + .../test/test_joint_state_broadcaster.cpp | 219 +++++++++++++++++- .../test/test_joint_state_broadcaster.hpp | 8 +- 6 files changed, 248 insertions(+), 8 deletions(-) diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index 5c383897cc..cc8dc18bf6 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -16,6 +16,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rcutils realtime_tools sensor_msgs + urdf ) find_package(ament_cmake REQUIRED) @@ -72,6 +73,7 @@ if(BUILD_TESTING) ) ament_target_dependencies(test_joint_state_broadcaster hardware_interface + ros2_control_test_assets ) endif() diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 336c04f2c7..aa66a9641e 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -22,6 +22,7 @@ rcutils realtime_tools sensor_msgs + urdf 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 5eb0a9b9b6..209b44e557 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -25,6 +25,7 @@ #include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" #include "std_msgs/msg/header.hpp" +#include "urdf/model.h" namespace rclcpp_lifecycle { @@ -242,6 +243,17 @@ bool JointStateBroadcaster::init_joint_data() name_if_value_mapping_[si->get_prefix_name()][interface_name] = kUninitializedValue; } + const std::string & urdf = get_robot_description(); + + urdf::Model model; + const bool is_model_loaded = !urdf.empty() && model.initString(urdf); + if (!is_model_loaded) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to parse robot description. Will publish all the interfaces with '%s', '%s' and '%s'", + HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT); + } // filter state interfaces that have at least one of the joint_states fields, // the rest will be ignored for this message for (const auto & name_ifv : name_if_value_mapping_) @@ -249,7 +261,12 @@ bool JointStateBroadcaster::init_joint_data() const auto & interfaces_and_values = name_ifv.second; if (has_any_key(interfaces_and_values, {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT})) { - joint_names_.push_back(name_ifv.first); + if ( + !params_.use_urdf_to_filter || !params_.joints.empty() || !is_model_loaded || + model.getJoint(name_ifv.first)) + { + joint_names_.push_back(name_ifv.first); + } } } diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml index 8f0d4da6c5..c8ca928a12 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml +++ b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml @@ -39,3 +39,10 @@ joint_state_broadcaster: type: string, default_value: "effort", } + use_urdf_to_filter: { + type: bool, + default_value: true, + description: "Uses the robot_description to filter the ``joint_states`` topic. + If true, the broadcaster will publish the data of the joints present in the URDF alone. + If false, the broadcaster will publish the data of any interface that has type ``position``, ``velocity``, or ``effort``." + } diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 7400d6bbb9..877d199419 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -28,6 +28,7 @@ #include "rclcpp/executor.hpp" #include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" #include "test_joint_state_broadcaster.hpp" using hardware_interface::HW_IF_EFFORT; @@ -61,15 +62,17 @@ void JointStateBroadcasterTest::TearDown() { state_broadcaster_.reset(nullptr); void JointStateBroadcasterTest::SetUpStateBroadcaster( const std::vector & joint_names, const std::vector & interfaces) { - init_broadcaster_and_set_parameters(joint_names, interfaces); + init_broadcaster_and_set_parameters("", joint_names, interfaces); assign_state_interfaces(joint_names, interfaces); } void JointStateBroadcasterTest::init_broadcaster_and_set_parameters( - const std::vector & joint_names, const std::vector & interfaces) + const std::string & robot_description, const std::vector & joint_names, + const std::vector & interfaces) { const auto result = state_broadcaster_->init( - "joint_state_broadcaster", "", 0, "", state_broadcaster_->define_custom_node_options()); + "joint_state_broadcaster", robot_description, 0, "", + state_broadcaster_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); state_broadcaster_->get_node()->set_parameter({"joints", joint_names}); @@ -262,6 +265,212 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) ElementsAreArray(interface_names_)); } +TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF) +{ + const std::vector JOINT_NAMES = {}; + const std::vector IF_NAMES = {interface_names_[0]}; + init_broadcaster_and_set_parameters("", JOINT_NAMES, IF_NAMES); + assign_state_interfaces(JOINT_NAMES, IF_NAMES); + + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = joint_names_.size(); + + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + + // dynamic joint state initialized + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); +} + +TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDescription) +{ + const std::vector JOINT_NAMES = {}; + const std::vector IF_NAMES = {interface_names_[0]}; + + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) + + ros2_control_test_assets::hardware_resources + ros2_control_test_assets::urdf_tail; + const std::vector joint_in_urdf({"joint1", "joint2"}); + init_broadcaster_and_set_parameters(urdf_to_test, JOINT_NAMES, IF_NAMES); + assign_state_interfaces(JOINT_NAMES, IF_NAMES); + + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = joint_in_urdf.size(); + + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_in_urdf)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + + // dynamic joint state initialized and it will have the data of all the interfaces + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(joint_names_.size())); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(joint_names_.size())); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); +} + +TEST_F(JointStateBroadcasterTest, ActivateTestWithJointsAndNoInterfaces) +{ + const std::vector JOINT_NAMES = {"joint1"}; + const std::vector IF_NAMES = {}; + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) + + ros2_control_test_assets::hardware_resources + ros2_control_test_assets::urdf_tail; + const std::vector joint_in_urdf({"joint1", "joint2"}); + init_broadcaster_and_set_parameters(urdf_to_test, JOINT_NAMES, IF_NAMES); + assign_state_interfaces(JOINT_NAMES, IF_NAMES); + + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = joint_in_urdf.size(); + + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_in_urdf)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + + // dynamic joint state initialized and it will have the data of all the interfaces + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(joint_names_.size())); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(joint_names_.size())); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); +} + +TEST_F(JointStateBroadcasterTest, ActivateTestWithJointsAndInterfaces) +{ + const std::vector JOINT_NAMES = {"joint1"}; + const std::vector IF_NAMES = interface_names_; + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) + + ros2_control_test_assets::hardware_resources + ros2_control_test_assets::urdf_tail; + init_broadcaster_and_set_parameters(urdf_to_test, JOINT_NAMES, IF_NAMES); + assign_state_interfaces(JOINT_NAMES, IF_NAMES); + + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = JOINT_NAMES.size(); + + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + + // dynamic joint state initialized and it will have the data of all the interfaces + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); +} + TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) { const std::vector JOINT_NAMES = {"joint1"}; @@ -426,7 +635,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing) const std::vector JOINT_NAMES = {joint_names_[0], joint_names_[1]}; const std::vector IF_NAMES = {interface_names_[0], interface_names_[1]}; - init_broadcaster_and_set_parameters(JOINT_NAMES, {interface_names_[0], interface_names_[1]}); + init_broadcaster_and_set_parameters("", JOINT_NAMES, {interface_names_[0], interface_names_[1]}); // assign state with interfaces which are not set in parameters --> We should actually not assign // anything because CM will also not do that @@ -444,7 +653,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) const std::vector JOINT_NAMES = {joint_names_[0], joint_names_[1]}; const std::vector IF_NAMES = {interface_names_[0], interface_names_[1]}; - init_broadcaster_and_set_parameters(JOINT_NAMES, {interface_names_[0], interface_names_[1]}); + init_broadcaster_and_set_parameters("", JOINT_NAMES, {interface_names_[0], interface_names_[1]}); // Manually assign existing interfaces --> one we need is missing std::vector state_ifs; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 3e54116d5c..0b4c50e89e 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -35,6 +35,10 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest); FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyTest); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDescription); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithJointsAndNoInterfaces); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithJointsAndInterfaces); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter); FRIEND_TEST(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces); @@ -60,8 +64,8 @@ class JointStateBroadcasterTest : public ::testing::Test const std::vector & interfaces = {}); void init_broadcaster_and_set_parameters( - const std::vector & joint_names = {}, - const std::vector & interfaces = {}); + const std::string & robot_description, const std::vector & joint_names, + const std::vector & interfaces); void assign_state_interfaces( const std::vector & joint_names = {}, From 4ab22a51456abee127aeffbbd8ef0c54ac70c92a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 22 Aug 2024 13:03:47 +0200 Subject: [PATCH 40/97] Fixes tests to work with use_global_arguments NodeOptions parameter (#1256) --- ackermann_steering_controller/CMakeLists.txt | 6 ++---- .../test_load_ackermann_steering_controller.cpp | 13 ++++++++----- admittance_controller/CMakeLists.txt | 6 ++---- .../test/test_load_admittance_controller.cpp | 9 ++++++--- bicycle_steering_controller/CMakeLists.txt | 6 ++---- .../test/test_load_bicycle_steering_controller.cpp | 12 ++++++++---- diff_drive_controller/CMakeLists.txt | 6 ++---- .../test/test_load_diff_drive_controller.cpp | 10 +++++++--- force_torque_sensor_broadcaster/CMakeLists.txt | 5 ++--- .../test_load_force_torque_sensor_broadcaster.cpp | 14 +++++++++----- imu_sensor_broadcaster/CMakeLists.txt | 5 ++--- .../test/test_load_imu_sensor_broadcaster.cpp | 11 +++++++---- parallel_gripper_controller/CMakeLists.txt | 5 ++--- ...est_load_parallel_gripper_action_controller.cpp | 13 ++++++++----- range_sensor_broadcaster/CMakeLists.txt | 5 ++--- .../test/test_load_range_sensor_broadcaster.cpp | 11 +++++++---- tricycle_controller/CMakeLists.txt | 6 ++---- .../test/test_load_tricycle_controller.cpp | 9 ++++++--- tricycle_steering_controller/CMakeLists.txt | 6 ++---- .../test_load_tricycle_steering_controller.cpp | 13 ++++++++----- 20 files changed, 94 insertions(+), 77 deletions(-) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index a34be3c672..83edf7c157 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -58,10 +58,8 @@ if(BUILD_TESTING) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_ackermann_steering_controller - test/test_load_ackermann_steering_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_ackermann_steering_controller test/test_load_ackermann_steering_controller.cpp) ament_target_dependencies(test_load_ackermann_steering_controller controller_manager hardware_interface diff --git a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp index 8e81643835..e7b56e2788 100644 --- a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -29,12 +29,15 @@ TEST(TestLoadAckermannSteeringController, load_controller) controller_manager::ControllerManager cm( executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/ackermann_steering_controller_params.yaml"; - ASSERT_NE( - cm.load_controller( - "test_ackermann_steering_controller", - "ackermann_steering_controller/AckermannSteeringController"), - nullptr); + cm.set_parameter({"test_ackermann_steering_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_ackermann_steering_controller.type", + "ackermann_steering_controller/AckermannSteeringController"}); + + ASSERT_NE(cm.load_controller("test_ackermann_steering_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 10ca5caa40..cd16714a31 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -68,10 +68,8 @@ if(BUILD_TESTING) 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 - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp) ament_target_dependencies(test_load_admittance_controller controller_manager hardware_interface diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp index ea4c14caac..5006920858 100644 --- a/admittance_controller/test/test_load_admittance_controller.cpp +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -30,10 +30,13 @@ TEST(TestLoadAdmittanceController, load_controller) controller_manager::ControllerManager cm( executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = std::string(TEST_FILES_DIRECTORY) + "/test_params.yaml"; - ASSERT_EQ( - cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController"), - nullptr); + cm.set_parameter({"load_admittance_controller.params_file", test_file_path}); + cm.set_parameter( + {"load_admittance_controller.type", "admittance_controller/AdmittanceController"}); + + ASSERT_EQ(cm.load_controller("load_admittance_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index 6535a73260..db1f5cf4ba 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -57,10 +57,8 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_bicycle_steering_controller - test/test_load_bicycle_steering_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_params.yaml - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_bicycle_steering_controller test/test_load_bicycle_steering_controller.cpp) ament_target_dependencies(test_load_bicycle_steering_controller controller_manager hardware_interface diff --git a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp index 7be5e2dd1d..4d17a4bdd7 100644 --- a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp @@ -29,11 +29,15 @@ TEST(TestLoadBicycleSteeringController, load_controller) controller_manager::ControllerManager cm( executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/bicycle_steering_controller_params.yaml"; - ASSERT_NE( - cm.load_controller( - "test_bicycle_steering_controller", "bicycle_steering_controller/BicycleSteeringController"), - nullptr); + cm.set_parameter({"test_bicycle_steering_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_bicycle_steering_controller.type", + "bicycle_steering_controller/BicycleSteeringController"}); + + ASSERT_NE(cm.load_controller("test_bicycle_steering_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 70a54e302c..4eeb98b9d2 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -70,10 +70,8 @@ if(BUILD_TESTING) tf2_msgs ) - add_rostest_with_parameters_gmock(test_load_diff_drive_controller - test/test_load_diff_drive_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_diff_drive_controller test/test_load_diff_drive_controller.cpp) ament_target_dependencies(test_load_diff_drive_controller controller_manager ros2_control_test_assets diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index aa23f83ec9..302bdf6924 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -29,10 +29,14 @@ TEST(TestLoadDiffDriveController, load_controller) controller_manager::ControllerManager cm( executor, ros2_control_test_assets::diffbot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/config/test_diff_drive_controller.yaml"; - ASSERT_NE( - cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), - nullptr); + cm.set_parameter({"test_diff_drive_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_diff_drive_controller.type", "diff_drive_controller/DiffDriveController"}); + + ASSERT_NE(cm.load_controller("test_diff_drive_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 38c984192e..02c343f050 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -53,9 +53,8 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_force_torque_sensor_broadcaster - test/test_load_force_torque_sensor_broadcaster.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/force_torque_sensor_broadcaster_params.yaml) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + 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 diff --git a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp index f8b367d341..b5887f09e1 100644 --- a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp @@ -34,11 +34,15 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) controller_manager::ControllerManager cm( executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); - ASSERT_NE( - cm.load_controller( - "test_force_torque_sensor_broadcaster", - "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster"), - nullptr); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/force_torque_sensor_broadcaster_params.yaml"; + + cm.set_parameter({"test_force_torque_sensor_broadcaster.params_file", test_file_path}); + cm.set_parameter( + {"test_force_torque_sensor_broadcaster.type", + "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_force_torque_sensor_broadcaster"), nullptr); } int main(int argc, char ** argv) diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 6782012c65..5539dea9ff 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -53,9 +53,8 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_imu_sensor_broadcaster - test/test_load_imu_sensor_broadcaster.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/imu_sensor_broadcaster_params.yaml) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + 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 diff --git a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp index fa2afab200..eca29082c1 100644 --- a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp @@ -33,11 +33,14 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) controller_manager::ControllerManager cm( executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/imu_sensor_broadcaster_params.yaml"; - ASSERT_NE( - cm.load_controller( - "test_imu_sensor_broadcaster", "imu_sensor_broadcaster/IMUSensorBroadcaster"), - nullptr); + cm.set_parameter({"test_imu_sensor_broadcaster.params_file", test_file_path}); + cm.set_parameter( + {"test_imu_sensor_broadcaster.type", "imu_sensor_broadcaster/IMUSensorBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_imu_sensor_broadcaster"), nullptr); } int main(int argc, char ** argv) diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt index 75380a953f..cb0f4fafc9 100644 --- a/parallel_gripper_controller/CMakeLists.txt +++ b/parallel_gripper_controller/CMakeLists.txt @@ -48,9 +48,8 @@ if(BUILD_TESTING) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_parallel_gripper_action_controllers - test/test_load_parallel_gripper_action_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/gripper_action_controller_params.yaml) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_parallel_gripper_action_controllers test/test_load_parallel_gripper_action_controller.cpp) target_include_directories(test_load_parallel_gripper_action_controllers PRIVATE include) ament_target_dependencies(test_load_parallel_gripper_action_controllers controller_manager diff --git a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp index 35a10a8c31..6453539fa0 100644 --- a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp +++ b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp @@ -27,12 +27,15 @@ TEST(TestLoadGripperActionControllers, load_controller) controller_manager::ControllerManager cm( executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/gripper_action_controller_params.yaml"; - ASSERT_NE( - cm.load_controller( - "test_gripper_action_position_controller", - "parallel_gripper_action_controller/GripperActionController"), - nullptr); + cm.set_parameter({"test_gripper_action_position_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_gripper_action_position_controller.type", + "parallel_gripper_action_controller/GripperActionController"}); + + ASSERT_NE(cm.load_controller("test_gripper_action_position_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/range_sensor_broadcaster/CMakeLists.txt b/range_sensor_broadcaster/CMakeLists.txt index d7002d1b17..6f21607c9c 100644 --- a/range_sensor_broadcaster/CMakeLists.txt +++ b/range_sensor_broadcaster/CMakeLists.txt @@ -59,9 +59,8 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_range_sensor_broadcaster - test/test_load_range_sensor_broadcaster.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/range_sensor_broadcaster_params.yaml) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_range_sensor_broadcaster test/test_load_range_sensor_broadcaster.cpp) target_include_directories(test_load_range_sensor_broadcaster PRIVATE include) target_link_libraries(test_load_range_sensor_broadcaster range_sensor_broadcaster diff --git a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp index b416ac4ef9..11aae67f5d 100644 --- a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp @@ -33,11 +33,14 @@ TEST(TestLoadRangeSensorBroadcaster, load_controller) controller_manager::ControllerManager cm( executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/range_sensor_broadcaster_params.yaml"; - ASSERT_NE( - cm.load_controller( - "test_range_sensor_broadcaster", "range_sensor_broadcaster/RangeSensorBroadcaster"), - nullptr); + cm.set_parameter({"test_range_sensor_broadcaster.params_file", test_file_path}); + cm.set_parameter( + {"test_range_sensor_broadcaster.type", "range_sensor_broadcaster/RangeSensorBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_range_sensor_broadcaster"), nullptr); } int main(int argc, char ** argv) diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index e264c1f258..6834b753a0 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -76,10 +76,8 @@ if(BUILD_TESTING) tf2_msgs ) - add_rostest_with_parameters_gmock(test_load_tricycle_controller - test/test_load_tricycle_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_tricycle_controller test/test_load_tricycle_controller.cpp) target_link_libraries(test_load_tricycle_controller tricycle_controller ) diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index 486c04515b..7f7f0e6fed 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -33,10 +33,13 @@ TEST(TestLoadTricycleController, load_controller) controller_manager::ControllerManager cm( executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/config/test_tricycle_controller.yaml"; - ASSERT_NE( - cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController"), - nullptr); + cm.set_parameter({"test_tricycle_controller.params_file", test_file_path}); + cm.set_parameter({"test_tricycle_controller.type", "tricycle_controller/TricycleController"}); + + ASSERT_NE(cm.load_controller("test_tricycle_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index 92c2782092..24b4cd1a22 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -57,10 +57,8 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_tricycle_steering_controller - test/test_load_tricycle_steering_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_params.yaml - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_tricycle_steering_controller test/test_load_tricycle_steering_controller.cpp) ament_target_dependencies(test_load_tricycle_steering_controller controller_manager hardware_interface diff --git a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp index 210153eef8..d3579c902f 100644 --- a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -29,12 +29,15 @@ TEST(TestLoadTricycleSteeringController, load_controller) controller_manager::ControllerManager cm( executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/tricycle_steering_controller_params.yaml"; - ASSERT_NE( - cm.load_controller( - "test_tricycle_steering_controller", - "tricycle_steering_controller/TricycleSteeringController"), - nullptr); + cm.set_parameter({"test_tricycle_steering_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_tricycle_steering_controller.type", + "tricycle_steering_controller/TricycleSteeringController"}); + + ASSERT_NE(cm.load_controller("test_tricycle_steering_controller"), nullptr); } int main(int argc, char ** argv) From 31f7fbebf023de7cd8bfc2be5117f39bfa77af42 Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Thu, 22 Aug 2024 16:02:58 +0200 Subject: [PATCH 41/97] Fix segfault at reconfigure of AdmittanceController (#1248) --- .../include/admittance_controller/admittance_rule_impl.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 77f1277b35..cab8b4cf45 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -46,11 +46,17 @@ controller_interface::return_type AdmittanceRule::configure( { try { + // Make sure we destroy the interface first. Otherwise we might run into a segfault + if (kinematics_loader_) + { + kinematics_.reset(); + } 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)) { From cdfc0af5a4ee09c17c2fffa2c732dc79dd9c669f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 22 Aug 2024 16:07:09 +0200 Subject: [PATCH 42/97] Also test if python files were changed (#1264) --- .github/workflows/humble-abi-compatibility.yml | 3 ++- .github/workflows/humble-binary-build.yml | 6 ++++-- .github/workflows/humble-coverage-build.yml | 6 ++++-- .github/workflows/humble-debian-build.yml | 3 ++- .github/workflows/humble-rhel-semi-binary-build.yml | 3 ++- .github/workflows/humble-semi-binary-build.yml | 6 ++++-- .github/workflows/humble-source-build.yml | 2 ++ .github/workflows/iron-binary-build.yml | 6 ++++-- .github/workflows/iron-coverage-build.yml | 2 ++ .github/workflows/iron-debian-build.yml | 1 + .github/workflows/iron-semi-binary-build.yml | 1 + .github/workflows/iron-source-build.yml | 1 + .github/workflows/jazzy-binary-build.yml | 1 + .github/workflows/jazzy-debian-build.yml | 1 + .github/workflows/jazzy-rhel-semi-binary-build.yml | 1 + .github/workflows/jazzy-semi-binary-build.yml | 2 ++ .github/workflows/jazzy-source-build.yml | 1 + .github/workflows/rolling-binary-build.yml | 3 +++ .github/workflows/rolling-coverage-build.yml | 2 ++ .github/workflows/rolling-debian-build.yml | 1 + .github/workflows/rolling-rhel-semi-binary-build.yml | 1 + .github/workflows/rolling-semi-binary-build.yml | 2 ++ .github/workflows/rolling-source-build.yml | 1 + 23 files changed, 45 insertions(+), 11 deletions(-) diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index d72d3725b3..247371ba7d 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -8,8 +8,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/humble-abi-compatibility.yml' + - '**.py' - '**.yaml' + - '.github/workflows/humble-abi-compatibility.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.humble.repos' diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml index a78f6135b6..a031d823cf 100644 --- a/.github/workflows/humble-binary-build.yml +++ b/.github/workflows/humble-binary-build.yml @@ -11,8 +11,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/humble-binary-build.yml' + - '**.py' - '**.yaml' + - '.github/workflows/humble-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.humble.repos' @@ -23,8 +24,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/humble-binary-build.yml' + - '**.py' - '**.yaml' + - '.github/workflows/humble-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.humble.repos' diff --git a/.github/workflows/humble-coverage-build.yml b/.github/workflows/humble-coverage-build.yml index f39758a512..d0dcb9c350 100644 --- a/.github/workflows/humble-coverage-build.yml +++ b/.github/workflows/humble-coverage-build.yml @@ -8,9 +8,10 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' + - '**.yaml' - '.github/workflows/humble-coverage-build.yml' - 'codecov.yml' - - '**.yaml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' @@ -21,9 +22,10 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' + - '**.yaml' - '.github/workflows/humble-coverage-build.yml' - 'codecov.yml' - - '**.yaml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index b018fcea84..cd25248caf 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -8,8 +8,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/humble-debian-build.yml' + - '**.py' - '**.yaml' + - '.github/workflows/humble-debian-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' diff --git a/.github/workflows/humble-rhel-semi-binary-build.yml b/.github/workflows/humble-rhel-semi-binary-build.yml index 2a7814f8b3..25c755ada5 100644 --- a/.github/workflows/humble-rhel-semi-binary-build.yml +++ b/.github/workflows/humble-rhel-semi-binary-build.yml @@ -8,8 +8,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/humble-rhel-semi-binary-build.yml' + - '**.py' - '**.yaml' + - '.github/workflows/humble-rhel-semi-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index 72287fa115..4246d18160 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -10,8 +10,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/humble-semi-binary-build.yml' + - '**.py' - '**.yaml' + - '.github/workflows/humble-semi-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' @@ -22,8 +23,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/humble-semi-binary-build.yml' + - '**.py' - '**.yaml' + - '.github/workflows/humble-semi-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index 44d474ded4..94af6edd8c 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -8,6 +8,8 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' + - '**.yaml' - '.github/workflows/humble-source-build.yml' - '**/package.xml' - '**/CMakeLists.txt' diff --git a/.github/workflows/iron-binary-build.yml b/.github/workflows/iron-binary-build.yml index 802ca97bb8..0d2f0ff7f3 100644 --- a/.github/workflows/iron-binary-build.yml +++ b/.github/workflows/iron-binary-build.yml @@ -13,8 +13,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/iron-binary-build.yml' + - '**.py' - '**.yaml' + - '.github/workflows/iron-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.iron.repos' @@ -25,8 +26,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/iron-binary-build.yml' + - '**.py' - '**.yaml' + - '.github/workflows/iron-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.iron.repos' diff --git a/.github/workflows/iron-coverage-build.yml b/.github/workflows/iron-coverage-build.yml index bbc9c4e975..e1d2e84bc8 100644 --- a/.github/workflows/iron-coverage-build.yml +++ b/.github/workflows/iron-coverage-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/iron-coverage-build.yml' - 'codecov.yml' @@ -21,6 +22,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/iron-coverage-build.yml' - 'codecov.yml' diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml index 105eb29aba..ebd2f46c18 100644 --- a/.github/workflows/iron-debian-build.yml +++ b/.github/workflows/iron-debian-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/iron-debian-build.yml' - '**/package.xml' diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml index bf5edcaa56..3aca5e5b70 100644 --- a/.github/workflows/iron-semi-binary-build.yml +++ b/.github/workflows/iron-semi-binary-build.yml @@ -12,6 +12,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/iron-semi-binary-build.yml' - '**/package.xml' diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml index c460762259..c54ead7877 100644 --- a/.github/workflows/iron-source-build.yml +++ b/.github/workflows/iron-source-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/iron-source-build.yml' - '**/package.xml' diff --git a/.github/workflows/jazzy-binary-build.yml b/.github/workflows/jazzy-binary-build.yml index 6ec46eb6c1..cda4969abf 100644 --- a/.github/workflows/jazzy-binary-build.yml +++ b/.github/workflows/jazzy-binary-build.yml @@ -13,6 +13,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/jazzy-binary-build.yml' - '**/package.xml' diff --git a/.github/workflows/jazzy-debian-build.yml b/.github/workflows/jazzy-debian-build.yml index 61a47d0f2e..e3e3b8a353 100644 --- a/.github/workflows/jazzy-debian-build.yml +++ b/.github/workflows/jazzy-debian-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/jazzy-debian-build.yml' - '**/package.xml' diff --git a/.github/workflows/jazzy-rhel-semi-binary-build.yml b/.github/workflows/jazzy-rhel-semi-binary-build.yml index 94ddfeb54f..1c62fcf2ac 100644 --- a/.github/workflows/jazzy-rhel-semi-binary-build.yml +++ b/.github/workflows/jazzy-rhel-semi-binary-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/jazzy-rhel-semi-binary-build.yml' - '**/package.xml' diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index 7c5bdfc4dc..ffa4f914b9 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -12,6 +12,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/jazzy-semi-binary-build.yml' - '**/package.xml' @@ -24,6 +25,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/jazzy-semi-binary-build.yml' - '**/package.xml' diff --git a/.github/workflows/jazzy-source-build.yml b/.github/workflows/jazzy-source-build.yml index f77e9a5060..45f6d1d25e 100644 --- a/.github/workflows/jazzy-source-build.yml +++ b/.github/workflows/jazzy-source-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/jazzy-source-build.yml' - '**/package.xml' diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml index 529c13ac64..b512eb9db7 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build.yml @@ -13,6 +13,8 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' + - '**.yaml' - '.github/workflows/rolling-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' @@ -24,6 +26,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/rolling-binary-build.yml' - '**/package.xml' diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml index 3b4b195ff7..058ff9bb33 100644 --- a/.github/workflows/rolling-coverage-build.yml +++ b/.github/workflows/rolling-coverage-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/rolling-coverage-build.yml' - 'codecov.yml' @@ -21,6 +22,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/rolling-coverage-build.yml' - 'codecov.yml' diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index c1313d7d5d..d7efd3ea0a 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/rolling-debian-build.yml' - '**/package.xml' diff --git a/.github/workflows/rolling-rhel-semi-binary-build.yml b/.github/workflows/rolling-rhel-semi-binary-build.yml index 6b7fb6b8c4..29a53b810a 100644 --- a/.github/workflows/rolling-rhel-semi-binary-build.yml +++ b/.github/workflows/rolling-rhel-semi-binary-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/rolling-rhel-semi-binary-build.yml' - '**/package.xml' diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index db0cd33de5..872c509931 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -12,6 +12,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/rolling-semi-binary-build.yml' - '**/package.xml' @@ -24,6 +25,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/rolling-semi-binary-build.yml' - '**/package.xml' diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index 66e53d1da1..e19a1e8cf7 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/rolling-source-build.yml' - '**/package.xml' From f57eedd825874391ac076355a341a90e26e9a8ed Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 22 Aug 2024 19:50:49 +0100 Subject: [PATCH 43/97] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 5 +++++ admittance_controller/CHANGELOG.rst | 6 ++++++ bicycle_steering_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 | 3 +++ gripper_controllers/CHANGELOG.rst | 3 +++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 3 +++ parallel_gripper_controller/CHANGELOG.rst | 5 +++++ pid_controller/CHANGELOG.rst | 3 +++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 3 +++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 3 +++ 22 files changed, 89 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index d156aac14a..895bba5487 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + 4.12.1 (2024-08-14) ------------------- diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 27944504fa..0c7259307e 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix segfault at reconfigure of AdmittanceController (`#1248 `_) +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Lennart Nachtigall, Sai Kishor Kothakota + 4.12.1 (2024-08-14) ------------------- * Fix admittance controller interface read/write logic (`#1232 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index ff5a8c13c7..abbdc8b28e 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + 4.12.1 (2024-08-14) ------------------- diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index a2a3a23ee2..18d6956fd5 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + 4.12.1 (2024-08-14) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 23b1c687d8..9ed00feb8b 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.1 (2024-08-14) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 5c8f0da1bc..6dc273b420 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 +----------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + 4.12.1 (2024-08-14) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 24232cac8a..55b779c876 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.1 (2024-08-14) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index add7f90438..8887be16c2 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.1 (2024-08-14) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index a8d07e2caf..9251f6ca43 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + 4.12.1 (2024-08-14) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 802f2f8b90..519943aeee 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Joint State Broadcaster] Publish the joint_states of joints present in the URDF (`#1233 `_) +* Contributors: Sai Kishor Kothakota + 4.12.1 (2024-08-14) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index aefe4f1a68..a254b043ef 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.1 (2024-08-14) ------------------- diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 96b3d90756..b619740475 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + 4.12.1 (2024-08-14) ------------------- diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 05cfd65c65..0740cac45b 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.1 (2024-08-14) ------------------- diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index caa2319219..04bf8e50d6 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.1 (2024-08-14) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 13b29cbd8f..ad33ffdd91 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + 4.12.1 (2024-08-14) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 5122cc1d98..36fec11996 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.1 (2024-08-14) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index bbd2aaf3d0..f66f6a86fb 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 +----------- + 4.12.1 (2024-08-14) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index c167335f3d..6dfd067460 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 +----------- + 4.12.1 (2024-08-14) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 47cbebc6ce..b391f2fbb4 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.1 (2024-08-14) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 0424e7f4f5..76e23e1184 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + 4.12.1 (2024-08-14) ------------------- diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index ae42dff585..ca8795f8b5 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + 4.12.1 (2024-08-14) ------------------- diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index be0f90301e..ad7015f903 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.12.1 (2024-08-14) ------------------- From 20f6f0bba9a73cb49b4547cc5316ee058a2298aa Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 22 Aug 2024 19:51:10 +0100 Subject: [PATCH 44/97] 4.13.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_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 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/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 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 46 files changed, 68 insertions(+), 68 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 895bba5487..e4ea6ceba8 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) * Contributors: Sai Kishor Kothakota diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 1ab37fa944..a191cede5d 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.12.1 + 4.13.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 0c7259307e..3f0f44ebde 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- * Fix segfault at reconfigure of AdmittanceController (`#1248 `_) * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) * Contributors: Lennart Nachtigall, Sai Kishor Kothakota diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 75a2827417..bd37557b06 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.12.1 + 4.13.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index abbdc8b28e..9de6109a7e 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) * Contributors: Sai Kishor Kothakota diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 819280a96b..73b658220a 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.12.1 + 4.13.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 18d6956fd5..e82c7ee4f4 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) * Contributors: Sai Kishor Kothakota diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 4f77e3a919..d352de7ed4 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.12.1 + 4.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 9ed00feb8b..573f951c1b 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- 4.12.1 (2024-08-14) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 0cea01d743..3ec600b0fa 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.12.1 + 4.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 6dc273b420..d8ee1d38d1 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 ------------ +4.13.0 (2024-08-22) +------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) * Contributors: Sai Kishor Kothakota diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 3973cd91cd..a41568d2d5 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.12.1 + 4.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 55b779c876..04ce12659e 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- 4.12.1 (2024-08-14) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 1b996a1b5e..7bef16d4fd 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.12.1 + 4.13.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 8887be16c2..8a22013b50 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- 4.12.1 (2024-08-14) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 47e809afae..65d8c58e0f 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.12.1 + 4.13.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 9251f6ca43..1d357eb88f 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) * Contributors: Sai Kishor Kothakota diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 6efaea8076..c4e31db09f 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.12.1 + 4.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 519943aeee..3b61fbec71 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- * [Joint State Broadcaster] Publish the joint_states of joints present in the URDF (`#1233 `_) * Contributors: Sai Kishor Kothakota diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index aa66a9641e..121ca68f6a 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.12.1 + 4.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 a254b043ef..9ba9778c37 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- 4.12.1 (2024-08-14) ------------------- diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index edfef38b39..9ff1dd03c9 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.12.1 + 4.13.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index b619740475..5a5a872dff 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) * Contributors: Sai Kishor Kothakota diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 98b9cd1b14..82d38cc417 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 4.12.1 + 4.13.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 0740cac45b..02a10e05db 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- 4.12.1 (2024-08-14) ------------------- diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 6148c43b94..767e5eecbc 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.12.1 + 4.13.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 04bf8e50d6..1aa9b26515 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- 4.12.1 (2024-08-14) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 5b05352a21..9545700d07 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.12.1 + 4.13.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index ad33ffdd91..5a60a36dcd 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) * Contributors: Sai Kishor Kothakota diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 1271dd9db1..cb814b1b8f 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.12.1 + 4.13.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 36fec11996..cb8ac7281f 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- 4.12.1 (2024-08-14) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index c6791dd0a5..5a5ad3f632 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.12.1 + 4.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 f66f6a86fb..c4e08ae2c1 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 ------------ +4.13.0 (2024-08-22) +------------------- 4.12.1 (2024-08-14) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 6d7d362847..d1fd4d3844 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.12.1 + 4.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 9b2a14c367..a9c8fb6b60 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.12.1", + version="4.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 6dfd067460..f3371a8320 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 ------------ +4.13.0 (2024-08-22) +------------------- 4.12.1 (2024-08-14) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 13f6136350..c9b086e587 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 - 4.12.1 + 4.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 3d10574eec..cc2fc8b00b 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.12.1", + version="4.13.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index b391f2fbb4..8b2791a8e7 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- 4.12.1 (2024-08-14) ------------------- diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 73c2bf8198..bff5ccc8e5 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.12.1 + 4.13.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 76e23e1184..a4c6912ce0 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) * Contributors: Sai Kishor Kothakota diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 0e87370142..aa37106fac 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.12.1 + 4.13.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index ca8795f8b5..f535a1309e 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) * Contributors: Sai Kishor Kothakota diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 249d2041e7..041f4425e8 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.12.1 + 4.13.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index ad7015f903..7b0e141de7 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.13.0 (2024-08-22) +------------------- 4.12.1 (2024-08-14) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 03283cbc14..f7a6fcac72 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.12.1 + 4.13.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 1c4d58e55efd86630451eb615ade5d1f7da7e952 Mon Sep 17 00:00:00 2001 From: Takashi Sato Date: Sun, 25 Aug 2024 01:55:15 +0900 Subject: [PATCH 45/97] [JSB] Move the initialize of urdf::Model from on_activate to on_configure to improve real-time performance (#1269) --- .../joint_state_broadcaster.hpp | 4 +++ .../src/joint_state_broadcaster.cpp | 27 ++++++++++--------- 2 files changed, 18 insertions(+), 13 deletions(-) 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 b3fa69f94c..ecc3c767f6 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 @@ -27,6 +27,7 @@ #include "joint_state_broadcaster_parameters.hpp" #include "realtime_tools/realtime_publisher.h" #include "sensor_msgs/msg/joint_state.hpp" +#include "urdf/model.h" namespace joint_state_broadcaster { @@ -111,6 +112,9 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface dynamic_joint_state_publisher_; std::shared_ptr> realtime_dynamic_joint_state_publisher_; + + urdf::Model model_; + bool is_model_loaded_ = false; }; } // namespace joint_state_broadcaster diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 209b44e557..fe0b32213a 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -164,6 +164,18 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; } + + const std::string & urdf = get_robot_description(); + + is_model_loaded_ = !urdf.empty() && model_.initString(urdf); + if (!is_model_loaded_) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to parse robot description. Will publish all the interfaces with '%s', '%s' and '%s'", + HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT); + } + return CallbackReturn::SUCCESS; } @@ -243,17 +255,6 @@ bool JointStateBroadcaster::init_joint_data() name_if_value_mapping_[si->get_prefix_name()][interface_name] = kUninitializedValue; } - const std::string & urdf = get_robot_description(); - - urdf::Model model; - const bool is_model_loaded = !urdf.empty() && model.initString(urdf); - if (!is_model_loaded) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Failed to parse robot description. Will publish all the interfaces with '%s', '%s' and '%s'", - HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT); - } // filter state interfaces that have at least one of the joint_states fields, // the rest will be ignored for this message for (const auto & name_ifv : name_if_value_mapping_) @@ -262,8 +263,8 @@ bool JointStateBroadcaster::init_joint_data() if (has_any_key(interfaces_and_values, {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT})) { if ( - !params_.use_urdf_to_filter || !params_.joints.empty() || !is_model_loaded || - model.getJoint(name_ifv.first)) + !params_.use_urdf_to_filter || !params_.joints.empty() || !is_model_loaded_ || + model_.getJoint(name_ifv.first)) { joint_names_.push_back(name_ifv.first); } From ce12694c6c948509a470783fd3b770cfab60a26e Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Mon, 26 Aug 2024 14:51:49 +0200 Subject: [PATCH 46/97] rename get/set_state to get/set_lifecylce_state (#1250) --- diff_drive_controller/src/diff_drive_controller.cpp | 2 +- .../src/joint_trajectory_controller.cpp | 6 +++--- tricycle_controller/src/tricycle_controller.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index a64f210503..66da6d6738 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -101,7 +101,7 @@ controller_interface::return_type DiffDriveController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { auto logger = get_node()->get_logger(); - if (get_state().id() == State::PRIMARY_STATE_INACTIVE) + if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) { if (!is_halted) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 9563568ad5..e4923604fd 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -137,7 +137,7 @@ JointTrajectoryController::state_interface_configuration() const controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { return controller_interface::return_type::OK; } @@ -593,7 +593,7 @@ void JointTrajectoryController::query_state_service( { const auto logger = get_node()->get_logger(); // Preconditions - if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { RCLCPP_ERROR(logger, "Can't sample trajectory. Controller is not active."); response->success = false; @@ -1112,7 +1112,7 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_received_callback( RCLCPP_INFO(get_node()->get_logger(), "Received new action goal"); // Precondition: Running controller - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR( get_node()->get_logger(), "Can't accept new action goals. Controller is not running."); diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 94ff63e659..ec7ca7bd5e 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -86,7 +86,7 @@ InterfaceConfiguration TricycleController::state_interface_configuration() const controller_interface::return_type TricycleController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { - if (get_state().id() == State::PRIMARY_STATE_INACTIVE) + if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) { if (!is_halted) { From 3be3fe95c3eb2a6a7e07bde2b96a9b8acc558d9a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Mon, 26 Aug 2024 14:52:27 +0200 Subject: [PATCH 47/97] Fix bug for displaying all controllers (#1259) --- .../joint_trajectory_controller.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 4cc6c901af..5b27c2c832 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 @@ -238,8 +238,11 @@ def _update_jtc_list(self): # for _all_ their joints running_jtc = self._running_jtc_info() if running_jtc and not self._robot_joint_limits: + self._robot_joint_limits = {} for jtc_info in running_jtc: - self._robot_joint_limits = get_joint_limits(self._node, _jtc_joint_names(jtc_info)) + self._robot_joint_limits.update( + get_joint_limits(self._node, _jtc_joint_names(jtc_info)) + ) valid_jtc = [] if self._robot_joint_limits: for jtc_info in running_jtc: From a7b2af5ca896ab142ff283217367c5fc889afc70 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 29 Aug 2024 13:48:18 +0200 Subject: [PATCH 48/97] [PID Controller] Export state interfaces for easier chaining with other controllers (#1214) --- .../include/pid_controller/pid_controller.hpp | 2 + pid_controller/src/pid_controller.cpp | 64 +++++++++++++++++++ pid_controller/src/pid_controller.yaml | 1 + pid_controller/test/test_pid_controller.hpp | 1 + .../test/test_pid_controller_preceding.cpp | 18 ++++++ 5 files changed, 86 insertions(+) diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 236b067929..ce66fd06d4 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -117,6 +117,8 @@ class PidController : public controller_interface::ChainableControllerInterface // override methods from ChainableControllerInterface std::vector on_export_reference_interfaces() override; + std::vector on_export_state_interfaces() override; + bool on_set_chained_mode(bool chained_mode) override; // internal methods diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index c8e6cc0fe0..032dc1d666 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -187,6 +187,43 @@ controller_interface::CallbackReturn PidController::on_configure( auto measured_state_callback = [&](const std::shared_ptr state_msg) -> void { + if (state_msg->dof_names.size() != reference_and_state_dof_names_.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Size of input data names (%zu) is not matching the expected size (%zu).", + state_msg->dof_names.size(), reference_and_state_dof_names_.size()); + return; + } + if (state_msg->values.size() != reference_and_state_dof_names_.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Size of input data values (%zu) is not matching the expected size (%zu).", + state_msg->values.size(), reference_and_state_dof_names_.size()); + return; + } + + if (!state_msg->values_dot.empty()) + { + if (params_.reference_and_state_interfaces.size() != 2) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "The reference_and_state_interfaces parameter has to have two interfaces [the " + "interface and the derivative of the interface], in order to use the values_dot " + "field."); + return; + } + if (state_msg->values_dot.size() != reference_and_state_dof_names_.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Size of input data values_dot (%zu) is not matching the expected size (%zu).", + state_msg->values_dot.size(), reference_and_state_dof_names_.size()); + return; + } + } // TODO(destogl): Sort the input values based on joint and interface names measured_state_.writeFromNonRT(state_msg); }; @@ -363,6 +400,27 @@ std::vector PidController::on_export_refer return reference_interfaces; } +std::vector PidController::on_export_state_interfaces() +{ + std::vector state_interfaces; + state_interfaces.reserve(state_interfaces_values_.size()); + + state_interfaces_values_.resize( + reference_and_state_dof_names_.size() * params_.reference_and_state_interfaces.size(), + std::numeric_limits::quiet_NaN()); + size_t index = 0; + for (const auto & interface : params_.reference_and_state_interfaces) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + state_interfaces.push_back(hardware_interface::StateInterface( + get_node()->get_name(), dof_name + "/" + interface, &state_interfaces_values_[index])); + ++index; + } + } + return state_interfaces; +} + bool PidController::on_set_chained_mode(bool chained_mode) { // Always accept switch to/from chained mode @@ -438,6 +496,12 @@ controller_interface::return_type PidController::update_and_write_commands( } } + // Fill the information of the exported state interfaces + for (size_t i = 0; i < measured_state_values_.size(); ++i) + { + state_interfaces_values_[i] = measured_state_values_[i]; + } + for (size_t i = 0; i < dof_; ++i) { double tmp_command = std::numeric_limits::quiet_NaN(); diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index f645738862..651cc1e7de 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -34,6 +34,7 @@ pid_controller: read_only: true, validation: { not_empty<>: null, + size_gt<>: 0, size_lt<>: 3, } } diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 158b5d9147..4471f35a12 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -71,6 +71,7 @@ class TestablePidController : public pid_controller::PidController const rclcpp_lifecycle::State & previous_state) override { auto ref_itfs = on_export_reference_interfaces(); + auto state_itfs = on_export_state_interfaces(); return pid_controller::PidController::on_activate(previous_state); } diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index 498ca633da..9e6a7ef04c 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -90,6 +90,24 @@ TEST_F(PidControllerTest, check_exported_interfaces) ++ri_index; } } + + // check exported state itfs + auto exported_state_itfs = controller_->export_state_interfaces(); + ASSERT_EQ(exported_state_itfs.size(), dof_state_values_.size()); + size_t esi_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + const std::string state_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(exported_state_itfs[esi_index].get_name(), state_itf_name); + EXPECT_EQ( + exported_state_itfs[esi_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(exported_state_itfs[esi_index].get_interface_name(), dof_name + "/" + interface); + ++esi_index; + } + } } int main(int argc, char ** argv) From c08bdab049cc090df0643d3399bf53862f4571be Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 1 Sep 2024 22:10:10 +0200 Subject: [PATCH 49/97] Bump version of pre-commit hooks (#1279) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 6fadbbdace..43bb778260 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -50,13 +50,13 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/psf/black - rev: 24.4.2 + rev: 24.8.0 hooks: - id: black args: ["--line-length=99"] - repo: https://github.com/pycqa/flake8 - rev: 7.1.0 + rev: 7.1.1 hooks: - id: flake8 args: ["--extend-ignore=E501"] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.1 + rev: 0.29.2 hooks: - id: check-github-workflows args: ["--verbose"] From 48a7f8b46eb9d5fb486684bcd5b6d94a3779c351 Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Wed, 11 Sep 2024 13:17:14 +0200 Subject: [PATCH 50/97] Fix deprecation warning in paramater declaration (#1280) --- .../publisher_forward_position_controller.py | 2 +- .../publisher_joint_trajectory_controller.py | 4 +--- 2 files 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 5cf28ac604..bb6add77ef 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 @@ -37,7 +37,7 @@ def __init__(self): # Read all positions from parameters self.goals = [] for name in goal_names: - self.declare_parameter(name) + self.declare_parameter(name, rclpy.Parameter.Type.DOUBLE_ARRAY) goal = self.get_parameter(name).value if goal is None or len(goal) == 0: raise Exception(f'Values for goal "{name}" not set!') 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 cb66f58468..27f28da1be 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,7 +18,6 @@ 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 @@ -67,8 +66,7 @@ def __init__(self): # Read all positions from parameters self.goals = [] # List of JointTrajectoryPoint for name in goal_names: - self.declare_parameter(name, descriptor=ParameterDescriptor(dynamic_typing=True)) - + self.declare_parameter(name, rclpy.Parameter.Type.DOUBLE_ARRAY) point = JointTrajectoryPoint() def get_sub_param(sub_param): From 1dc3d2aa47bb7746f5a414d016e9566c9eef4060 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Wed, 11 Sep 2024 14:18:40 +0200 Subject: [PATCH 51/97] fix(steering-odometry): handle infinite turning radius properly (#1285) --- .../src/steering_odometry.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index ba431faf33..824ec86f59 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -133,11 +133,17 @@ double SteeringOdometry::get_linear_velocity_double_traction_axle( const double steer_pos) { double turning_radius = wheelbase_ / std::tan(steer_pos); + const double vel_wheel_r = right_traction_wheel_vel * wheel_radius_; + const double vel_wheel_l = left_traction_wheel_vel * wheel_radius_; + + if (std::isinf(turning_radius)) + { + return (vel_wheel_r + vel_wheel_l) * 0.5; + } + // overdetermined, we take the average - double vel_r = right_traction_wheel_vel * wheel_radius_ * turning_radius / - (turning_radius + wheel_track_ * 0.5); - double vel_l = left_traction_wheel_vel * wheel_radius_ * turning_radius / - (turning_radius - wheel_track_ * 0.5); + const double vel_r = vel_wheel_r * turning_radius / (turning_radius + wheel_track_ * 0.5); + const double vel_l = vel_wheel_l * turning_radius / (turning_radius - wheel_track_ * 0.5); return (vel_r + vel_l) * 0.5; } From 75ae6c9951ba9a40cd34f851595aa5998b1102f3 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 11 Sep 2024 13:21:59 +0100 Subject: [PATCH 52/97] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 +++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_controller/CHANGELOG.rst | 3 +++ diff_drive_controller/CHANGELOG.rst | 5 +++++ 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 | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ parallel_gripper_controller/CHANGELOG.rst | 3 +++ pid_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 5 +++++ rqt_joint_trajectory_controller/CHANGELOG.rst | 5 +++++ steering_controllers_library/CHANGELOG.rst | 5 +++++ tricycle_controller/CHANGELOG.rst | 5 +++++ tricycle_steering_controller/CHANGELOG.rst | 3 +++ velocity_controllers/CHANGELOG.rst | 3 +++ 22 files changed, 82 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index e4ea6ceba8..7e13c10e72 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 3f0f44ebde..149dfbd238 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-08-22) ------------------- * Fix segfault at reconfigure of AdmittanceController (`#1248 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 9de6109a7e..e8a3d25c63 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index e82c7ee4f4..91bc2e9cb5 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* rename get/set_state to get/set_lifecylce_state (`#1250 `_) +* Contributors: Manuel Muth + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 573f951c1b..325995f232 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-08-22) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index d8ee1d38d1..19856d93a0 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 +----------- + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 04ce12659e..03b4568b7b 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-08-22) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 8a22013b50..6bc2ec944e 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-08-22) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 1d357eb88f..f009291e9f 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 3b61fbec71..83b30c40c1 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JSB] Move the initialize of urdf::Model from on_activate to on_configure to improve real-time performance (`#1269 `_) +* Contributors: Takashi Sato + 4.13.0 (2024-08-22) ------------------- * [Joint State Broadcaster] Publish the joint_states of joints present in the URDF (`#1233 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 9ba9778c37..75b72ef7a4 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* rename get/set_state to get/set_lifecylce_state (`#1250 `_) +* Contributors: Manuel Muth + 4.13.0 (2024-08-22) ------------------- diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 5a5a872dff..5add92849e 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 02a10e05db..aea95e153e 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [PID Controller] Export state interfaces for easier chaining with other controllers (`#1214 `_) +* Contributors: Sai Kishor Kothakota + 4.13.0 (2024-08-22) ------------------- diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 1aa9b26515..a28a6e6b26 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-08-22) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 5a60a36dcd..4d65c4843a 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index cb8ac7281f..aeba8e3e80 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-08-22) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index c4e08ae2c1..16e6436d88 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 +----------- +* Fix deprecation warning in paramater declaration (`#1280 `_) +* Contributors: Sanjeev + 4.13.0 (2024-08-22) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index f3371a8320..d8a49b7ff2 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 +----------- +* Fix bug for displaying all controllers (`#1259 `_) +* Contributors: Francisco Martín Rico + 4.13.0 (2024-08-22) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 8b2791a8e7..3907b16ae7 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix(steering-odometry): handle infinite turning radius properly (`#1285 `_) +* Contributors: Rein Appeldoorn + 4.13.0 (2024-08-22) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index a4c6912ce0..66bd3b83e8 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* rename get/set_state to get/set_lifecylce_state (`#1250 `_) +* Contributors: Manuel Muth + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index f535a1309e..2c3c327f3c 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 7b0e141de7..b7fa6e9738 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-08-22) ------------------- From 57c50e584e33b316dd64801916cf6d951e0cff5b Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 11 Sep 2024 13:23:08 +0100 Subject: [PATCH 53/97] 4.14.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_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 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/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 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 46 files changed, 68 insertions(+), 68 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 7e13c10e72..066ef29830 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- 4.13.0 (2024-08-22) ------------------- diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index a191cede5d..3c9836e97b 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.13.0 + 4.14.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 149dfbd238..bb640c49fe 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- 4.13.0 (2024-08-22) ------------------- diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index bd37557b06..d9d6d24b7e 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.13.0 + 4.14.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index e8a3d25c63..d2a06f3329 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- 4.13.0 (2024-08-22) ------------------- diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 73b658220a..5858b53dc3 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.13.0 + 4.14.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 91bc2e9cb5..4907b259bf 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- * rename get/set_state to get/set_lifecylce_state (`#1250 `_) * Contributors: Manuel Muth diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index d352de7ed4..7f4cb4cbd6 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.13.0 + 4.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 325995f232..388fde8829 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- 4.13.0 (2024-08-22) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 3ec600b0fa..1208cd4732 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.13.0 + 4.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 19856d93a0..30c2e36143 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 ------------ +4.14.0 (2024-09-11) +------------------- 4.13.0 (2024-08-22) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index a41568d2d5..758f448fdf 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.13.0 + 4.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 03b4568b7b..04a2dd86c5 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- 4.13.0 (2024-08-22) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 7bef16d4fd..bfa8a68955 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.13.0 + 4.14.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 6bc2ec944e..fc2e5c571a 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- 4.13.0 (2024-08-22) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 65d8c58e0f..019f1e2e44 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.13.0 + 4.14.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index f009291e9f..89a39358ed 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- 4.13.0 (2024-08-22) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index c4e31db09f..0ed268d9ad 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.13.0 + 4.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 83b30c40c1..21dfc85d82 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- * [JSB] Move the initialize of urdf::Model from on_activate to on_configure to improve real-time performance (`#1269 `_) * Contributors: Takashi Sato diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 121ca68f6a..8776cb31d3 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.13.0 + 4.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 75b72ef7a4..8b35a6986c 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- * rename get/set_state to get/set_lifecylce_state (`#1250 `_) * Contributors: Manuel Muth diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 9ff1dd03c9..c8b670dac4 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.13.0 + 4.14.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 5add92849e..f8e7c77ddb 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- 4.13.0 (2024-08-22) ------------------- diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 82d38cc417..055b5311b7 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 4.13.0 + 4.14.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index aea95e153e..08e37d0836 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- * [PID Controller] Export state interfaces for easier chaining with other controllers (`#1214 `_) * Contributors: Sai Kishor Kothakota diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 767e5eecbc..9ef1964cd3 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.13.0 + 4.14.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index a28a6e6b26..17db75c10b 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- 4.13.0 (2024-08-22) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 9545700d07..e79dd05059 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.13.0 + 4.14.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 4d65c4843a..36335db719 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- 4.13.0 (2024-08-22) ------------------- diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index cb814b1b8f..433120ca8d 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.13.0 + 4.14.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index aeba8e3e80..70fdee135d 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- 4.13.0 (2024-08-22) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 5a5ad3f632..6ff486a1d8 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.13.0 + 4.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 16e6436d88..5f2afedd10 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 ------------ +4.14.0 (2024-09-11) +------------------- * Fix deprecation warning in paramater declaration (`#1280 `_) * Contributors: Sanjeev diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index d1fd4d3844..0b30bdd7ad 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.13.0 + 4.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 a9c8fb6b60..071e90ca0b 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.13.0", + version="4.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 d8a49b7ff2..4cd8b487d8 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 ------------ +4.14.0 (2024-09-11) +------------------- * Fix bug for displaying all controllers (`#1259 `_) * Contributors: Francisco Martín Rico diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index c9b086e587..c5973fd96e 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 - 4.13.0 + 4.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 cc2fc8b00b..6c390b22e9 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.13.0", + version="4.14.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 3907b16ae7..ab57a3b720 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- * fix(steering-odometry): handle infinite turning radius properly (`#1285 `_) * Contributors: Rein Appeldoorn diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index bff5ccc8e5..41f7e3703d 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.13.0 + 4.14.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 66bd3b83e8..82721a5d1a 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- * rename get/set_state to get/set_lifecylce_state (`#1250 `_) * Contributors: Manuel Muth diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index aa37106fac..0fea2baab6 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.13.0 + 4.14.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 2c3c327f3c..697878ac81 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- 4.13.0 (2024-08-22) ------------------- diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 041f4425e8..5bf5a2d283 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.13.0 + 4.14.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index b7fa6e9738..5a662df92b 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-09-11) +------------------- 4.13.0 (2024-08-22) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index f7a6fcac72..e7ebdcc827 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.13.0 + 4.14.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From 50036e109d6a03e41bb9b43a4f67411887b48640 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Wed, 25 Sep 2024 22:53:41 +0200 Subject: [PATCH 54/97] fix(steering-odometry): convert twist to steering angle (#1288) --- steering_controllers_library/src/steering_odometry.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 824ec86f59..dbe210ed41 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -208,12 +208,9 @@ void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) { - if (fabs(v_bx) < std::numeric_limits::epsilon()) - { - // avoid division by zero - return 0.; - } - return std::atan(omega_bz * wheelbase_ / v_bx); + // phi can be nan if both v_bx and omega_bz are zero + const auto phi = std::atan(omega_bz * wheelbase_ / v_bx); + return std::isfinite(phi) ? phi : 0.0; } std::tuple, std::vector> SteeringOdometry::get_commands( From aee0f8256d241b412fc42b4814d76fc8e38a8342 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 1 Oct 2024 09:38:31 +0100 Subject: [PATCH 55/97] Bump version of pre-commit hooks (#1300) --- .pre-commit-config.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 43bb778260..63e7f08682 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.8 + rev: v19.1.0 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -109,7 +109,7 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: v1.1.1 + rev: v1.1.2 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.2 + rev: 0.29.3 hooks: - id: check-github-workflows args: ["--verbose"] From 10040541b9cca8e83408315d6747f3b0fcf7cf27 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Tue, 1 Oct 2024 10:39:09 +0200 Subject: [PATCH 56/97] fix(timeout): do not reset steer wheels to 0. on timeout (#1289) --- doc/release_notes.rst | 1 + .../src/steering_controllers_library.cpp | 29 ++++++------------ .../test_steering_controllers_library.cpp | 30 +++++++++++-------- 3 files changed, 28 insertions(+), 32 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index b55db861b3..1fb66c4475 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -56,6 +56,7 @@ steering_controllers_library ******************************** * Changing default int values to double in steering controller's yaml file. The controllers should now initialize successfully without specifying these parameters (`#927 `_). * A fix for Ackermann steering odometry was added (`#921 `_). +* Do not reset the steering wheels to ``0.0`` on timeout (`#1289 `_). tricycle_controller ************************ diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index f193098f82..836574f150 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -355,26 +355,11 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = time - (current_ref)->header.stamp; - // send message only if there is no timeout - if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) - { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.angular.z; - } - } - else - { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) - { - reference_interfaces_[0] = 0.0; - reference_interfaces_[1] = 0.0; - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); - } + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.angular.z; } return controller_interface::return_type::OK; @@ -396,13 +381,17 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c last_linear_velocity_ = reference_interfaces_[0]; last_angular_velocity_ = reference_interfaces_[1]; + const auto age_of_last_command = time - (*(input_ref_.readFromRT()))->header.stamp; + const auto timeout = + age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); + auto [traction_commands, steering_commands] = odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop); if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { - command_interfaces_[i].set_value(traction_commands[i]); + command_interfaces_[i].set_value(timeout ? 0. : traction_commands[i]); } for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { @@ -414,7 +403,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { - command_interfaces_[i].set_value(traction_commands[i]); + command_interfaces_[i].set_value(timeout ? 0. : traction_commands[i]); } for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index fca7d00946..e73f32cd5e 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -133,8 +133,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) @@ -142,10 +142,13 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) EXPECT_TRUE(std::isnan(interface)); } - for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) - { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); - } + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + + // Steer angles should not reset + EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); // case 2 position_feedback = true controller_->params_.position_feedback = true; @@ -174,8 +177,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) @@ -183,10 +186,13 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) EXPECT_TRUE(std::isnan(interface)); } - for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) - { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); - } + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + + // Steer angles should not reset + EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); } int main(int argc, char ** argv) From 9c5be9f44ee0880b2d99b393bc5b3b49f95a8cc7 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Mon, 7 Oct 2024 20:14:25 +0200 Subject: [PATCH 57/97] Adapt test to new way of exporting reference interfaces (Related to #1240 in ros2_control) (#1103) --------- Co-authored-by: Bence Magyar --- .../test/test_ackermann_steering_controller.cpp | 6 +++--- ...test_ackermann_steering_controller_preceeding.cpp | 6 +++--- .../test/test_bicycle_steering_controller.cpp | 6 +++--- .../test_bicycle_steering_controller_preceeding.cpp | 6 +++--- pid_controller/test/test_pid_controller.cpp | 6 +++--- .../test/test_pid_controller_preceding.cpp | 12 ++++++------ .../test/test_steering_controllers_library.cpp | 6 +++--- .../test/test_tricycle_steering_controller.cpp | 6 +++--- .../test_tricycle_steering_controller_preceeding.cpp | 6 +++--- 9 files changed, 30 insertions(+), 30 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index c04e87be95..c434f73467 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -89,9 +89,9 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index 96dd20d80e..4f67beaa07 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -91,9 +91,9 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 573992b24e..7ec0983f93 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -73,9 +73,9 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp index 0bc03f4886..0a321009c2 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -77,9 +77,9 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 9b53dccb23..60d484e463 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -97,9 +97,9 @@ TEST_F(PidControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); + EXPECT_EQ(ref_if_conf[ri_index]->get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), dof_name + "/" + interface); ++ri_index; } } diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index 9e6a7ef04c..ee5fe46754 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -84,9 +84,9 @@ TEST_F(PidControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); + EXPECT_EQ(ref_if_conf[ri_index]->get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), dof_name + "/" + interface); ++ri_index; } } @@ -101,10 +101,10 @@ TEST_F(PidControllerTest, check_exported_interfaces) { const std::string state_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(exported_state_itfs[esi_index].get_name(), state_itf_name); + EXPECT_EQ(exported_state_itfs[esi_index]->get_name(), state_itf_name); EXPECT_EQ( - exported_state_itfs[esi_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(exported_state_itfs[esi_index].get_interface_name(), dof_name + "/" + interface); + exported_state_itfs[esi_index]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(exported_state_itfs[esi_index]->get_interface_name(), dof_name + "/" + interface); ++esi_index; } } diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index e73f32cd5e..3378efbef8 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -70,9 +70,9 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 3f2589cb6c..8e29314f8e 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -81,9 +81,9 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(ref_if_conf[i]->get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp index 2170659ee7..566169e34e 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -84,9 +84,9 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(ref_if_conf[i]->get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[i]->get_interface_name(), joint_reference_interfaces_[i]); } } From 5e1ecbf51bf45f5e03f37c0eaafd7daed0bc66e5 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 7 Oct 2024 19:24:58 +0100 Subject: [PATCH 58/97] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 5 +++++ admittance_controller/CHANGELOG.rst | 3 +++ bicycle_steering_controller/CHANGELOG.rst | 5 +++++ 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 | 3 +++ joint_trajectory_controller/CHANGELOG.rst | 3 +++ parallel_gripper_controller/CHANGELOG.rst | 3 +++ pid_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 3 +++ range_sensor_broadcaster/CHANGELOG.rst | 3 +++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 3 +++ steering_controllers_library/CHANGELOG.rst | 7 +++++++ tricycle_controller/CHANGELOG.rst | 3 +++ tricycle_steering_controller/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 3 +++ 22 files changed, 78 insertions(+) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 066ef29830..71d615dbc3 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* Contributors: Manuel Muth + 4.14.0 (2024-09-11) ------------------- diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index bb640c49fe..ae4e47bcad 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index d2a06f3329..ec4523f9c9 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* Contributors: Manuel Muth + 4.14.0 (2024-09-11) ------------------- diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 4907b259bf..95b11d2c40 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- * rename get/set_state to get/set_lifecylce_state (`#1250 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 388fde8829..61ee0921d1 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 30c2e36143..6d8ea46c1f 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 +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 04a2dd86c5..dc496e5566 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index fc2e5c571a..c03d284e02 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 89a39358ed..26613db617 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 21dfc85d82..4db42c39e4 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- * [JSB] Move the initialize of urdf::Model from on_activate to on_configure to improve real-time performance (`#1269 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 8b35a6986c..0979f0c073 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- * rename get/set_state to get/set_lifecylce_state (`#1250 `_) diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index f8e7c77ddb..bd99385f23 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 08e37d0836..0aa8677522 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* Contributors: Manuel Muth + 4.14.0 (2024-09-11) ------------------- * [PID Controller] Export state interfaces for easier chaining with other controllers (`#1214 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 17db75c10b..b645154cdf 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 36335db719..16e164a2f4 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 70fdee135d..e93bcf4403 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 5f2afedd10..ab1debdaef 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 +----------- + 4.14.0 (2024-09-11) ------------------- * Fix deprecation warning in paramater declaration (`#1280 `_) diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 4cd8b487d8..c6c414caf4 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 +----------- + 4.14.0 (2024-09-11) ------------------- * Fix bug for displaying all controllers (`#1259 `_) diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index ab57a3b720..c2aeee8fef 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* fix(timeout): do not reset steer wheels to 0. on timeout (`#1289 `_) +* fix(steering-odometry): convert twist to steering angle (`#1288 `_) +* Contributors: Manuel Muth, Rein Appeldoorn + 4.14.0 (2024-09-11) ------------------- * fix(steering-odometry): handle infinite turning radius properly (`#1285 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 82721a5d1a..f27568de08 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- * rename get/set_state to get/set_lifecylce_state (`#1250 `_) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 697878ac81..6b12ac5c44 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* Contributors: Manuel Muth + 4.14.0 (2024-09-11) ------------------- diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 5a662df92b..089267d835 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-09-11) ------------------- From 97c1e2472f2bbc98a9cb86449b41611a3e298b30 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 7 Oct 2024 19:25:30 +0100 Subject: [PATCH 59/97] 4.15.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_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 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/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 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 46 files changed, 68 insertions(+), 68 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 71d615dbc3..bc2997b8a9 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) * Contributors: Manuel Muth diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 3c9836e97b..5764de56c1 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.14.0 + 4.15.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index ae4e47bcad..46899f9cd6 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index d9d6d24b7e..06fde3cb59 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.14.0 + 4.15.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index ec4523f9c9..0d096951ca 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) * Contributors: Manuel Muth diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 5858b53dc3..f58cf42dff 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.14.0 + 4.15.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 95b11d2c40..4c48fb234d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 7f4cb4cbd6..45ab80d5cb 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.14.0 + 4.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 61ee0921d1..ea2efdcf4e 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 1208cd4732..fab6d671ad 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.14.0 + 4.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 6d8ea46c1f..7d2a7a1a8a 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 ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 758f448fdf..1383342246 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.14.0 + 4.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 dc496e5566..54caa9d764 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index bfa8a68955..f9f30dd053 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.14.0 + 4.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index c03d284e02..11c61f86cf 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 019f1e2e44..d46c98f608 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.14.0 + 4.15.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 26613db617..40af7a5360 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 0ed268d9ad..ed1f794afa 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.14.0 + 4.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 4db42c39e4..cf422e3eb8 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 8776cb31d3..c3a1e66574 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.14.0 + 4.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 0979f0c073..81696246b1 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index c8b670dac4..112c4ad9a1 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.14.0 + 4.15.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index bd99385f23..a3024fbf62 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 055b5311b7..b396ba7b85 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 4.14.0 + 4.15.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 0aa8677522..5a27471199 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) * Contributors: Manuel Muth diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 9ef1964cd3..79a66d3cc3 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.14.0 + 4.15.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index b645154cdf..d52201f2b4 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index e79dd05059..c03888eaf8 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.14.0 + 4.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 16e164a2f4..77eae62d2a 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 433120ca8d..92283fe3ad 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.14.0 + 4.15.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index e93bcf4403..bd9e0a8b38 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 6ff486a1d8..0886b489c7 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.14.0 + 4.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 ab1debdaef..29ca6cd84d 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 ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 0b30bdd7ad..f61a51e34e 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.14.0 + 4.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 071e90ca0b..59aee66875 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.14.0", + version="4.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 c6c414caf4..043930c536 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 ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index c5973fd96e..7920e63246 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 - 4.14.0 + 4.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 6c390b22e9..a70342e154 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.14.0", + version="4.15.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index c2aeee8fef..39360d6921 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) * fix(timeout): do not reset steer wheels to 0. on timeout (`#1289 `_) * fix(steering-odometry): convert twist to steering angle (`#1288 `_) diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 41f7e3703d..5c96c4624c 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.14.0 + 4.15.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index f27568de08..34520eeb83 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 0fea2baab6..4e6ab510f3 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.14.0 + 4.15.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 6b12ac5c44..4e1f9abd1e 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) * Contributors: Manuel Muth diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 5bf5a2d283..27aac82dd1 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.14.0 + 4.15.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 089267d835..a04bf1c4ae 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-10-07) +------------------- 4.14.0 (2024-09-11) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index e7ebdcc827..ea295db0ed 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.14.0 + 4.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From f96d2fc0fbf94537f769cffcf844858f7a085671 Mon Sep 17 00:00:00 2001 From: Kenta Kato Date: Sat, 26 Oct 2024 23:57:38 +0900 Subject: [PATCH 60/97] [JTC] Add Parameter to Toggle State Setting on Activation (#1231) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * [JTC] Add param to setting last command interface value as state on activation * [JTC] add a note about set_last_command_interface_value_as_state_on_activation to release_notes. Updated the parameters.yaml description to match the same wording. --------- Co-authored-by: Bence Magyar Co-authored-by: Christoph Fröhlich --- doc/release_notes.rst | 2 ++ .../src/joint_trajectory_controller.cpp | 4 +++- .../src/joint_trajectory_controller_parameters.yaml | 5 +++++ 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 1fb66c4475..688c35724d 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -48,6 +48,8 @@ joint_trajectory_controller * -1 - The tolerance is "erased". If there was a default, the joint will be allowed to move without restriction. +* Add the boolean parameter ``set_last_command_interface_value_as_state_on_activation``. When set to ``true``, the last command interface value is used as both the current state and the last commanded state upon activation. When set to ``false``, the current state is used for both (`#1231 `_). + pid_controller ************************ * 🚀 The PID controller was added 🎉 (`#434 `_). diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index e4923604fd..bea37e02be 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -924,7 +924,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( // running already) trajectory_msgs::msg::JointTrajectoryPoint state; resize_joint_trajectory_point(state, dof_); - if (read_state_from_command_interfaces(state)) + if ( + params_.set_last_command_interface_value_as_state_on_activation && + read_state_from_command_interfaces(state)) { state_current_ = state; last_commanded_state_ = state; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 8bd64b6314..14b71f0711 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -64,6 +64,11 @@ joint_trajectory_controller: default_value: false, description: "Allow integration in goal trajectories to accept goals without position or velocity specified", } + set_last_command_interface_value_as_state_on_activation: { + type: bool, + default_value: true, + description: "When set to true, the last command interface value is used as both the current state and the last commanded state upon activation. When set to false, the current state is used for both.", + } action_monitor_rate: { type: double, default_value: 20.0, From 7c89c17a5cf6ac5d553c79ccc63182f2e1454388 Mon Sep 17 00:00:00 2001 From: RobertWilbrandt Date: Sun, 27 Oct 2024 00:03:25 +0200 Subject: [PATCH 61/97] Implement new PoseBroadcaster controller (#1311) --- doc/controllers_index.rst | 1 + pose_broadcaster/CMakeLists.txt | 106 ++++++++++ pose_broadcaster/README.md | 8 + pose_broadcaster/doc/userdoc.rst | 27 +++ .../pose_broadcaster/pose_broadcaster.hpp | 77 +++++++ .../pose_broadcaster/visibility_control.h | 49 +++++ pose_broadcaster/package.xml | 30 +++ pose_broadcaster/pose_broadcaster.xml | 9 + pose_broadcaster/src/pose_broadcaster.cpp | 195 +++++++++++++++++ .../src/pose_broadcaster_parameters.yaml | 32 +++ .../test/pose_broadcaster_params.yaml | 4 + .../test/test_load_pose_broadcaster.cpp | 50 +++++ .../test/test_pose_broadcaster.cpp | 198 ++++++++++++++++++ .../test/test_pose_broadcaster.hpp | 96 +++++++++ 14 files changed, 882 insertions(+) create mode 100644 pose_broadcaster/CMakeLists.txt create mode 100644 pose_broadcaster/README.md create mode 100644 pose_broadcaster/doc/userdoc.rst create mode 100644 pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp create mode 100644 pose_broadcaster/include/pose_broadcaster/visibility_control.h create mode 100644 pose_broadcaster/package.xml create mode 100644 pose_broadcaster/pose_broadcaster.xml create mode 100644 pose_broadcaster/src/pose_broadcaster.cpp create mode 100644 pose_broadcaster/src/pose_broadcaster_parameters.yaml create mode 100644 pose_broadcaster/test/pose_broadcaster_params.yaml create mode 100644 pose_broadcaster/test/test_load_pose_broadcaster.cpp create mode 100644 pose_broadcaster/test/test_pose_broadcaster.cpp create mode 100644 pose_broadcaster/test/test_pose_broadcaster.hpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 683f23c202..d5b956aa8a 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -71,6 +71,7 @@ In the sense of ros2_control, broadcasters are still controllers using the same IMU Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> Range Sensor Broadcaster <../range_sensor_broadcaster/doc/userdoc.rst> + Pose Broadcaster <../pose_broadcaster/doc/userdoc.rst> Common Controller Parameters diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt new file mode 100644 index 0000000000..46028cf258 --- /dev/null +++ b/pose_broadcaster/CMakeLists.txt @@ -0,0 +1,106 @@ +cmake_minimum_required(VERSION 3.16) +project(pose_broadcaster + LANGUAGES + CXX +) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2_msgs +) + +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(pose_broadcaster_parameters + src/pose_broadcaster_parameters.yaml +) + +add_library(pose_broadcaster SHARED + src/pose_broadcaster.cpp +) +target_compile_features(pose_broadcaster PUBLIC + cxx_std_17 +) +target_include_directories(pose_broadcaster PUBLIC + $ + $ +) +target_link_libraries(pose_broadcaster PUBLIC + pose_broadcaster_parameters +) +ament_target_dependencies(pose_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(pose_broadcaster PRIVATE "POSE_BROADCASTER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface pose_broadcaster.xml +) + +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) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_pose_broadcaster + test/test_load_pose_broadcaster.cpp + ) + target_link_libraries(test_load_pose_broadcaster + pose_broadcaster + ) + ament_target_dependencies(test_load_pose_broadcaster + controller_manager + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock(test_pose_broadcaster + test/test_pose_broadcaster.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pose_broadcaster_params.yaml + ) + target_link_libraries(test_pose_broadcaster + pose_broadcaster + ) + ament_target_dependencies(test_pose_broadcaster + hardware_interface + ) +endif() + +install( + DIRECTORY + include/ + DESTINATION include/${PROJECT_NAME} +) +install( + TARGETS + pose_broadcaster + pose_broadcaster_parameters + EXPORT export_${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/pose_broadcaster/README.md b/pose_broadcaster/README.md new file mode 100644 index 0000000000..bf47411048 --- /dev/null +++ b/pose_broadcaster/README.md @@ -0,0 +1,8 @@ +pose_broadcaster +========================================== + +Controller to publish poses provided by pose sensors. + +Pluginlib-Library: pose_broadcaster + +Plugin: pose_broadcaster/PoseBroadcaster (controller_interface::ControllerInterface) diff --git a/pose_broadcaster/doc/userdoc.rst b/pose_broadcaster/doc/userdoc.rst new file mode 100644 index 0000000000..0ae40e2fad --- /dev/null +++ b/pose_broadcaster/doc/userdoc.rst @@ -0,0 +1,27 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pose_broadcaster/doc/userdoc.rst + +.. _pose_broadcaster_userdoc: + +Pose Broadcaster +-------------------------------- +Broadcaster for poses measured by a robot or a sensor. +Poses are published as ``geometry_msgs/msg/PoseStamped`` messages and optionally as tf transforms. + +The controller is a wrapper around the ``PoseSensor`` semantic component (see ``controller_interface`` package). + +Parameters +^^^^^^^^^^^ +This controller 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. + +List of parameters +========================= +.. generate_parameter_library_details:: ../src/pose_broadcaster_parameters.yaml + + +An example parameter file +========================= + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/pose_broadcaster_params.yaml + :language: yaml diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp new file mode 100644 index 0000000000..621a90cc85 --- /dev/null +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -0,0 +1,77 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// 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 POSE_BROADCASTER__POSE_BROADCASTER_HPP_ +#define POSE_BROADCASTER__POSE_BROADCASTER_HPP_ + +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "pose_broadcaster/visibility_control.h" +#include "pose_broadcaster_parameters.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_publisher.h" +#include "semantic_components/pose_sensor.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +namespace pose_broadcaster +{ + +class PoseBroadcaster : public controller_interface::ControllerInterface +{ +public: + POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration + command_interface_configuration() const override; + + POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration + state_interface_configuration() const override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + std::shared_ptr param_listener_; + Params params_; + + std::unique_ptr pose_sensor_; + + rclcpp::Publisher::SharedPtr pose_publisher_; + std::unique_ptr> + realtime_publisher_; + + std::optional tf_publish_period_; + rclcpp::Time tf_last_publish_time_{0, 0, RCL_CLOCK_UNINITIALIZED}; + rclcpp::Publisher::SharedPtr tf_publisher_; + std::unique_ptr> + realtime_tf_publisher_; +}; + +} // namespace pose_broadcaster + +#endif // POSE_BROADCASTER__POSE_BROADCASTER_HPP_ diff --git a/pose_broadcaster/include/pose_broadcaster/visibility_control.h b/pose_broadcaster/include/pose_broadcaster/visibility_control.h new file mode 100644 index 0000000000..5ce272658d --- /dev/null +++ b/pose_broadcaster/include/pose_broadcaster/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// 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 POSE_BROADCASTER__VISIBILITY_CONTROL_H_ +#define POSE_BROADCASTER__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 POSE_BROADCASTER_EXPORT __attribute__((dllexport)) +#define POSE_BROADCASTER_IMPORT __attribute__((dllimport)) +#else +#define POSE_BROADCASTER_EXPORT __declspec(dllexport) +#define POSE_BROADCASTER_IMPORT __declspec(dllimport) +#endif +#ifdef POSE_BROADCASTER_BUILDING_DLL +#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_EXPORT +#else +#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_IMPORT +#endif +#define POSE_BROADCASTER_PUBLIC_TYPE POSE_BROADCASTER_PUBLIC +#define POSE_BROADCASTER_LOCAL +#else +#define POSE_BROADCASTER_EXPORT __attribute__((visibility("default"))) +#define POSE_BROADCASTER_IMPORT +#if __GNUC__ >= 4 +#define POSE_BROADCASTER_PUBLIC __attribute__((visibility("default"))) +#define POSE_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) +#else +#define POSE_BROADCASTER_PUBLIC +#define POSE_BROADCASTER_LOCAL +#endif +#define POSE_BROADCASTER_PUBLIC_TYPE +#endif + +#endif // POSE_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml new file mode 100644 index 0000000000..eae2ccad2c --- /dev/null +++ b/pose_broadcaster/package.xml @@ -0,0 +1,30 @@ + + + + pose_broadcaster + 0.0.0 + Broadcaster to publish cartesian states. + Robert Wilbrandt + + Apache License 2.0 + + ament_cmake + + backward_ros + controller_interface + generate_parameter_library + geometry_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2_msgs + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/pose_broadcaster/pose_broadcaster.xml b/pose_broadcaster/pose_broadcaster.xml new file mode 100644 index 0000000000..6578958004 --- /dev/null +++ b/pose_broadcaster/pose_broadcaster.xml @@ -0,0 +1,9 @@ + + + + This controller publishes a Cartesian state as a geometry_msgs/PoseStamped message and optionally as a tf transform. + + + diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp new file mode 100644 index 0000000000..7e3aeaddf3 --- /dev/null +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -0,0 +1,195 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// 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 "pose_broadcaster/pose_broadcaster.hpp" + +namespace +{ + +constexpr auto DEFAULT_POSE_TOPIC = "~/pose"; +constexpr auto DEFAULT_TF_TOPIC = "/tf"; + +} // namespace + +namespace pose_broadcaster +{ + +controller_interface::InterfaceConfiguration PoseBroadcaster::command_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration PoseBroadcaster::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = pose_sensor_->get_state_interface_names(); + + return state_interfaces_config; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & ex) + { + fprintf(stderr, "Exception thrown during init stage with message: %s\n", ex.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + pose_sensor_ = std::make_unique(params_.pose_name); + tf_publish_period_ = + params_.tf.publish_rate == 0.0 + ? std::nullopt + : std::optional{rclcpp::Duration::from_seconds(1.0 / params_.tf.publish_rate)}; + + try + { + pose_publisher_ = get_node()->create_publisher( + DEFAULT_POSE_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = + std::make_unique>( + pose_publisher_); + + if (params_.tf.enable) + { + tf_publisher_ = get_node()->create_publisher( + DEFAULT_TF_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_tf_publisher_ = + std::make_unique>( + tf_publisher_); + } + } + catch (const std::exception & ex) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message: %s\n", + ex.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // Initialize pose message + realtime_publisher_->lock(); + realtime_publisher_->msg_.header.frame_id = params_.frame_id; + realtime_publisher_->unlock(); + + // Initialize tf message if tf publishing is enabled + if (realtime_tf_publisher_) + { + realtime_tf_publisher_->lock(); + + realtime_tf_publisher_->msg_.transforms.resize(1); + auto & tf_transform = realtime_tf_publisher_->msg_.transforms.front(); + tf_transform.header.frame_id = params_.frame_id; + if (params_.tf.child_frame_id.empty()) + { + tf_transform.child_frame_id = params_.pose_name; + } + else + { + tf_transform.child_frame_id = params_.tf.child_frame_id; + } + + realtime_tf_publisher_->unlock(); + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + pose_sensor_->assign_loaned_state_interfaces(state_interfaces_); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + pose_sensor_->release_interfaces(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type PoseBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + geometry_msgs::msg::Pose pose; + pose_sensor_->get_values_as_message(pose); + + if (realtime_publisher_ && realtime_publisher_->trylock()) + { + realtime_publisher_->msg_.header.stamp = time; + realtime_publisher_->msg_.pose = pose; + realtime_publisher_->unlockAndPublish(); + } + + if (realtime_tf_publisher_ && realtime_tf_publisher_->trylock()) + { + bool do_publish = false; + // rlcpp::Time comparisons throw if clock types are not the same + if (tf_last_publish_time_.get_clock_type() != time.get_clock_type()) + { + do_publish = true; + } + else if (!tf_publish_period_ || (tf_last_publish_time_ + *tf_publish_period_ <= time)) + { + do_publish = true; + } + + if (do_publish) + { + auto & tf_transform = realtime_tf_publisher_->msg_.transforms[0]; + tf_transform.header.stamp = time; + + tf_transform.transform.translation.x = pose.position.x; + tf_transform.transform.translation.y = pose.position.y; + tf_transform.transform.translation.z = pose.position.z; + + tf_transform.transform.rotation.x = pose.orientation.x; + tf_transform.transform.rotation.y = pose.orientation.y; + tf_transform.transform.rotation.z = pose.orientation.z; + tf_transform.transform.rotation.w = pose.orientation.w; + + realtime_tf_publisher_->unlockAndPublish(); + + tf_last_publish_time_ = time; + } + else + { + realtime_tf_publisher_->unlock(); + } + } + + return controller_interface::return_type::OK; +} + +} // namespace pose_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(pose_broadcaster::PoseBroadcaster, controller_interface::ControllerInterface) diff --git a/pose_broadcaster/src/pose_broadcaster_parameters.yaml b/pose_broadcaster/src/pose_broadcaster_parameters.yaml new file mode 100644 index 0000000000..11c53b5e57 --- /dev/null +++ b/pose_broadcaster/src/pose_broadcaster_parameters.yaml @@ -0,0 +1,32 @@ +pose_broadcaster: + frame_id: + type: string + default_value: "" + description: "frame_id in which values are published" + validation: + not_empty<>: null + pose_name: + type: string + default_value: "" + description: "Base name used as prefix for controller interfaces. + The state interface names are: ``/position.x, ..., /position.z, + /orientation.x, ..., /orientation.w``" + validation: + not_empty<>: null + tf: + enable: + type: bool + default_value: true + description: "Whether to publish the pose as a tf transform" + child_frame_id: + type: string + default_value: "" + description: "Child frame id of published tf transforms. Defaults to ``pose_name`` if left + empty." + publish_rate: + type: double + default_value: 0.0 + description: "Rate to limit publishing of tf transforms to (Hz). If set to 0, no limiting is + performed." + validation: + gt_eq<>: 0.0 diff --git a/pose_broadcaster/test/pose_broadcaster_params.yaml b/pose_broadcaster/test/pose_broadcaster_params.yaml new file mode 100644 index 0000000000..a2f8477dd1 --- /dev/null +++ b/pose_broadcaster/test/pose_broadcaster_params.yaml @@ -0,0 +1,4 @@ +test_pose_broadcaster: + ros__parameters: + pose_name: "test_pose" + frame_id: "pose_frame" diff --git a/pose_broadcaster/test/test_load_pose_broadcaster.cpp b/pose_broadcaster/test/test_load_pose_broadcaster.cpp new file mode 100644 index 0000000000..bdf72d7b23 --- /dev/null +++ b/pose_broadcaster/test/test_load_pose_broadcaster.cpp @@ -0,0 +1,50 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// 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(TestLoadPoseBroadcaster, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm{ + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"}; + + const std::string test_file_path = + std::string{TEST_FILES_DIRECTORY} + "/pose_broadcaster_params.yaml"; + cm.set_parameter({"test_pose_broadcaster.params_file", test_file_path}); + + cm.set_parameter({"test_pose_broadcaster.type", "pose_broadcaster/PoseBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_pose_broadcaster"), nullptr); +} + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} diff --git a/pose_broadcaster/test/test_pose_broadcaster.cpp b/pose_broadcaster/test/test_pose_broadcaster.cpp new file mode 100644 index 0000000000..0ed2e84619 --- /dev/null +++ b/pose_broadcaster/test/test_pose_broadcaster.cpp @@ -0,0 +1,198 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// 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 "test_pose_broadcaster.hpp" + +#include +#include + +using hardware_interface::LoanedStateInterface; + +void PoseBroadcasterTest::SetUp() { pose_broadcaster_ = std::make_unique(); } + +void PoseBroadcasterTest::TearDown() { pose_broadcaster_.reset(nullptr); } + +void PoseBroadcasterTest::SetUpPoseBroadcaster() +{ + ASSERT_EQ( + pose_broadcaster_->init( + "test_pose_broadcaster", "", 0, "", pose_broadcaster_->define_custom_node_options()), + controller_interface::return_type::OK); + + std::vector state_interfaces; + state_interfaces.emplace_back(pose_position_x_); + state_interfaces.emplace_back(pose_position_y_); + state_interfaces.emplace_back(pose_position_z_); + state_interfaces.emplace_back(pose_orientation_x_); + state_interfaces.emplace_back(pose_orientation_y_); + state_interfaces.emplace_back(pose_orientation_z_); + state_interfaces.emplace_back(pose_orientation_w_); + + pose_broadcaster_->assign_interfaces({}, std::move(state_interfaces)); +} + +TEST_F(PoseBroadcasterTest, Configure_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command interface configuration + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ(command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + // Verify state interface configuration + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); +} + +TEST_F(PoseBroadcasterTest, Activate_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command and state interface configuration + { + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ( + command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); + } + + // Deactivate controller + ASSERT_EQ( + pose_broadcaster_->on_deactivate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command and state interface configuration + { + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ( + command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); // Should not change when deactivating + } +} + +TEST_F(PoseBroadcasterTest, Update_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + ASSERT_EQ( + pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PoseBroadcasterTest, PublishSuccess) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Set 'tf.enable' and 'tf.child_frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"tf.enable", true}); + pose_broadcaster_->get_node()->set_parameter({"tf.child_frame_id", tf_child_frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Subscribe to pose topic + geometry_msgs::msg::PoseStamped pose_msg; + subscribe_and_get_message("/test_pose_broadcaster/pose", pose_msg); + + // Verify content of pose message + EXPECT_EQ(pose_msg.header.frame_id, frame_id_); + EXPECT_EQ(pose_msg.pose.position.x, pose_values_[0]); + EXPECT_EQ(pose_msg.pose.position.y, pose_values_[1]); + EXPECT_EQ(pose_msg.pose.position.z, pose_values_[2]); + EXPECT_EQ(pose_msg.pose.orientation.x, pose_values_[3]); + EXPECT_EQ(pose_msg.pose.orientation.y, pose_values_[4]); + EXPECT_EQ(pose_msg.pose.orientation.z, pose_values_[5]); + EXPECT_EQ(pose_msg.pose.orientation.w, pose_values_[6]); + + // Subscribe to tf topic + tf2_msgs::msg::TFMessage tf_msg; + subscribe_and_get_message("/tf", tf_msg); + + // Verify content of tf message + ASSERT_EQ(tf_msg.transforms.size(), 1lu); + EXPECT_EQ(tf_msg.transforms[0].header.frame_id, frame_id_); + EXPECT_EQ(tf_msg.transforms[0].child_frame_id, tf_child_frame_id_); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.x, pose_values_[0]); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.y, pose_values_[1]); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.z, pose_values_[2]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.x, pose_values_[3]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.y, pose_values_[4]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.z, pose_values_[5]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.w, pose_values_[6]); +} + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} diff --git a/pose_broadcaster/test/test_pose_broadcaster.hpp b/pose_broadcaster/test/test_pose_broadcaster.hpp new file mode 100644 index 0000000000..a164b2c6ac --- /dev/null +++ b/pose_broadcaster/test/test_pose_broadcaster.hpp @@ -0,0 +1,96 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// 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_POSE_BROADCASTER_HPP_ +#define TEST_POSE_BROADCASTER_HPP_ + +#include + +#include +#include +#include + +#include "rclcpp/executors.hpp" + +#include "pose_broadcaster/pose_broadcaster.hpp" + +using pose_broadcaster::PoseBroadcaster; + +class PoseBroadcasterTest : public ::testing::Test +{ +public: + void SetUp(); + void TearDown(); + + void SetUpPoseBroadcaster(); + +protected: + const std::string pose_name_ = "test_pose"; + const std::string frame_id_ = "pose_base_frame"; + const std::string tf_child_frame_id_ = "pose_frame"; + + std::array pose_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7}; + + hardware_interface::StateInterface pose_position_x_{pose_name_, "position.x", &pose_values_[0]}; + hardware_interface::StateInterface pose_position_y_{pose_name_, "position.x", &pose_values_[1]}; + hardware_interface::StateInterface pose_position_z_{pose_name_, "position.x", &pose_values_[2]}; + hardware_interface::StateInterface pose_orientation_x_{ + pose_name_, "orientation.x", &pose_values_[3]}; + hardware_interface::StateInterface pose_orientation_y_{ + pose_name_, "orientation.y", &pose_values_[4]}; + hardware_interface::StateInterface pose_orientation_z_{ + pose_name_, "orientation.z", &pose_values_[5]}; + hardware_interface::StateInterface pose_orientation_w_{ + pose_name_, "orientation.w", &pose_values_[6]}; + + std::unique_ptr pose_broadcaster_; + + template + void subscribe_and_get_message(const std::string & topic, T & msg); +}; + +template +void PoseBroadcasterTest::subscribe_and_get_message(const std::string & topic, T & msg) +{ + // Create node for subscribing + rclcpp::Node node{"test_subscription_node"}; + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node.get_node_base_interface()); + + // Create subscription + typename T::SharedPtr received_msg; + const auto msg_callback = [&](const typename T::SharedPtr sub_msg) { received_msg = sub_msg; }; + const auto subscription = node.create_subscription(topic, 10, msg_callback); + + // Update controller and spin until a message is received + // Since update doesn't guarantee a published message, republish until received + constexpr size_t max_sub_check_loop_count = 5; + for (size_t i = 0; !received_msg; ++i) + { + ASSERT_LT(i, max_sub_check_loop_count); + + pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01)); + + const auto timeout = std::chrono::milliseconds{5}; + const auto until = node.get_clock()->now() + timeout; + while (!received_msg && node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds{10}); + } + } + + msg = *received_msg; +} + +#endif // TEST_POSE_BROADCASTER_HPP_ From fa42b5ec97b0af5420060844b7027b8e8912c05d Mon Sep 17 00:00:00 2001 From: Gilmar Correia Date: Thu, 31 Oct 2024 08:46:39 -0300 Subject: [PATCH 62/97] fixes for windows compilation (#1330) Co-authored-by: SENAI-GilmarCorreia --- .../include/joint_trajectory_controller/tolerances.hpp | 2 +- pid_controller/CMakeLists.txt | 10 ++++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 8c556703ab..4902fd1dcc 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -133,7 +133,7 @@ double resolve_tolerance_source(const double default_value, const double goal_va // * -1 - The tolerance is "erased". // If there was a default, the joint will be allowed to move without restriction. constexpr double ERASE_VALUE = -1.0; - auto is_erase_value = [](double value) + auto is_erase_value = [=](double value) { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; if (goal_value > 0.0) diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index 6c9e00ef8b..f9a9abce89 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -5,6 +5,16 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() +if(WIN32) + add_compile_definitions( + # For math constants + _USE_MATH_DEFINES + # Minimize Windows namespace collision + NOMINMAX + WIN32_LEAN_AND_MEAN + ) +endif() + set(THIS_PACKAGE_INCLUDE_DEPENDS angles control_msgs From feaf122bcb15113d3c446e03bb1df6ff2e3ba6aa Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Fri, 1 Nov 2024 09:49:37 +0100 Subject: [PATCH 63/97] Bump version of pre-commit hooks (#1334) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 63e7f08682..205e0f63ab 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -16,7 +16,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.6.0 + rev: v5.0.0 hooks: - id: check-added-large-files - id: check-ast @@ -37,7 +37,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.17.0 + rev: v3.19.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -50,7 +50,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/psf/black - rev: 24.8.0 + rev: 24.10.0 hooks: - id: black args: ["--line-length=99"] @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.0 + rev: v19.1.3 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.3 + rev: 0.29.4 hooks: - id: check-github-workflows args: ["--verbose"] From 1716351d0a9b8f3cef9281095d9a72d5621643c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 1 Nov 2024 11:34:01 +0100 Subject: [PATCH 64/97] Add hardware_interface_testing dependency (#1335) --- pose_broadcaster/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index eae2ccad2c..4c3506f1cd 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -22,6 +22,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets From 87f21b39c5e81327eb8adf8ed16e5fde1101bd97 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 1 Nov 2024 20:13:05 +0100 Subject: [PATCH 65/97] [JSB] Fix the behaviour of publishing unavailable state interfaces when they are previously available (#1331) * Add test to reproduce the behaviour of https://github.com/ros-controls/ros2_control_demos/pull/417#discussion_r1823443110 * Add fix to solve the issue of the publishing non-existing joint_states --- .../src/joint_state_broadcaster.cpp | 3 + .../test/test_joint_state_broadcaster.cpp | 96 +++++++++++++++++++ .../test/test_joint_state_broadcaster.hpp | 1 + 3 files changed, 100 insertions(+) diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index fe0b32213a..9bbb862925 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -209,6 +209,7 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { joint_names_.clear(); + name_if_value_mapping_.clear(); return CallbackReturn::SUCCESS; } @@ -309,6 +310,8 @@ void JointStateBroadcaster::init_joint_state_msg() void JointStateBroadcaster::init_dynamic_joint_state_msg() { auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_; + dynamic_joint_state_msg.joint_names.clear(); + dynamic_joint_state_msg.interface_values.clear(); for (const auto & name_ifv : name_if_value_mapping_) { const auto & name = name_ifv.first; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 877d199419..de534c00b6 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -216,6 +216,102 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) ElementsAreArray(interface_names_)); } +TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest) +{ + // publishers not initialized yet + ASSERT_FALSE(state_broadcaster_->joint_state_publisher_); + ASSERT_FALSE(state_broadcaster_->dynamic_joint_state_publisher_); + + SetUpStateBroadcaster(); + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = joint_names_.size(); + + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + + // dynamic joint state initialized + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); + + // Now deactivate and activate with only 2 set of joints and interfaces (to create as in one of + // the interface is unavailable) + ASSERT_EQ(state_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + const std::vector JOINT_NAMES = {"joint1", "joint2"}; + assign_state_interfaces(JOINT_NAMES, interface_names_); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS_WITH_ONE_DEACTIVATED = JOINT_NAMES.size(); + + // check interface configuration + cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & new_joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(new_joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(new_joint_state_msg.position, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT(new_joint_state_msg.velocity, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT(new_joint_state_msg.effort, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + + // dynamic joint state initialized + const auto & new_dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(new_dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT(new_dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); +} + TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) { const std::vector JOINT_NAMES = {}; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 0b4c50e89e..4f4241d3d7 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -34,6 +34,7 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr { FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest); FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyTest); + FRIEND_TEST(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDescription); From 4343c7a4632a97f52fc664b5dcdcad258ffb5e2a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 4 Nov 2024 12:50:08 +0100 Subject: [PATCH 66/97] [ForceTorqueSensorBroadcaster] added force and torque offsets to the parameters + export state interfaces (#1215) --- .../force_torque_sensor_broadcaster.xml | 2 +- .../force_torque_sensor_broadcaster.hpp | 16 +++- .../src/force_torque_sensor_broadcaster.cpp | 80 ++++++++++++++++++- ..._torque_sensor_broadcaster_parameters.yaml | 33 ++++++++ .../test_force_torque_sensor_broadcaster.cpp | 78 ++++++++++++++++++ 5 files changed, 202 insertions(+), 7 deletions(-) diff --git a/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml b/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml index 10d19a93c5..8e61e013de 100644 --- a/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml +++ b/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml @@ -1,6 +1,6 @@ + type="force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster" base_class_type="controller_interface::ChainableControllerInterface"> This controller publishes the readings of force-torque interfaces as geometry_msgs/WrenchStamped message. 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 bd477ed68a..e5a5349c32 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 @@ -20,8 +20,9 @@ #define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ #include +#include -#include "controller_interface/controller_interface.hpp" +#include "controller_interface/chainable_controller_interface.hpp" #include "force_torque_sensor_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "force_torque_sensor_broadcaster_parameters.hpp" @@ -32,7 +33,7 @@ namespace force_torque_sensor_broadcaster { -class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInterface +class ForceTorqueSensorBroadcaster : public controller_interface::ChainableControllerInterface { public: FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC @@ -59,10 +60,19 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte const rclcpp_lifecycle::State & previous_state) override; FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC - controller_interface::return_type update( + controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; + FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC + std::vector on_export_state_interfaces() override; + protected: + void apply_sensor_offset(const Params & params, geometry_msgs::msg::WrenchStamped & msg); + std::shared_ptr param_listener_; Params params_; 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 9b570d353f..ae105a511c 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -24,7 +24,7 @@ namespace force_torque_sensor_broadcaster { ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster() -: controller_interface::ControllerInterface() +: controller_interface::ChainableControllerInterface() { } @@ -141,23 +141,97 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type ForceTorqueSensorBroadcaster::update( +controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + } if (realtime_publisher_ && realtime_publisher_->trylock()) { realtime_publisher_->msg_.header.stamp = time; force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench); + this->apply_sensor_offset(params_, realtime_publisher_->msg_); realtime_publisher_->unlockAndPublish(); } return controller_interface::return_type::OK; } +controller_interface::return_type ForceTorqueSensorBroadcaster::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + return controller_interface::return_type::OK; +} + +std::vector +ForceTorqueSensorBroadcaster::on_export_state_interfaces() +{ + std::vector exported_state_interfaces; + + std::vector force_names( + {params_.interface_names.force.x, params_.interface_names.force.y, + params_.interface_names.force.z}); + std::vector torque_names( + {params_.interface_names.torque.x, params_.interface_names.torque.y, + params_.interface_names.torque.z}); + if (!params_.sensor_name.empty()) + { + const auto semantic_comp_itf_names = force_torque_sensor_->get_state_interface_names(); + std::copy( + semantic_comp_itf_names.begin(), semantic_comp_itf_names.begin() + 3, force_names.begin()); + std::copy( + semantic_comp_itf_names.begin() + 3, semantic_comp_itf_names.end(), torque_names.begin()); + } + const std::string controller_name = get_node()->get_name(); + if (!force_names[0].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, force_names[0], &realtime_publisher_->msg_.wrench.force.x)); + } + if (!force_names[1].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, force_names[1], &realtime_publisher_->msg_.wrench.force.y)); + } + if (!force_names[2].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, force_names[2], &realtime_publisher_->msg_.wrench.force.z)); + } + if (!torque_names[0].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, torque_names[0], &realtime_publisher_->msg_.wrench.torque.x)); + } + if (!torque_names[1].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, torque_names[1], &realtime_publisher_->msg_.wrench.torque.y)); + } + if (!torque_names[2].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, torque_names[2], &realtime_publisher_->msg_.wrench.torque.z)); + } + return exported_state_interfaces; +} + +void ForceTorqueSensorBroadcaster::apply_sensor_offset( + const Params & params, geometry_msgs::msg::WrenchStamped & msg) +{ + msg.wrench.force.x += params.offset.force.x; + msg.wrench.force.y += params.offset.force.y; + msg.wrench.force.z += params.offset.force.z; + msg.wrench.torque.x += params.offset.torque.x; + msg.wrench.torque.y += params.offset.torque.y; + msg.wrench.torque.z += params.offset.torque.z; +} } // namespace force_torque_sensor_broadcaster #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster, - controller_interface::ControllerInterface) + controller_interface::ChainableControllerInterface) 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 index 3e75ab6012..0869f5cf3c 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -46,3 +46,36 @@ force_torque_sensor_broadcaster: default_value: "", description: "Name of the state interface with torque values around 'z' axis.", } + offset: + force: + x: { + type: double, + default_value: 0.0, + description: "The offset of force values on 'x' axis.", + } + y: { + type: double, + default_value: 0.0, + description: "The offset of force values on 'y' axis.", + } + z: { + type: double, + default_value: 0.0, + description: "The offset of force values on 'z' axis.", + } + torque: + x: { + type: double, + default_value: 0.0, + description: "The offset of torque values around 'x' axis.", + } + y: { + type: double, + default_value: 0.0, + description: "The offset of torque values around 'y' axis.", + } + z: { + type: double, + default_value: 0.0, + description: "The offset of torque values around 'z' axis.", + } diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 1ea25520cc..e436beb2e5 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -278,6 +278,60 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); } +TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets) +{ + SetUpFTSBroadcaster(); + + std::array force_offsets = {10.0, 30.0, -50.0}; + std::array torque_offsets = {1.0, -1.2, -5.2}; + // set the params 'sensor_name' and 'frame_id' + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->set_parameter({"offset.force.x", force_offsets[0]}); + fts_broadcaster_->get_node()->set_parameter({"offset.force.y", force_offsets[1]}); + fts_broadcaster_->get_node()->set_parameter({"offset.force.z", force_offsets[2]}); + fts_broadcaster_->get_node()->set_parameter({"offset.torque.x", torque_offsets[0]}); + fts_broadcaster_->get_node()->set_parameter({"offset.torque.y", torque_offsets[1]}); + fts_broadcaster_->get_node()->set_parameter({"offset.torque.z", torque_offsets[2]}); + + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + geometry_msgs::msg::WrenchStamped wrench_msg; + subscribe_and_get_message(wrench_msg); + + ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); + ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0] + force_offsets[0]); + ASSERT_EQ(wrench_msg.wrench.force.y, sensor_values_[1] + force_offsets[1]); + ASSERT_EQ(wrench_msg.wrench.force.z, sensor_values_[2] + force_offsets[2]); + ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3] + torque_offsets[0]); + ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4] + torque_offsets[1]); + ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5] + torque_offsets[2]); + + // Check the exported state interfaces + const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 6u); + const std::string controller_name = fts_broadcaster_->get_node()->get_name(); + ASSERT_EQ( + exported_state_interfaces[0]->get_name(), controller_name + "/" + sensor_name_ + "/force.x"); + ASSERT_EQ( + exported_state_interfaces[1]->get_name(), controller_name + "/" + sensor_name_ + "/force.y"); + ASSERT_EQ( + exported_state_interfaces[2]->get_name(), controller_name + "/" + sensor_name_ + "/force.z"); + ASSERT_EQ( + exported_state_interfaces[3]->get_name(), controller_name + "/" + sensor_name_ + "/torque.x"); + ASSERT_EQ( + exported_state_interfaces[4]->get_name(), controller_name + "/" + sensor_name_ + "/torque.y"); + ASSERT_EQ( + exported_state_interfaces[5]->get_name(), controller_name + "/" + sensor_name_ + "/torque.z"); + for (size_t i = 0; i < 6; ++i) + { + ASSERT_EQ( + exported_state_interfaces[i]->get_value(), + sensor_values_[i] + (i < 3 ? force_offsets[i] : torque_offsets[i - 3])); + } +} + TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) { SetUpFTSBroadcaster(); @@ -300,6 +354,15 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) ASSERT_TRUE(std::isnan(wrench_msg.wrench.torque.x)); ASSERT_TRUE(std::isnan(wrench_msg.wrench.torque.y)); ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); + + // Check the exported state interfaces + const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 2u); + const std::string controller_name = fts_broadcaster_->get_node()->get_name(); + ASSERT_EQ(exported_state_interfaces[0]->get_name(), controller_name + "/fts_sensor/force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_name(), controller_name + "/fts_sensor/torque.z"); + ASSERT_EQ(exported_state_interfaces[0]->get_value(), sensor_values_[0]); + ASSERT_EQ(exported_state_interfaces[1]->get_value(), sensor_values_[5]); } TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) @@ -328,6 +391,21 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3]); ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4]); ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); + + // Check the exported state interfaces + const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 6u); + const std::string controller_name = fts_broadcaster_->get_node()->get_name(); + ASSERT_EQ(exported_state_interfaces[0]->get_name(), controller_name + "/fts_sensor/force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_name(), controller_name + "/fts_sensor/force.y"); + ASSERT_EQ(exported_state_interfaces[2]->get_name(), controller_name + "/fts_sensor/force.z"); + ASSERT_EQ(exported_state_interfaces[3]->get_name(), controller_name + "/fts_sensor/torque.x"); + ASSERT_EQ(exported_state_interfaces[4]->get_name(), controller_name + "/fts_sensor/torque.y"); + ASSERT_EQ(exported_state_interfaces[5]->get_name(), controller_name + "/fts_sensor/torque.z"); + for (size_t i = 0; i < 6; ++i) + { + ASSERT_EQ(exported_state_interfaces[i]->get_value(), sensor_values_[i]); + } } int main(int argc, char ** argv) From f2da66240082c1abf2e25a624640b148171e861d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 5 Nov 2024 16:50:59 +0100 Subject: [PATCH 67/97] Update ros2_controllers.humble.repos (#1350) --- ros2_controllers.humble.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index 2a3b96551d..a0432f65b3 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -10,7 +10,7 @@ repositories: kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git - version: master + version: humble control_msgs: type: git url: https://github.com/ros-controls/control_msgs.git From 9439764015ef4a8b245ed07381459c19883e5a2b Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 6 Nov 2024 22:47:17 +0100 Subject: [PATCH 68/97] Adding use of robot description parameter in the Admittance Controller (#1247) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Co-authored-by: Kevin DeMarco Co-authored-by: Nikola Banovic Co-authored-by: Bence Magyar Co-authored-by: Christoph Fröhlich --- .../include/admittance_controller/admittance_rule.hpp | 3 ++- .../admittance_controller/admittance_rule_impl.hpp | 6 ++++-- admittance_controller/src/admittance_controller.cpp | 4 +++- .../test/test_admittance_controller.hpp | 9 +++------ 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 7223dbe9d1..a326b663d0 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -102,7 +102,8 @@ class AdmittanceRule /// Configure admittance rule memory using number of joints. controller_interface::return_type configure( - const std::shared_ptr & node, const size_t num_joint); + const std::shared_ptr & node, const size_t num_joint, + const std::string & robot_description); /// Reset all values back to default controller_interface::return_type reset(const size_t num_joints); diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index cab8b4cf45..13d4e67fbc 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -20,6 +20,7 @@ #include "admittance_controller/admittance_rule.hpp" #include +#include #include #include @@ -34,7 +35,8 @@ constexpr auto NUM_CARTESIAN_DOF = 6; // (3 translation + 3 rotation) /// 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) + const std::shared_ptr & node, const size_t num_joints, + const std::string & robot_description) { num_joints_ = num_joints; @@ -58,7 +60,7 @@ controller_interface::return_type AdmittanceRule::configure( kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); if (!kinematics_->initialize( - node->get_node_parameters_interface(), parameters_.kinematics.tip)) + robot_description, node->get_node_parameters_interface(), "kinematics")) { return controller_interface::return_type::ERROR; } diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index a4b56d739c..6e0e38140a 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -280,7 +280,9 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name)); // configure admittance rule - if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR) + if ( + admittance_->configure(get_node(), num_joints_, this->get_robot_description()) == + controller_interface::return_type::ERROR) { return controller_interface::CallbackReturn::ERROR; } diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index b2a95c12fa..4f4635d861 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -102,7 +102,6 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon } } -private: 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; }; @@ -110,10 +109,7 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon class AdmittanceControllerTest : public ::testing::Test { public: - static void SetUpTestCase() - { - // rclcpp::init(0, nullptr); - } + static void SetUpTestCase() {} void SetUp() { @@ -163,7 +159,8 @@ class AdmittanceControllerTest : public ::testing::Test controller_interface::return_type SetUpControllerCommon( const std::string & controller_name, const rclcpp::NodeOptions & options) { - auto result = controller_->init(controller_name, "", 0, "", options); + auto result = + controller_->init(controller_name, controller_->robot_description_, 0, "", options); controller_->export_reference_interfaces(); assign_interfaces(); From b0391e2069c8ec9a94ce35cb3e395bac3ca62eb6 Mon Sep 17 00:00:00 2001 From: RobertWilbrandt Date: Thu, 7 Nov 2024 09:06:38 +0100 Subject: [PATCH 69/97] [jtc] Improve trajectory sampling efficiency (#1297) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Co-authored-by: Christoph Fröhlich Co-authored-by: Bence Magyar --- .../trajectory.hpp | 21 +++- .../src/trajectory.cpp | 5 +- .../test/test_trajectory.cpp | 115 ++++++++++++++++++ 3 files changed, 137 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index b00d79481c..14373b006e 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -69,6 +69,9 @@ class Trajectory * acceleration respectively. Deduction assumes that the provided velocity or acceleration have to * be reached at the time defined in the segment. * + * This function assumes that sampling is only done at monotonically increasing \p sample_time + * for any trajectory. + * * Specific case returns for start_segment_itr and end_segment_itr: * - Sampling before the trajectory start: * start_segment_itr = begin(), end_segment_itr = begin() @@ -85,9 +88,12 @@ class Trajectory * * \param[in] sample_time Time at which trajectory will be sampled. * \param[in] interpolation_method Specify whether splines, another method, or no interpolation at - * all. \param[out] expected_state Calculated new at \p sample_time. \param[out] start_segment_itr - * Iterator to the start segment for given \p sample_time. See description above. \param[out] - * end_segment_itr Iterator to the end segment for given \p sample_time. See description above. + * all. + * \param[out] output_state Calculated new at \p sample_time. + * \param[out] start_segment_itr Iterator to the start segment for given \p sample_time. See + * description above. + * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See + * description above. */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool sample( @@ -147,6 +153,14 @@ class Trajectory JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool is_sampled_already() const { return sampled_already_; } + /// Get the index of the segment start returned by the last \p sample() operation. + /** + * As the trajectory is only accessed at monotonically increasing sampling times, this index is + * used to speed up the selection of relevant trajectory points. + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + size_t last_sample_index() const { return last_sample_idx_; } + private: void deduce_from_derivatives( trajectory_msgs::msg::JointTrajectoryPoint & first_state, @@ -160,6 +174,7 @@ class Trajectory trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_; bool sampled_already_ = false; + size_t last_sample_idx_ = 0; }; /** diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 54f785a070..512b16f3c5 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -84,6 +84,7 @@ void Trajectory::update(std::shared_ptr j trajectory_msg_ = joint_trajectory; trajectory_start_time_ = static_cast(joint_trajectory->header.stamp); sampled_already_ = false; + last_sample_idx_ = 0; } bool Trajectory::sample( @@ -149,7 +150,7 @@ bool Trajectory::sample( // time_from_start + trajectory time is the expected arrival time of trajectory const auto last_idx = trajectory_msg_->points.size() - 1; - for (size_t i = 0; i < last_idx; ++i) + for (size_t i = last_sample_idx_; i < last_idx; ++i) { auto & point = trajectory_msg_->points[i]; auto & next_point = trajectory_msg_->points[i + 1]; @@ -175,6 +176,7 @@ bool Trajectory::sample( } start_segment_itr = begin() + i; end_segment_itr = begin() + (i + 1); + last_sample_idx_ = i; return true; } } @@ -182,6 +184,7 @@ bool Trajectory::sample( // whole animation has played out start_segment_itr = --end(); end_segment_itr = end(); + last_sample_idx_ = last_idx; output_state = (*start_segment_itr); // the trajectories in msg may have empty velocities/accel, so resize them if (output_state.velocities.empty()) diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index b0d7ad8e18..44f511aa31 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -44,10 +44,12 @@ TEST(TestTrajectory, initialize_trajectory) empty_msg->header.stamp.nanosec = 2; const rclcpp::Time empty_time = empty_msg->header.stamp; auto traj = joint_trajectory_controller::Trajectory(empty_msg); + EXPECT_EQ(0, traj.last_sample_index()); trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -64,6 +66,7 @@ TEST(TestTrajectory, initialize_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -109,6 +112,7 @@ TEST(TestTrajectory, sample_trajectory_positions) // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -121,6 +125,7 @@ TEST(TestTrajectory, sample_trajectory_positions) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(result, false); } @@ -129,6 +134,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); double half_current_to_p1 = (point_before_msg.positions[0] + p1.positions[0]) * 0.5; @@ -142,6 +148,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p1.positions[0], expected_state.positions[0], EPS); @@ -154,6 +161,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); double half_p1_to_p2 = (p1.positions[0] + p2.positions[0]) * 0.5; @@ -165,6 +173,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); double half_p2_to_p3 = (p2.positions[0] + p3.positions[0]) * 0.5; EXPECT_NEAR(half_p2_to_p3, expected_state.positions[0], EPS); } @@ -174,6 +183,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -182,6 +192,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -193,6 +204,7 @@ TEST(TestTrajectory, sample_trajectory_positions) traj.sample( time_now + rclcpp::Duration::from_seconds(30.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -350,6 +362,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(traj.last_sample_index(), 0); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -362,6 +375,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(result, false); } @@ -370,6 +384,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); double half_current_to_p1 = @@ -390,6 +405,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -404,6 +420,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); double half_p1_to_p2 = @@ -424,6 +441,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); @@ -438,6 +456,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); double half_p2_to_p3 = @@ -458,6 +477,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -471,6 +491,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -523,6 +544,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -536,6 +558,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(result, false); } @@ -544,6 +567,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); // double half_current_to_p1 = point_before_msg.positions[0] + @@ -564,6 +588,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -615,6 +640,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) // sample at trajectory starting time { traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -628,6 +654,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(result, false); } @@ -642,6 +669,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -658,6 +686,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); @@ -674,6 +703,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -686,6 +716,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -732,6 +763,7 @@ TEST(TestTrajectory, skip_interpolation) // sample at trajectory starting time { traj.sample(time_now, no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -747,6 +779,7 @@ TEST(TestTrajectory, skip_interpolation) bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(result, false); } @@ -755,6 +788,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); // For passthrough, this should just return the first waypoint @@ -771,6 +805,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); @@ -786,6 +821,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); @@ -796,6 +832,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), no_interpolation, expected_state, start, end); + EXPECT_EQ(1, traj.last_sample_index()); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -804,6 +841,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), no_interpolation, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -812,6 +850,7 @@ TEST(TestTrajectory, skip_interpolation) traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), no_interpolation, expected_state, start, end); + EXPECT_EQ(2, traj.last_sample_index()); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -819,6 +858,82 @@ TEST(TestTrajectory, skip_interpolation) } } +TEST(TestTrajectory, update_trajectory) +{ + // Verify that sampling works correctly after updating with a new trajectory + auto first_msg = std::make_shared(); + first_msg->header.stamp = rclcpp::Time(0); + + trajectory_msgs::msg::JointTrajectoryPoint p1; + p1.positions.push_back(1.0); + p1.time_from_start = rclcpp::Duration::from_seconds(1.0); + first_msg->points.push_back(p1); + + trajectory_msgs::msg::JointTrajectoryPoint p2; + p2.positions.push_back(2.0); + p2.time_from_start = rclcpp::Duration::from_seconds(2.0); + first_msg->points.push_back(p2); + + trajectory_msgs::msg::JointTrajectoryPoint p3; + p3.positions.push_back(3.0); + p3.time_from_start = rclcpp::Duration::from_seconds(3.0); + first_msg->points.push_back(p3); + + trajectory_msgs::msg::JointTrajectoryPoint point_before_msg; + point_before_msg.time_from_start = rclcpp::Duration::from_seconds(0.0); + point_before_msg.positions.push_back(0.0); + + const rclcpp::Time time_now = rclcpp::Clock().now(); + auto traj = joint_trajectory_controller::Trajectory(time_now, point_before_msg, first_msg); + EXPECT_EQ(0, traj.last_sample_index()); + + trajectory_msgs::msg::JointTrajectoryPoint expected_state; + joint_trajectory_controller::TrajectoryPointConstIter start, end; + + // Sample at starting time + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); + + // Sample 2.5s after msg + traj.sample( + time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(1, traj.last_sample_index()); + + // Update trajectory + auto snd_msg = std::make_shared(); + snd_msg->header.stamp = rclcpp::Time(0); + + snd_msg->points.push_back(p1); + snd_msg->points.push_back(p2); + snd_msg->points.push_back(p3); + + traj.update(snd_msg); + + // Sample at starting time + { + traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + EXPECT_EQ(0, traj.last_sample_index()); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(traj.begin(), end); + EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); + EXPECT_NEAR( + (p1.positions[0] - point_before_msg.positions[0]), expected_state.velocities[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); + } + + // Sample 1.5s after msg + { + traj.sample( + time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, + end); + EXPECT_EQ(0, traj.last_sample_index()); + EXPECT_EQ(traj.begin(), start); + EXPECT_EQ(std::next(traj.begin()), end); + EXPECT_NEAR((p1.positions[0] + p2.positions[0]) / 2, expected_state.positions[0], EPS); + } +} + TEST(TestWrapAroundJoint, no_wraparound) { const std::vector initial_position(3, 0.); From 7ed1a0ee49187139bcf2b574b0b7c992fe0d06a3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 11:43:45 +0100 Subject: [PATCH 70/97] [JTC] Fix the JTC length_error exceptions in the tests (#1360) --- .../test/test_trajectory_controller_utils.hpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 796503c036..a850891331 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -26,6 +26,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "joint_trajectory_controller/tolerances.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "ros2_control_test_assets/descriptions.hpp" namespace @@ -391,6 +392,21 @@ class TrajectoryControllerTest : public ::testing::Test return traj_controller_->get_node()->activate(); } + void DeactivateTrajectoryController() + { + if (traj_controller_) + { + if ( + traj_controller_->get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + EXPECT_EQ( + traj_controller_->get_node()->deactivate().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + } + } + } + static void TearDownTestCase() { rclcpp::shutdown(); } void subscribeToState(rclcpp::Executor & executor) @@ -776,6 +792,8 @@ class TrajectoryControllerTestParameterized state_interface_types_ = std::get<1>(GetParam()); } + virtual void TearDown() { TrajectoryControllerTest::DeactivateTrajectoryController(); } + static void TearDownTestCase() { TrajectoryControllerTest::TearDownTestCase(); } }; From 55ed331f761113be5f3acf3018ffde2f78872f6c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 8 Nov 2024 10:56:32 +0000 Subject: [PATCH 71/97] Update changelogs --- ackermann_steering_controller/CHANGELOG.rst | 3 + admittance_controller/CHANGELOG.rst | 5 + bicycle_steering_controller/CHANGELOG.rst | 3 + diff_drive_controller/CHANGELOG.rst | 3 + effort_controllers/CHANGELOG.rst | 3 + force_torque_sensor_broadcaster/CHANGELOG.rst | 5 + forward_command_controller/CHANGELOG.rst | 3 + gripper_controllers/CHANGELOG.rst | 3 + imu_sensor_broadcaster/CHANGELOG.rst | 3 + joint_state_broadcaster/CHANGELOG.rst | 5 + joint_trajectory_controller/CHANGELOG.rst | 8 + parallel_gripper_controller/CHANGELOG.rst | 3 + pid_controller/CHANGELOG.rst | 5 + pose_broadcaster/CHANGELOG.rst | 210 ++++++++++++++++++ pose_broadcaster/package.xml | 2 +- position_controllers/CHANGELOG.rst | 3 + range_sensor_broadcaster/CHANGELOG.rst | 3 + ros2_controllers/CHANGELOG.rst | 3 + ros2_controllers_test_nodes/CHANGELOG.rst | 3 + rqt_joint_trajectory_controller/CHANGELOG.rst | 3 + steering_controllers_library/CHANGELOG.rst | 3 + tricycle_controller/CHANGELOG.rst | 3 + tricycle_steering_controller/CHANGELOG.rst | 3 + velocity_controllers/CHANGELOG.rst | 3 + 24 files changed, 290 insertions(+), 1 deletion(-) create mode 100644 pose_broadcaster/CHANGELOG.rst diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index bc2997b8a9..8f13e3cb6f 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 46899f9cd6..bff4354f4a 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adding use of robot description parameter in the Admittance Controller (`#1247 `_) +* Contributors: Dr. Denis, Kevin DeMarco, Nikola Banovic, Bence Magyar, Christoph Fröhlich + 4.15.0 (2024-10-07) ------------------- diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 0d096951ca..82a4e610bb 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 4c48fb234d..e806e497af 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index ea2efdcf4e..2cc63d7c90 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 7d2a7a1a8a..258b1d52a3 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 +----------- +* [ForceTorqueSensorBroadcaster] added force and torque offsets to the parameters + export state interfaces (`#1215 `_) +* Contributors: Sai Kishor Kothakota + 4.15.0 (2024-10-07) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 54caa9d764..8130b880b3 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 11c61f86cf..5feab361b1 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 40af7a5360..214b914e08 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index cf422e3eb8..47ce981e63 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JSB] Fix the behaviour of publishing unavailable state interfaces when they are previously available (`#1331 `_) +* Contributors: Sai Kishor Kothakota + 4.15.0 (2024-10-07) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 81696246b1..e873080f17 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [JTC] Fix the JTC length_error exceptions in the tests (`#1360 `_) +* [jtc] Improve trajectory sampling efficiency (`#1297 `_) +* fixes for windows compilation (`#1330 `_) +* [JTC] Add Parameter to Toggle State Setting on Activation (`#1231 `_) +* Contributors: Gilmar Correia, Kenta Kato, RobertWilbrandt, Sai Kishor Kothakota + 4.15.0 (2024-10-07) ------------------- diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index a3024fbf62..da2098ecd0 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 5a27471199..c4ce2430f6 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fixes for windows compilation (`#1330 `_) +* Contributors: Gilmar Correia + 4.15.0 (2024-10-07) ------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst new file mode 100644 index 0000000000..729df5a2f9 --- /dev/null +++ b/pose_broadcaster/CHANGELOG.rst @@ -0,0 +1,210 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pose_broadcaster +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Add hardware_interface_testing dependency (`#1335 `_) +* Implement new PoseBroadcaster controller (`#1311 `_) +* Contributors: Christoph Fröhlich, RobertWilbrandt + +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- + +4.11.0 (2024-07-09) +------------------- + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-02-12) +------------------ + +4.5.0 (2024-01-31) +------------------ + +4.4.0 (2024-01-11) +------------------ + +4.3.0 (2024-01-08) +------------------ + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-12-01) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.17.0 (2023-10-31) +------------------- + +3.16.0 (2023-09-20) +------------------- + +3.15.0 (2023-09-11) +------------------- + +3.14.0 (2023-08-16) +------------------- + +3.13.0 (2023-08-04) +------------------- + +3.12.0 (2023-07-18) +------------------- + +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +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/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index 4c3506f1cd..6a71e11c7e 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -2,7 +2,7 @@ pose_broadcaster - 0.0.0 + 4.15.0 Broadcaster to publish cartesian states. Robert Wilbrandt diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index d52201f2b4..eb6d8df132 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 77eae62d2a..c485b49efe 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index bd9e0a8b38..51c989d4f2 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 29ca6cd84d..7a9ab97dc6 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 +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 043930c536..5db2db7bad 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 +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 39360d6921..44fed2a58c 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 34520eeb83..c867767b9a 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 4e1f9abd1e..3542beded0 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- * Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index a04bf1c4ae..82ceab063a 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-10-07) ------------------- From f519170c776649de11431addbfe5bba84ac3603d Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 8 Nov 2024 10:56:52 +0000 Subject: [PATCH 72/97] 4.16.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_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 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- pose_broadcaster/CHANGELOG.rst | 4 ++-- pose_broadcaster/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/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 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 48 files changed, 71 insertions(+), 71 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 8f13e3cb6f..1f275bd7ba 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 5764de56c1..128eabae4c 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.15.0 + 4.16.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index bff4354f4a..0d7daa5f5d 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * Adding use of robot description parameter in the Admittance Controller (`#1247 `_) * Contributors: Dr. Denis, Kevin DeMarco, Nikola Banovic, Bence Magyar, Christoph Fröhlich diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 06fde3cb59..1a08bff889 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.15.0 + 4.16.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 82a4e610bb..57d23a977a 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index f58cf42dff..c87e4e6642 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.15.0 + 4.16.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index e806e497af..e79f1faf1d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 45ab80d5cb..84f51a0750 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.15.0 + 4.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 2cc63d7c90..0abc5feee9 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index fab6d671ad..36ab9ce79e 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.15.0 + 4.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 258b1d52a3..6bd9e2febd 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 ------------ +4.16.0 (2024-11-08) +------------------- * [ForceTorqueSensorBroadcaster] added force and torque offsets to the parameters + export state interfaces (`#1215 `_) * Contributors: Sai Kishor Kothakota diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 1383342246..237cb2c888 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.15.0 + 4.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 8130b880b3..ae920d9f83 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f9f30dd053..8a02785a3a 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.15.0 + 4.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 5feab361b1..291cab691a 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index d46c98f608..76877ff6fa 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.15.0 + 4.16.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 214b914e08..2219d53fe4 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index ed1f794afa..9de0f9c316 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.15.0 + 4.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 47ce981e63..ed2f4b31eb 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * [JSB] Fix the behaviour of publishing unavailable state interfaces when they are previously available (`#1331 `_) * Contributors: Sai Kishor Kothakota diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index c3a1e66574..9e5412881c 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.15.0 + 4.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 e873080f17..87089f700a 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * [JTC] Fix the JTC length_error exceptions in the tests (`#1360 `_) * [jtc] Improve trajectory sampling efficiency (`#1297 `_) * fixes for windows compilation (`#1330 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 112c4ad9a1..c21db502ec 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.15.0 + 4.16.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index da2098ecd0..6ee5646ac4 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index b396ba7b85..5fbb69303e 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 4.15.0 + 4.16.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index c4ce2430f6..9c20af4e97 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * fixes for windows compilation (`#1330 `_) * Contributors: Gilmar Correia diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 79a66d3cc3..0a22ecfe7b 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.15.0 + 4.16.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index 729df5a2f9..c12bc9da80 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- * Add hardware_interface_testing dependency (`#1335 `_) * Implement new PoseBroadcaster controller (`#1311 `_) * Contributors: Christoph Fröhlich, RobertWilbrandt diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index 6a71e11c7e..01c91f21f2 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -2,7 +2,7 @@ pose_broadcaster - 4.15.0 + 4.16.0 Broadcaster to publish cartesian states. Robert Wilbrandt diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index eb6d8df132..e5478f518a 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index c03888eaf8..17bf0b736e 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.15.0 + 4.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index c485b49efe..5d97df0e0b 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 92283fe3ad..3b4276951f 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.15.0 + 4.16.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 51c989d4f2..a45a425f24 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 0886b489c7..6eb4144647 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.15.0 + 4.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 7a9ab97dc6..d3c3059abf 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 ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index f61a51e34e..1ad38581a2 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.15.0 + 4.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 59aee66875..fb26e9c5f7 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.15.0", + version="4.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 5db2db7bad..555a636d7e 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 ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 7920e63246..5954bcd03d 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 - 4.15.0 + 4.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 a70342e154..f2ee71c3d7 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.15.0", + version="4.16.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 44fed2a58c..417c4c9f05 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 5c96c4624c..93d304637d 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.15.0 + 4.16.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index c867767b9a..31258cac7c 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 4e6ab510f3..568bd5cb46 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.15.0 + 4.16.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 3542beded0..975c48303d 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 27aac82dd1..66c5456b9b 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.15.0 + 4.16.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 82ceab063a..90b143f675 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-11-08) +------------------- 4.15.0 (2024-10-07) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index ea295db0ed..60ca9c693c 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.15.0 + 4.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From de77fd0fad6cd85ab23027c1b4d685b2f8085526 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 12 Nov 2024 16:48:15 +0100 Subject: [PATCH 73/97] Update maintainers and add url tags (#1363) --- ackermann_steering_controller/package.xml | 13 ++++++++++--- admittance_controller/package.xml | 15 +++++++++++++-- bicycle_steering_controller/package.xml | 13 ++++++++++--- diff_drive_controller/package.xml | 16 ++++++++++++++-- effort_controllers/package.xml | 11 ++++++++++- force_torque_sensor_broadcaster/package.xml | 14 ++++++++++++-- forward_command_controller/package.xml | 11 ++++++++++- gripper_controllers/package.xml | 9 +++++++++ imu_sensor_broadcaster/package.xml | 13 +++++++++++-- joint_state_broadcaster/package.xml | 14 +++++++++++--- joint_trajectory_controller/package.xml | 8 +++++++- parallel_gripper_controller/package.xml | 9 +++++++++ pid_controller/package.xml | 13 +++++++++++-- pose_broadcaster/package.xml | 12 +++++++++++- position_controllers/package.xml | 11 ++++++++++- range_sensor_broadcaster/package.xml | 14 ++++++++++++-- ros2_controllers/package.xml | 9 +++++++-- ros2_controllers_test_nodes/package.xml | 10 ++++++++-- rqt_joint_trajectory_controller/package.xml | 9 ++++++++- steering_controllers_library/package.xml | 14 ++++++++++---- tricycle_controller/package.xml | 11 ++++++++++- tricycle_steering_controller/package.xml | 14 ++++++++++---- velocity_controllers/package.xml | 11 ++++++++++- 23 files changed, 233 insertions(+), 41 deletions(-) diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 128eabae4c..6ae2f0a438 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -4,10 +4,17 @@ ackermann_steering_controller 4.16.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. - Apache License 2.0 + Bence Magyar - Dr.-Ing. Denis Štogl - dr. sc. Tomislav Petkovic + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ Dr.-Ing. Denis Štogl dr. sc. Tomislav Petkovic diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 1a08bff889..5b48c69695 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -4,11 +4,22 @@ admittance_controller 4.16.0 Implementation of admittance controllers for different input and output interface. - Denis Štogl + Bence Magyar - Andy Zelenak + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Denis Štogl + Andy Zelenak + Paul Gesel + ament_cmake backward_ros diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index c87e4e6642..3fafbda077 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -4,10 +4,17 @@ bicycle_steering_controller 4.16.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. - Apache License 2.0 + Bence Magyar - Dr.-Ing. Denis Štogl - dr. sc. Tomislav Petkovic + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ Dr.-Ing. Denis Štogl dr. sc. Tomislav Petkovic diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 84f51a0750..0602d91898 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -2,12 +2,24 @@ diff_drive_controller 4.16.0 - Controller for a differential drive mobile base. + Controller for a differential-drive mobile base. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Bence Magyar + Enrique Fernández + Manuel Meraz + Jordan Palacios + ament_cmake generate_parameter_library diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 36ab9ce79e..b33224db9b 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -3,11 +3,20 @@ effort_controllers 4.16.0 Generic controller for forwarding commands. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + ament_cmake backward_ros diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 237cb2c888..f2f5124001 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -4,11 +4,21 @@ force_torque_sensor_broadcaster 4.16.0 Controller to publish state of force-torque sensors. + Bence Magyar - Denis Štogl - Subhas Das + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Denis Štogl + Subhas Das + ament_cmake backward_ros diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 8a02785a3a..be71c32052 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -3,11 +3,20 @@ forward_command_controller 4.16.0 Generic controller for forwarding commands. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + ament_cmake backward_ros diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 76877ff6fa..33f06f226a 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -6,11 +6,20 @@ gripper_controllers 4.16.0 The gripper_controllers package + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + Sachin Chitta + Jafar Abdi ament_cmake diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 9de0f9c316..a09f09dee2 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -4,11 +4,20 @@ imu_sensor_broadcaster 4.16.0 Controller to publish readings of IMU sensors. + Bence Magyar - Denis Štogl - Victor Lopez + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Victor Lopez + ament_cmake backward_ros diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9e5412881c..575c17c5a0 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -3,13 +3,21 @@ joint_state_broadcaster 4.16.0 Broadcaster to publish joint state + Bence Magyar - Denis Stogl - Jordan Palacios - Karsten Knese + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + Karsten Knese + ament_cmake backward_ros diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index c21db502ec..830321e7b6 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -3,12 +3,18 @@ joint_trajectory_controller 4.16.0 Controller for executing joint-space trajectories on a group of joints + Bence Magyar - Dr. Denis Štogl + Denis Štogl Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + ament_cmake angles diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 5fbb69303e..7e712b632a 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -6,11 +6,20 @@ parallel_gripper_controller 4.16.0 The parallel_gripper_controller package + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + Sachin Chitta + Paul Gesel ament_cmake diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 0a22ecfe7b..fb3f40184a 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -4,10 +4,19 @@ pid_controller 4.16.0 Controller based on PID implememenation from control_toolbox package. + Bence Magyar - Denis Štogl + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + Denis Štogl - Apache-2.0 ament_cmake diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index 01c91f21f2..dc5d629b50 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -4,10 +4,20 @@ pose_broadcaster 4.16.0 Broadcaster to publish cartesian states. - Robert Wilbrandt + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Robert Wilbrandt + ament_cmake backward_ros diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 17bf0b736e..8aa7bf8117 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -3,11 +3,20 @@ position_controllers 4.16.0 Generic controller for forwarding commands. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + ament_cmake backward_ros diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 3b4276951f..a81823a6a2 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -3,11 +3,21 @@ range_sensor_broadcaster 4.16.0 - Controller to publish readings of Range sensors. + Controller to publish readings of range sensors. + Bence Magyar - Florent Chretien + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Florent Chretien + ament_cmake backward_ros diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 6eb4144647..ccde6cf812 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -2,13 +2,18 @@ ros2_controllers 4.16.0 - Metapackage for ROS2 controllers related packages + Metapackage for ros2_controllers related packages + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ ament_cmake diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 1ad38581a2..c53f1b9971 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -5,10 +5,16 @@ 4.16.0 Demo nodes for showing and testing functionalities of the ros2_control framework. - Denis Štogl Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 - Apache-2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ rclpy std_msgs diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 5954bcd03d..da92d9d663 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -8,8 +8,15 @@ Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota - Apache-2.0 + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ Adolfo Rodriguez Tsouroukdissian Noel Jimenez Garcia diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 93d304637d..efe9f93a67 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -4,11 +4,17 @@ steering_controllers_library 4.16.0 Package for steering robot configurations including odometry and interfaces. - Apache License 2.0 + Bence Magyar - Dr.-Ing. Denis Štogl - dr. sc. Tomislav Petkovic - Tony Najjar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ Dr.-Ing. Denis Štogl dr. sc. Tomislav Petkovic diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 568bd5cb46..516adef248 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -4,9 +4,18 @@ tricycle_controller 4.16.0 Controller for a tricycle drive mobile base + Bence Magyar - Tony Najjar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + Tony Najjar ament_cmake diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 66c5456b9b..7c26e09ba0 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -4,11 +4,17 @@ tricycle_steering_controller 4.16.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. - Apache License 2.0 + Bence Magyar - Dr.-Ing. Denis Štogl - dr. sc. Tomislav Petkovic - Tony Najjar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ Dr.-Ing. Denis Štogl dr. sc. Tomislav Petkovic diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 60ca9c693c..50e662cc58 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -3,11 +3,20 @@ velocity_controllers 4.16.0 Generic controller for forwarding commands. + Bence Magyar - Jordan Palacios + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Jordan Palacios + ament_cmake backward_ros From 28dd7e12987bad86e131c0f2c5684f8c84a61c59 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 12 Nov 2024 16:53:32 +0100 Subject: [PATCH 74/97] TractionLimiter: Fix wrong input checks (#1341) --- tricycle_controller/CMakeLists.txt | 6 + .../tricycle_controller/traction_limiter.hpp | 15 +- tricycle_controller/src/traction_limiter.cpp | 30 +- .../test/test_traction_limiter.cpp | 546 ++++++++++++++++++ 4 files changed, 590 insertions(+), 7 deletions(-) create mode 100644 tricycle_controller/test/test_traction_limiter.cpp diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 6834b753a0..7afe150c43 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -85,6 +85,12 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) + + ament_add_gmock(test_traction_limiter + test/test_traction_limiter.cpp) + target_link_libraries(test_traction_limiter + tricycle_controller + ) endif() install( diff --git a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp index ea0bb16025..437f6cd4c2 100644 --- a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp +++ b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp @@ -28,14 +28,25 @@ class TractionLimiter public: /** * \brief Constructor + * + * Parameters are applied symmetrically for both directions, i.e., are applied + * to the absolute value of the corresponding quantity. + * + * \warning + * - Setting min_velocity: the robot can't stand still + * + * - Setting min_deceleration/min_acceleration: the robot can't move with constant velocity + * + * - Setting min_jerk: the robot can't move with constant acceleration + * * \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 + * \param [in] min_jerk Minimum jerk [m/s^3] + * \param [in] max_jerk Maximum jerk [m/s^3] */ TractionLimiter( double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN, diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp index a8019714e1..e31e5e1e82 100644 --- a/tricycle_controller/src/traction_limiter.cpp +++ b/tricycle_controller/src/traction_limiter.cpp @@ -44,13 +44,13 @@ TractionLimiter::TractionLimiter( 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(max_deceleration_) && std::isnan(min_deceleration_)) 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 " + " 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) @@ -58,19 +58,39 @@ TractionLimiter::TractionLimiter( throw std::invalid_argument("Velocity cannot be negative." + error); } + if (min_velocity_ > max_velocity_) + { + throw std::invalid_argument("Min velocity cannot be greater than max velocity."); + } + if (min_acceleration_ < 0 || max_acceleration_ < 0) { - throw std::invalid_argument("Acceleration cannot be negative." + error); + throw std::invalid_argument("Acceleration limits cannot be negative." + error); + } + + if (min_acceleration_ > max_acceleration_) + { + throw std::invalid_argument("Min acceleration cannot be greater than max acceleration."); } if (min_deceleration_ < 0 || max_deceleration_ < 0) { - throw std::invalid_argument("Deceleration cannot be negative." + error); + throw std::invalid_argument("Deceleration limits cannot be negative." + error); + } + + if (min_deceleration_ > max_deceleration_) + { + throw std::invalid_argument("Min deceleration cannot be greater than max deceleration."); } if (min_jerk_ < 0 || max_jerk_ < 0) { - throw std::invalid_argument("Jerk cannot be negative." + error); + throw std::invalid_argument("Jerk limits cannot be negative." + error); + } + + if (min_jerk_ > max_jerk_) + { + throw std::invalid_argument("Min jerk cannot be greater than max jerk."); } } diff --git a/tricycle_controller/test/test_traction_limiter.cpp b/tricycle_controller/test/test_traction_limiter.cpp new file mode 100644 index 0000000000..4afed7f217 --- /dev/null +++ b/tricycle_controller/test/test_traction_limiter.cpp @@ -0,0 +1,546 @@ +// Copyright 2024 AIT - Austrian Institute of Technology GmbH +// +// 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 "tricycle_controller/traction_limiter.hpp" + +TEST(SpeedLimiterTest, testWrongParams) +{ + EXPECT_NO_THROW(tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + )); + + // velocity + { + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + -10., // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + -10., // min_velocity + -20., // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + -10., // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + 20., // min_velocity + 10., // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + } + + // acceleration + { + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + -10., // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + -10., // min_acceleration + -20., // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + -10., // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + 20., // min_acceleration + 10., // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + } + + // deceleration + { + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + -10., // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + -10., // min_deceleration + -20., // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + -10., // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + 20., // min_deceleration + 10., // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + } + + // jerk + { + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + -10., // min_jerk + std::numeric_limits::quiet_NaN() // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + -10., // min_jerk + -20. // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + std::numeric_limits::quiet_NaN(), // min_jerk + -10. // max_jerk + ), + std::invalid_argument); + + EXPECT_THROW( + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), // min_velocity + std::numeric_limits::quiet_NaN(), // max_velocity + std::numeric_limits::quiet_NaN(), // min_acceleration + std::numeric_limits::quiet_NaN(), // max_acceleration + std::numeric_limits::quiet_NaN(), // min_deceleration + std::numeric_limits::quiet_NaN(), // max_deceleration + 20., // min_jerk + 10. // max_jerk + ), + std::invalid_argument); + } +} + +TEST(SpeedLimiterTest, testNoLimits) +{ + tricycle_controller::TractionLimiter limiter; + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, 10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, -10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); +} + +TEST(SpeedLimiterTest, testVelocityLimits) +{ + tricycle_controller::TractionLimiter limiter(0.5, 1.0, 0.5, 1.0, 2.0, 3.0, 0.5, 5.0); + { + double v = 10.0; + double limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now 1.0 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, 1.0); + EXPECT_DOUBLE_EQ(limiting_factor, 0.1); + + v = 0.1; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now 0.5 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 0.1); + + // TODO(christophfroehlich): does this behavior make sense? + v = 0.0; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now 0.5 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = -10.0; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now -1.0 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, -1.0); + EXPECT_DOUBLE_EQ(limiting_factor, -1.0 / -10.0); + + v = -0.1; + limiting_factor = limiter.limit_velocity(v); + // check if the robot speed is now -0.5 m.s-1, the limit + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, -0.5 / -0.1); + } + + { + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } +} + +TEST(SpeedLimiterTest, testVelocityNoLimits) +{ + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit_velocity(v); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, 10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + + v = -10.0; + limiting_factor = limiter.limit_velocity(v); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, -10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + } + + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } + + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // jerk is now limiting, not velocity + EXPECT_DOUBLE_EQ(v, 2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // jerk is now limiting, not velocity + EXPECT_DOUBLE_EQ(v, -2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + } +} + +TEST(SpeedLimiterTest, testAccelerationLimits) +{ + { + // test max_acceleration + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + + double v = 10.0; + double limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now -0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } + { + // test min_acceleration + // TODO(christophfroehlich): does this behavior make sense? + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); + double v = 0.0; + double limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now 0.25m.s-1 = 0.5m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = -std::numeric_limits::epsilon(); + limiting_factor = limiter.limit_acceleration(v, 0.0, 0.5); + // check if the robot speed is now -0.25m.s-1 = -0.5m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + } + + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } +} + +TEST(SpeedLimiterTest, testDecelerationLimits) +{ + { + // test max_deceleration + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + + double v = 0.0; + double limiting_factor = limiter.limit_acceleration(v, 10.0, 0.5); + // check if the robot speed is now 8.5 m.s-1, which is 10.0 - 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = 0.0; + limiting_factor = limiter.limit_acceleration(v, -10.0, 0.5); + // check if the robot speed is now -8.5 m.s-1, which is -10.0 + 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + } + { + // test min_deceleration + // TODO(christophfroehlich): does this behavior make sense? + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); + double v = 9.9; + limiter.limit_acceleration(v, 10.0, 0.5); + // check if the robot speed is now 9.0m.s-1 = 10 - 2.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 9.0); + + v = -9.9; + limiter.limit_acceleration(v, -10., 0.5); + // check if the robot speed is now -9.0m.s-1 = -10 + 2.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -9.0); + } + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + double v = 0.0; + double limiting_factor = limiter.limit(v, 10.0, 10.0, 0.5); + // check if the robot speed is now 8.5 m.s-1, which is 10.0 - 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = 0.0; + limiting_factor = limiter.limit(v, -10.0, -10.0, 0.5); + // check if the robot speed is now -8.5 m.s-1, which is -10.0 + 3.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -8.5); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + } +} + +TEST(SpeedLimiterTest, testJerkLimits) +{ + { + // test max_jerk + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 2.5m.s-1 = 5.0m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, 2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -2.5m.s-1 = -5.0m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, -2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5 / 10.0); + } + { + // test min_jerk + // TODO(christophfroehlich): does this behavior make sense? + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 5.0); + double v = 0.0; + double limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 0.25m.s-1 = 0.5m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, 0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); // div by 0! + + v = -std::numeric_limits::epsilon(); + limiting_factor = limiter.limit_jerk(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -0.25m.s-1 = -0.5m.s-3 * 2 * 0.5s * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + } + { + tricycle_controller::TractionLimiter limiter( + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), 0.5, 1.0, + 2.0, 3.0, 0.5, 5.0); + // acceleration is limiting, not jerk + + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now 0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the robot speed is now -0.5 m.s-1, which is -1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5 / 10.0); + } +} From 2e5791759c1f02d831766fc60a7af37d65c78ecb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 18 Nov 2024 13:17:13 +0100 Subject: [PATCH 75/97] test_nodes: catch keyboard interrupt and add simple launch tests (#1369) --- .../publisher_forward_position_controller.py | 13 ++- .../publisher_joint_trajectory_controller.py | 13 ++- ros2_controllers_test_nodes/setup.py | 3 +- .../rrbot_forward_position_publisher.yaml | 11 ++ .../rrbot_joint_trajectory_publisher.yaml | 24 ++++ ...sher_forward_position_controller_launch.py | 107 ++++++++++++++++++ ...sher_joint_trajectory_controller_launch.py | 106 +++++++++++++++++ 7 files changed, 269 insertions(+), 8 deletions(-) create mode 100644 ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml create mode 100644 ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml create mode 100644 ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py create mode 100644 ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py 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 bb6add77ef..51582a90d4 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 @@ -68,9 +68,16 @@ def main(args=None): publisher_forward_position = PublisherForwardPosition() - rclpy.spin(publisher_forward_position) - publisher_forward_position.destroy_node() - rclpy.shutdown() + try: + rclpy.spin(publisher_forward_position) + except KeyboardInterrupt: + print("Keyboard interrupt received. Shutting down node.") + except Exception as e: + print(f"Unhandled exception: {e}") + finally: + if rclpy.ok(): + publisher_forward_position.destroy_node() + rclpy.shutdown() if __name__ == "__main__": 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 27f28da1be..91d39855aa 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 @@ -184,9 +184,16 @@ def main(args=None): publisher_joint_trajectory = PublisherJointTrajectory() - rclpy.spin(publisher_joint_trajectory) - publisher_joint_trajectory.destroy_node() - rclpy.shutdown() + try: + rclpy.spin(publisher_joint_trajectory) + except KeyboardInterrupt: + print("Keyboard interrupt received. Shutting down node.") + except Exception as e: + print(f"Unhandled exception: {e}") + finally: + if rclpy.ok(): + publisher_joint_trajectory.destroy_node() + rclpy.shutdown() if __name__ == "__main__": diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index fb26e9c5f7..3900392a10 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -25,8 +25,7 @@ data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), - ("share/" + package_name, glob("launch/*.launch.py")), - ("share/" + package_name + "/configs", glob("configs/*.*")), + ("share/" + package_name + "/test", glob("test/*.yaml")), ], install_requires=["setuptools"], zip_safe=True, diff --git a/ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml b/ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml new file mode 100644 index 0000000000..879ad34ab9 --- /dev/null +++ b/ros2_controllers_test_nodes/test/rrbot_forward_position_publisher.yaml @@ -0,0 +1,11 @@ +publisher_forward_position_controller: + ros__parameters: + + wait_sec_between_publish: 5 + publish_topic: "/forward_position_controller/commands" + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785] + pos2: [0.0, 0.0] + pos3: [-0.785, -0.785] + pos4: [0.0, 0.0] diff --git a/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml b/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml new file mode 100644 index 0000000000..7dd8304134 --- /dev/null +++ b/ros2_controllers_test_nodes/test/rrbot_joint_trajectory_publisher.yaml @@ -0,0 +1,24 @@ +publisher_position_trajectory_controller: + ros__parameters: + + controller_name: "joint_trajectory_position_controller" + wait_sec_between_publish: 6 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: + positions: [0.785, 0.785] + pos2: + positions: [0.0, 0.0] + pos3: + positions: [-0.785, -0.785] + pos4: + positions: [0.0, 0.0] + + joints: + - joint1 + - joint2 + + check_starting_point: false + starting_point_limits: + joint1: [-0.1,0.1] + joint2: [-0.1,0.1] diff --git a/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py new file mode 100644 index 0000000000..8ebb2df7b3 --- /dev/null +++ b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py @@ -0,0 +1,107 @@ +# Copyright (c) 2024 AIT - Austrian Institute of Technology GmbH +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Christoph Froehlich + +import pytest +import unittest +import time + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch_testing.actions import ReadyToTest +from launch_testing_ros import WaitForTopics + +import launch_testing.markers +import rclpy +import launch_ros.actions +from rclpy.node import Node + +from std_msgs.msg import Float64MultiArray + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.launch_test +def generate_test_description(): + + params = PathJoinSubstitution( + [ + FindPackageShare("ros2_controllers_test_nodes"), + "test", + "rrbot_forward_position_publisher.yaml", + ] + ) + + pub_node = launch_ros.actions.Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + parameters=[params], + output="both", + ) + + return LaunchDescription([pub_node, ReadyToTest()]) + + +# This is our test fixture. Each method is a test case. +# These run alongside the processes specified in generate_test_description() +class TestFixture(unittest.TestCase): + + def setUp(self): + rclpy.init() + self.node = Node("test_node") + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_node_start(self): + start = time.time() + found = False + while time.time() - start < 2.0 and not found: + found = "publisher_forward_position_controller" in self.node.get_node_names() + time.sleep(0.1) + assert found, "publisher_forward_position_controller not found!" + + def test_check_if_topic_published(self): + topic = "/forward_position_controller/commands" + wait_for_topics = WaitForTopics([(topic, Float64MultiArray)], timeout=20.0) + assert wait_for_topics.wait(), f"Topic '{topic}' not found!" + msgs = wait_for_topics.received_messages(topic) + msg = msgs[0] + assert len(msg.data) == 2, "Wrong number of joints in message" + wait_for_topics.shutdown() + + +@launch_testing.post_shutdown_test() +# These tests are run after the processes in generate_test_description() have shut down. +class TestPublisherShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + """Check if the processes exited normally.""" + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py new file mode 100644 index 0000000000..d8d6e2a577 --- /dev/null +++ b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py @@ -0,0 +1,106 @@ +# Copyright (c) 2024 AIT - Austrian Institute of Technology GmbH +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Christoph Froehlich + +import pytest +import unittest +import time + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch_testing.actions import ReadyToTest +from launch_testing_ros import WaitForTopics + +import launch_testing.markers +import rclpy +import launch_ros.actions +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.launch_test +def generate_test_description(): + + params = PathJoinSubstitution( + [ + FindPackageShare("ros2_controllers_test_nodes"), + "test", + "rrbot_joint_trajectory_publisher.yaml", + ] + ) + + pub_node = launch_ros.actions.Node( + package="ros2_controllers_test_nodes", + executable="publisher_joint_trajectory_controller", + parameters=[params], + output="both", + ) + + return LaunchDescription([pub_node, ReadyToTest()]) + + +# This is our test fixture. Each method is a test case. +# These run alongside the processes specified in generate_test_description() +class TestFixture(unittest.TestCase): + + def setUp(self): + rclpy.init() + self.node = Node("test_node") + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_node_start(self): + start = time.time() + found = False + while time.time() - start < 2.0 and not found: + found = "publisher_position_trajectory_controller" in self.node.get_node_names() + time.sleep(0.1) + assert found, "publisher_position_trajectory_controller not found!" + + def test_check_if_topic_published(self): + topic = "/joint_trajectory_position_controller/joint_trajectory" + wait_for_topics = WaitForTopics([(topic, JointTrajectory)], timeout=20.0) + assert wait_for_topics.wait(), f"Topic '{topic}' not found!" + msgs = wait_for_topics.received_messages(topic) + msg = msgs[0] + assert len(msg.joint_names) == 2, "Wrong number of joints in message" + wait_for_topics.shutdown() + + +@launch_testing.post_shutdown_test() +# These tests are run after the processes in generate_test_description() have shut down. +class TestPublisherShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + """Check if the processes exited normally.""" + launch_testing.asserts.assertExitCodes(proc_info) From 0590c6ac2f916b13ce5842913b8811a32fd4ffab Mon Sep 17 00:00:00 2001 From: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com> Date: Mon, 18 Nov 2024 15:07:19 +0100 Subject: [PATCH 76/97] Gpio command controller (#1251) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Co-authored-by: m.bednarczyk Co-authored-by: Maciej Bednarczyk Co-authored-by: Bence Magyar Co-authored-by: Christoph Fröhlich Co-authored-by: Christoph Froehlich Co-authored-by: Sai Kishor Kothakota --- doc/controllers_index.rst | 1 + doc/release_notes.rst | 4 + gpio_controllers/CMakeLists.txt | 115 +++ gpio_controllers/doc/userdoc.rst | 57 ++ gpio_controllers/gpio_controllers_plugin.xml | 7 + .../gpio_command_controller.hpp | 114 +++ .../gpio_controllers/visibility_control.h | 49 ++ gpio_controllers/package.xml | 30 + .../src/gpio_command_controller.cpp | 416 +++++++++++ .../gpio_command_controller_parameters.yaml | 34 + .../test/test_gpio_command_controller.cpp | 693 ++++++++++++++++++ .../test_load_gpio_command_controller.cpp | 37 + ros2_controllers/package.xml | 1 + 13 files changed, 1558 insertions(+) create mode 100644 gpio_controllers/CMakeLists.txt create mode 100644 gpio_controllers/doc/userdoc.rst create mode 100644 gpio_controllers/gpio_controllers_plugin.xml create mode 100644 gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp create mode 100644 gpio_controllers/include/gpio_controllers/visibility_control.h create mode 100644 gpio_controllers/package.xml create mode 100644 gpio_controllers/src/gpio_command_controller.cpp create mode 100644 gpio_controllers/src/gpio_command_controller_parameters.yaml create mode 100644 gpio_controllers/test/test_gpio_command_controller.cpp create mode 100644 gpio_controllers/test/test_load_gpio_command_controller.cpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index d5b956aa8a..0a9c6ec485 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -56,6 +56,7 @@ The controllers are using `common hardware interface definitions`_, and may use PID Controller <../pid_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> + Gpio Command Controller <../gpio_controllers/doc/userdoc.rst> Broadcasters diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 688c35724d..b1db026df2 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -63,3 +63,7 @@ steering_controllers_library tricycle_controller ************************ * tricycle_controller now uses generate_parameter_library (`#957 `_). + +gpio_controllers +************************ +* The GPIO command controller was added 🎉 (`#1251 `_). diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt new file mode 100644 index 0000000000..6ea97e04ca --- /dev/null +++ b/gpio_controllers/CMakeLists.txt @@ -0,0 +1,115 @@ +cmake_minimum_required(VERSION 3.8) +project(gpio_controllers) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(control_msgs REQUIRED) + + +generate_parameter_library(gpio_command_controller_parameters + src/gpio_command_controller_parameters.yaml +) + +add_library(gpio_controllers + SHARED + src/gpio_command_controller.cpp +) +target_include_directories(gpio_controllers PRIVATE include) +target_link_libraries(gpio_controllers PUBLIC gpio_command_controller_parameters) +ament_target_dependencies(gpio_controllers PUBLIC + builtin_interfaces + controller_interface + hardware_interface + pluginlib + rclcpp_lifecycle + rcutils + realtime_tools + control_msgs +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(gpio_controllers PRIVATE "GPIO_COMMAND_CONTROLLER_BUILDING_DLL") +pluginlib_export_plugin_description_file(controller_interface gpio_controllers_plugin.xml) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + gpio_controllers + 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(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock( + test_load_gpio_command_controller + test/test_load_gpio_command_controller.cpp + ) + + target_include_directories(test_load_gpio_command_controller PRIVATE include) + ament_target_dependencies(test_load_gpio_command_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + ament_add_gmock( + test_gpio_command_controller + test/test_gpio_command_controller.cpp + ) + target_include_directories(test_gpio_command_controller PRIVATE include) + target_link_libraries(test_gpio_command_controller + gpio_controllers + ) + ament_target_dependencies(test_gpio_command_controller + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + ros2_control_test_assets + control_msgs + ) +endif() + +ament_export_dependencies( + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools +) +ament_export_include_directories( + include +) +ament_export_libraries( + gpio_controllers +) +ament_package() diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst new file mode 100644 index 0000000000..199a4bd208 --- /dev/null +++ b/gpio_controllers/doc/userdoc.rst @@ -0,0 +1,57 @@ +.. _gpio_controllers_userdoc: + +gpio_controllers +===================== + +This is a collection of controllers for hardware interfaces of type GPIO (```` tag in the URDF). + +gpio_command_controller +----------------------------- +gpio_command_controller let the user expose command interfaces of given GPIO interfaces and publishes state interfaces of the configured command interfaces. + +Description of controller's interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +- ``//gpio_states`` [``control_msgs/msg/DynamicJointState``]: Publishes all state interfaces of the given GPIO interfaces. +- ``//commands`` [``control_msgs/msg/DynamicJointState``]: A subscriber for configured command interfaces. + + +Parameters +^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller 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. + +.. generate_parameter_library_details:: + ../src/gpio_command_controller_parameters.yaml + +The controller expects at least one GPIO interface and the corresponding command interface names or state interface. However, these Command and State interfaces are optional. The controller behaves as a broadcaster when no Command Interface is present, thereby publishing the configured GPIO state interfaces if set, else the one present in the URDF. + +.. note:: + + When no state interface is provided in the param file, the controller will try to use state_interfaces from ros2_control's config placed in the URDF for configured gpio interfaces. + However, command interfaces will not be configured based on the available URDF setup. + +.. code-block:: yaml + + gpio_command_controller: + ros__parameters: + type: gpio_controllers/GpioCommandController + gpios: + - Gpio1 + - Gpio2 + command_interfaces: + Gpio1: + - interfaces: + - dig.1 + - dig.2 + - dig.3 + Gpio2: + - interfaces: + - ana.1 + - ana.2 + state_interfaces: + Gpio2: + - interfaces: + - ana.1 + - ana.2 + +With the above-defined controller configuration, the controller will accept commands for all gpios' interfaces and will only publish the state of Gpio2. diff --git a/gpio_controllers/gpio_controllers_plugin.xml b/gpio_controllers/gpio_controllers_plugin.xml new file mode 100644 index 0000000000..03fd3e1ee0 --- /dev/null +++ b/gpio_controllers/gpio_controllers_plugin.xml @@ -0,0 +1,7 @@ + + + + The gpio command controller commands a group of gpios using multiple interfaces. + + + diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp new file mode 100644 index 0000000000..5149c74a1c --- /dev/null +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -0,0 +1,114 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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 GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ +#define GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include "control_msgs/msg/dynamic_interface_group_values.hpp" +#include "controller_interface/controller_interface.hpp" +#include "gpio_command_controller_parameters.hpp" +#include "gpio_controllers/visibility_control.h" +#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" + +namespace gpio_controllers +{ +using CmdType = control_msgs::msg::DynamicInterfaceGroupValues; +using StateType = control_msgs::msg::DynamicInterfaceGroupValues; +using CallbackReturn = controller_interface::CallbackReturn; +using InterfacesNames = std::vector; +using MapOfReferencesToCommandInterfaces = std::unordered_map< + std::string, std::reference_wrapper>; +using MapOfReferencesToStateInterfaces = + std::unordered_map>; +using StateInterfaces = + std::vector>; + +class GpioCommandController : public controller_interface::ControllerInterface +{ +public: + GPIO_COMMAND_CONTROLLER_PUBLIC + GpioCommandController(); + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_init() override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + void store_command_interface_types(); + void store_state_interface_types(); + void initialize_gpio_state_msg(); + void update_gpios_states(); + controller_interface::return_type update_gpios_commands(); + template + std::unordered_map> create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params, std::vector & configured_interfaces); + template + bool check_if_configured_interfaces_matches_received( + const InterfacesNames & interfaces_from_params, const T & configured_interfaces); + void apply_state_value( + StateType & state_msg, std::size_t gpio_index, std::size_t interface_index) const; + void apply_command( + const CmdType & gpio_commands, std::size_t gpio_index, + std::size_t command_interface_index) const; + bool should_broadcast_all_interfaces_of_configured_gpios() const; + void set_all_state_interfaces_of_configured_gpios(); + InterfacesNames get_gpios_state_interfaces_names(const std::string & gpio_name) const; + bool update_dynamic_map_parameters(); + std::vector get_gpios_from_urdf() const; + +protected: + InterfacesNames command_interface_types_; + InterfacesNames state_interface_types_; + MapOfReferencesToCommandInterfaces command_interfaces_map_; + MapOfReferencesToStateInterfaces state_interfaces_map_; + + realtime_tools::RealtimeBuffer> rt_command_ptr_{}; + rclcpp::Subscription::SharedPtr gpios_command_subscriber_{}; + + std::shared_ptr> gpio_state_publisher_{}; + std::shared_ptr> realtime_gpio_state_publisher_{}; + + std::shared_ptr param_listener_{}; + gpio_command_controller_parameters::Params params_; +}; + +} // namespace gpio_controllers + +#endif // GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ diff --git a/gpio_controllers/include/gpio_controllers/visibility_control.h b/gpio_controllers/include/gpio_controllers/visibility_control.h new file mode 100644 index 0000000000..a735a1621c --- /dev/null +++ b/gpio_controllers/include/gpio_controllers/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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 GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ +#define GPIO_CONTROLLERS__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 GPIO_COMMAND_CONTROLLER_EXPORT __attribute__((dllexport)) +#define GPIO_COMMAND_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define GPIO_COMMAND_CONTROLLER_EXPORT __declspec(dllexport) +#define GPIO_COMMAND_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef GPIO_COMMAND_CONTROLLER_BUILDING_LIBRARY +#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_EXPORT +#else +#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_IMPORT +#endif +#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE GPIO_COMMAND_CONTROLLER_PUBLIC +#define GPIO_COMMAND_CONTROLLER_LOCAL +#else +#define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define GPIO_COMMAND_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define GPIO_COMMAND_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define GPIO_COMMAND_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define GPIO_COMMAND_CONTROLLER_PUBLIC +#define GPIO_COMMAND_CONTROLLER_LOCAL +#endif +#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml new file mode 100644 index 0000000000..9b98746545 --- /dev/null +++ b/gpio_controllers/package.xml @@ -0,0 +1,30 @@ + + + + gpio_controllers + 4.15.0 + Controllers to interact with gpios. + Maciej Bednarczyk + Wiktor Bajor + Apache License 2.0 + + ament_cmake + + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + control_msgs + realtime_tools + generate_parameter_library + + pluginlib + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp new file mode 100644 index 0000000000..a509eae774 --- /dev/null +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -0,0 +1,416 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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 "gpio_controllers/gpio_command_controller.hpp" + +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/component_parser.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription.hpp" + +namespace +{ +template +void print_interface(const rclcpp::Logger & logger, const T & command_interfaces) +{ + for (const auto & [interface_name, value] : command_interfaces) + { + RCLCPP_ERROR(logger, "Got %s", interface_name.c_str()); + } +} + +std::vector extract_gpios_from_hardware_info( + const std::vector & hardware_infos) +{ + std::vector result; + for (const auto & hardware_info : hardware_infos) + { + std::copy( + hardware_info.gpios.begin(), hardware_info.gpios.end(), std::back_insert_iterator(result)); + } + return result; +} +} // namespace + +namespace gpio_controllers +{ + +GpioCommandController::GpioCommandController() : controller_interface::ControllerInterface() {} + +CallbackReturn GpioCommandController::on_init() +try +{ + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + return CallbackReturn::SUCCESS; +} +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; +} + +CallbackReturn GpioCommandController::on_configure(const rclcpp_lifecycle::State &) +try +{ + if (!update_dynamic_map_parameters()) + { + return controller_interface::CallbackReturn::ERROR; + } + + store_command_interface_types(); + store_state_interface_types(); + + if (command_interface_types_.empty() && state_interface_types_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "No command or state interfaces are configured"); + return CallbackReturn::ERROR; + } + + if (!command_interface_types_.empty()) + { + gpios_command_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + } + + gpio_state_publisher_ = + get_node()->create_publisher("~/gpio_states", rclcpp::SystemDefaultsQoS()); + + realtime_gpio_state_publisher_ = + std::make_shared>(gpio_state_publisher_); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; +} +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; +} + +controller_interface::InterfaceConfiguration +GpioCommandController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names = command_interface_types_; + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration GpioCommandController::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = state_interface_types_; + + return state_interfaces_config; +} + +CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State &) +{ + command_interfaces_map_ = + create_map_of_references_to_interfaces(command_interface_types_, command_interfaces_); + state_interfaces_map_ = + create_map_of_references_to_interfaces(state_interface_types_, state_interfaces_); + if ( + !check_if_configured_interfaces_matches_received( + command_interface_types_, command_interfaces_map_) || + !check_if_configured_interfaces_matches_received(state_interface_types_, state_interfaces_map_)) + { + return CallbackReturn::ERROR; + } + + initialize_gpio_state_msg(); + rt_command_ptr_.reset(); + RCLCPP_INFO(get_node()->get_logger(), "activate successful"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::State &) +{ + rt_command_ptr_.reset(); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type GpioCommandController::update( + const rclcpp::Time &, const rclcpp::Duration &) +{ + update_gpios_states(); + return update_gpios_commands(); +} + +bool GpioCommandController::update_dynamic_map_parameters() +{ + auto logger = get_node()->get_logger(); + if (!param_listener_) + { + RCLCPP_ERROR(logger, "Error encountered during init"); + return false; + } + // 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(); + return true; +} + +void GpioCommandController::store_command_interface_types() +{ + for (const auto & [gpio_name, interfaces] : params_.command_interfaces.gpios_map) + { + std::transform( + interfaces.interfaces.cbegin(), interfaces.interfaces.cend(), + std::back_inserter(command_interface_types_), + [&](const auto & interface_name) { return gpio_name + "/" + interface_name; }); + } +} + +bool GpioCommandController::should_broadcast_all_interfaces_of_configured_gpios() const +{ + auto are_interfaces_empty = [](const auto & interfaces) + { return interfaces.second.interfaces.empty(); }; + return std::all_of( + params_.state_interfaces.gpios_map.cbegin(), params_.state_interfaces.gpios_map.cend(), + are_interfaces_empty); +} + +std::vector GpioCommandController::get_gpios_from_urdf() const +try +{ + return extract_gpios_from_hardware_info( + hardware_interface::parse_control_resources_from_urdf(get_robot_description())); +} +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during extracting gpios info from urdf %s \n", e.what()); + return {}; +} + +void GpioCommandController::set_all_state_interfaces_of_configured_gpios() +{ + const auto gpios{get_gpios_from_urdf()}; + for (const auto & gpio_name : params_.gpios) + { + for (const auto & gpio : gpios) + { + if (gpio_name == gpio.name) + { + std::transform( + gpio.state_interfaces.begin(), gpio.state_interfaces.end(), + std::back_insert_iterator(state_interface_types_), + [&gpio_name](const auto & interface_name) + { return gpio_name + '/' + interface_name.name; }); + } + } + } +} + +void GpioCommandController::store_state_interface_types() +{ + if (should_broadcast_all_interfaces_of_configured_gpios()) + { + RCLCPP_INFO( + get_node()->get_logger(), + "State interfaces are not configured. All available interfaces of configured GPIOs will be " + "broadcasted."); + set_all_state_interfaces_of_configured_gpios(); + return; + } + + for (const auto & [gpio_name, interfaces] : params_.state_interfaces.gpios_map) + { + std::transform( + interfaces.interfaces.cbegin(), interfaces.interfaces.cend(), + std::back_inserter(state_interface_types_), + [&](const auto & interface_name) { return gpio_name + "/" + interface_name; }); + } +} + +void GpioCommandController::initialize_gpio_state_msg() +{ + auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; + gpio_state_msg.header.stamp = get_node()->now(); + gpio_state_msg.interface_groups.resize(params_.gpios.size()); + gpio_state_msg.interface_values.resize(params_.gpios.size()); + + for (std::size_t gpio_index = 0; gpio_index < params_.gpios.size(); ++gpio_index) + { + const auto gpio_name = params_.gpios[gpio_index]; + gpio_state_msg.interface_groups[gpio_index] = gpio_name; + gpio_state_msg.interface_values[gpio_index].interface_names = + get_gpios_state_interfaces_names(gpio_name); + gpio_state_msg.interface_values[gpio_index].values = std::vector( + gpio_state_msg.interface_values[gpio_index].interface_names.size(), + std::numeric_limits::quiet_NaN()); + } +} + +InterfacesNames GpioCommandController::get_gpios_state_interfaces_names( + const std::string & gpio_name) const +{ + InterfacesNames result; + for (const auto & interface_name : state_interface_types_) + { + const auto it = state_interfaces_map_.find(interface_name); + if (it != state_interfaces_map_.cend() && it->second.get().get_prefix_name() == gpio_name) + { + result.emplace_back(it->second.get().get_interface_name()); + } + } + return result; +} + +template +std::unordered_map> +GpioCommandController::create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params, std::vector & configured_interfaces) +{ + std::unordered_map> map; + for (const auto & interface_name : interfaces_from_params) + { + auto interface = std::find_if( + configured_interfaces.begin(), configured_interfaces.end(), + [&](const auto & configured_interface) + { + const auto full_name_interface_name = configured_interface.get_name(); + return full_name_interface_name == interface_name; + }); + if (interface != configured_interfaces.end()) + { + map.emplace(interface_name, std::ref(*interface)); + } + } + return map; +} + +template +bool GpioCommandController::check_if_configured_interfaces_matches_received( + const InterfacesNames & interfaces_from_params, const T & configured_interfaces) +{ + if (!(configured_interfaces.size() == interfaces_from_params.size())) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %ld interfaces, got %ld", interfaces_from_params.size(), + configured_interfaces.size()); + for (const auto & interface : interfaces_from_params) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected %s", interface.c_str()); + } + print_interface(get_node()->get_logger(), configured_interfaces); + return false; + } + return true; +} + +controller_interface::return_type GpioCommandController::update_gpios_commands() +{ + auto gpio_commands_ptr = rt_command_ptr_.readFromRT(); + if (!gpio_commands_ptr || !(*gpio_commands_ptr)) + { + return controller_interface::return_type::OK; + } + + const auto gpio_commands = *(*gpio_commands_ptr); + for (std::size_t gpio_index = 0; gpio_index < gpio_commands.interface_groups.size(); ++gpio_index) + { + const auto & gpio_name = gpio_commands.interface_groups[gpio_index]; + if ( + gpio_commands.interface_values[gpio_index].values.size() != + gpio_commands.interface_values[gpio_index].interface_names.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "For gpio %s interfaces_names do not match values", + gpio_name.c_str()); + return controller_interface::return_type::ERROR; + } + for (std::size_t command_interface_index = 0; + command_interface_index < gpio_commands.interface_values[gpio_index].values.size(); + ++command_interface_index) + { + apply_command(gpio_commands, gpio_index, command_interface_index); + } + } + return controller_interface::return_type::OK; +} + +void GpioCommandController::apply_command( + const CmdType & gpio_commands, std::size_t gpio_index, std::size_t command_interface_index) const +{ + const auto full_command_interface_name = + gpio_commands.interface_groups[gpio_index] + '/' + + gpio_commands.interface_values[gpio_index].interface_names[command_interface_index]; + try + { + command_interfaces_map_.at(full_command_interface_name) + .get() + .set_value(gpio_commands.interface_values[gpio_index].values[command_interface_index]); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during applying command stage of %s with message: %s \n", + full_command_interface_name.c_str(), e.what()); + } +} + +void GpioCommandController::update_gpios_states() +{ + if (!realtime_gpio_state_publisher_ || !realtime_gpio_state_publisher_->trylock()) + { + return; + } + + auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; + gpio_state_msg.header.stamp = get_node()->now(); + for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg.interface_groups.size(); + ++gpio_index) + { + for (std::size_t interface_index = 0; + interface_index < gpio_state_msg.interface_values[gpio_index].interface_names.size(); + ++interface_index) + { + apply_state_value(gpio_state_msg, gpio_index, interface_index); + } + } + realtime_gpio_state_publisher_->unlockAndPublish(); +} + +void GpioCommandController::apply_state_value( + StateType & state_msg, std::size_t gpio_index, std::size_t interface_index) const +{ + const auto interface_name = + state_msg.interface_groups[gpio_index] + '/' + + state_msg.interface_values[gpio_index].interface_names[interface_index]; + try + { + state_msg.interface_values[gpio_index].values[interface_index] = + state_interfaces_map_.at(interface_name).get().get_value(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during reading state of: %s \n", interface_name.c_str()); + } +} + +} // namespace gpio_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + gpio_controllers::GpioCommandController, controller_interface::ControllerInterface) diff --git a/gpio_controllers/src/gpio_command_controller_parameters.yaml b/gpio_controllers/src/gpio_command_controller_parameters.yaml new file mode 100644 index 0000000000..0c6aade35f --- /dev/null +++ b/gpio_controllers/src/gpio_command_controller_parameters.yaml @@ -0,0 +1,34 @@ +gpio_command_controller_parameters: + gpios: { + type: string_array, + description: "List of gpios", + read_only: true, + validation: { + size_gt<>: [0], + unique<>: null + } + } + + command_interfaces: + __map_gpios: + interfaces: { + type: string_array, + description: "List of command interfaces for each gpio.", + read_only: true, + default_value: [], + validation: { + unique<>: null + } + } + + state_interfaces: + __map_gpios: + interfaces: { + type: string_array, + description: "List of state interfaces for each gpio. If empty all available gpios' states are used.", + read_only: true, + default_value: [], + validation: { + unique<>: null + } + } diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp new file mode 100644 index 0000000000..ec0e0f1228 --- /dev/null +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -0,0 +1,693 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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 "control_msgs/msg/dynamic_interface_group_values.hpp" +#include "gmock/gmock.h" +#include "gpio_controllers/gpio_command_controller.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/wait_result.hpp" +#include "rclcpp/wait_set.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +namespace +{ +const auto hardware_resources_with_gpio = + R"( + + + + + +)"; + +const auto minimal_robot_urdf_with_gpio = std::string(ros2_control_test_assets::urdf_head) + + std::string(hardware_resources_with_gpio) + + std::string(ros2_control_test_assets::urdf_tail); +} // namespace + +using CallbackReturn = controller_interface::CallbackReturn; +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using CmdType = control_msgs::msg::DynamicInterfaceGroupValues; +using StateType = control_msgs::msg::DynamicInterfaceGroupValues; +using hardware_interface::CommandInterface; +using hardware_interface::StateInterface; + +namespace +{ +rclcpp::NodeOptions create_node_options_with_overriden_parameters( + std::vector parameters) +{ + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + return node_options; +} +} // namespace + +class FriendGpioCommandController : public gpio_controllers::GpioCommandController +{ + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesThenValueOfInterfacesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandWithOnlyOneGpioThenInterfacesValuesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreValuesThenInterfacesNameForGpioUpdateShouldReturnFalse); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreInterfacesNameThenValuesForGpioUpdateShouldReturnFalse); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandInterfacesInDifferentOrderThenValueOfInterfacesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCmdContainsWrongGpioInterfacesOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated); +}; + +class GpioCommandControllerTestSuite : public ::testing::Test +{ +public: + GpioCommandControllerTestSuite() + { + rclcpp::init(0, nullptr); + node = std::make_unique("node"); + } + ~GpioCommandControllerTestSuite() { rclcpp::shutdown(); } + void setup_command_and_state_interfaces() + { + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + } + + void move_to_activate_state(controller_interface::return_type result_of_initialization) + { + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + setup_command_and_state_interfaces(); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + } + + void stop_test_when_message_cannot_be_published(int max_sub_check_loop_count) + { + ASSERT_GE(max_sub_check_loop_count, 0) + << "Test was unable to publish a message through controller/broadcaster update loop"; + } + + void assert_default_command_and_state_values() + { + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands.at(2)); + ASSERT_EQ(gpio_1_1_dig_state.get_value(), gpio_states.at(0)); + ASSERT_EQ(gpio_1_2_dig_state.get_value(), gpio_states.at(1)); + ASSERT_EQ(gpio_2_ana_state.get_value(), gpio_states.at(2)); + } + + void update_controller_loop() + { + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + } + + CmdType createGpioCommand( + const std::vector & gpios_names, + const std::vector & interface_values) + { + CmdType command; + command.interface_groups = gpios_names; + command.interface_values = interface_values; + return command; + } + + control_msgs::msg::InterfaceValue createInterfaceValue( + const std::vector & interfaces_names, + const std::vector & interfaces_values) + { + control_msgs::msg::InterfaceValue output; + output.interface_names = interfaces_names; + output.values = interfaces_values; + return output; + } + + void wait_one_miliseconds_for_timeout() + { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller.get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = controller.get_node()->get_clock()->now() + timeout; + while (controller.get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + int wait_for_subscription( + std::shared_ptr subscription, int max_sub_check_loop_count = 5) + { + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + return max_sub_check_loop_count; + } + + FriendGpioCommandController controller; + + const std::vector gpio_names{"gpio1", "gpio2"}; + std::vector gpio_commands{1.0, 0.0, 3.1}; + std::vector gpio_states{1.0, 0.0, 3.1}; + + CommandInterface gpio_1_1_dig_cmd{gpio_names.at(0), "dig.1", &gpio_commands.at(0)}; + CommandInterface gpio_1_2_dig_cmd{gpio_names.at(0), "dig.2", &gpio_commands.at(1)}; + CommandInterface gpio_2_ana_cmd{gpio_names.at(1), "ana.1", &gpio_commands.at(2)}; + + StateInterface gpio_1_1_dig_state{gpio_names.at(0), "dig.1", &gpio_states.at(0)}; + StateInterface gpio_1_2_dig_state{gpio_names.at(0), "dig.2", &gpio_states.at(1)}; + StateInterface gpio_2_ana_state{gpio_names.at(1), "ana.1", &gpio_states.at(2)}; + std::unique_ptr node; +}; + +TEST_F(GpioCommandControllerTestSuite, WhenNoParametersAreSetInitShouldFail) +{ + const auto result = controller.init( + "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", + controller.define_custom_node_options()); + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(GpioCommandControllerTestSuite, WhenGpiosParameterIsEmptyInitShouldFail) +{ + const auto node_options = + create_node_options_with_overriden_parameters({{"gpios", std::vector{}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsEmptyInitShouldNotFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsNotSetInitShouldNotFail) +{ + const auto node_options = + create_node_options_with_overriden_parameters({{"gpios", std::vector{"gpio1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGpiosAreSetAndInterfacesAreSetForAllGpiosThenInitShouldSuccess) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandAndStateInterfacesAreEmptyAndNoInterfacesAreSetInUrdfConfigurationShouldFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init( + "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", + node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenStateInterfaceAreNotConfiguredButAreSetInUrdfForConfiguredGpiosThenConfigureShouldSucceed) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init( + "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenStateInterfaceAreNotConfiguredAndProvidedUrdfIsEmptyThenConfigureShouldFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + setup_command_and_state_interfaces(); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenAssignedCommandInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenAssignedStateInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandInterfacesDontMatchStatesButBothMatchAssignedOnesThenOnActivationShouldSucceed) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenThereWasNoCommandForGpiosThenCommandInterfacesShouldHaveDefaultValues) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + assert_default_command_and_state_values(); + update_controller_loop(); + assert_default_command_and_state_values(); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreValuesThenInterfacesNameForGpioUpdateShouldReturnFalse) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreInterfacesNameThenValuesForGpioUpdateShouldReturnFalse) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, + {createInterfaceValue({"dig.1", "dig.2"}, {0.0}), createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandInterfacesInDifferentOrderThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio2", "gpio1"}, {createInterfaceValue({"ana.1"}, {30.0}), + createInterfaceValue({"dig.2", "dig.1"}, {1.0, 0.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithOnlyOneGpioThenInterfacesValuesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = + createGpioCommand({"gpio1"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0})}); + + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands[2]); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCmdContainsWrongGpioInterfacesOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio3"}, {createInterfaceValue({"dig.3", "dig.4"}, {20.0, 25.0}), + createInterfaceValue({"ana.1"}, {21.0})}); + + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands.at(2)); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesViaTopicThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + auto command_pub = node->create_publisher( + std::string(controller.get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + command_pub->publish(command); + wait_one_miliseconds_for_timeout(); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); +} + +TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurrentValues) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); + + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); + + StateType gpio_state_msg; + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); + + ASSERT_EQ(gpio_state_msg.interface_groups.size(), gpio_names.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_groups.at(1), "gpio2"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(1), "dig.2"); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).interface_names.at(0), "ana.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(1), 0.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).values.at(0), 3.1); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenStateInterfaceAreNotConfiguredButSetInUrdfForConfiguredGpiosThenThatStatesShouldBePublished) +{ + const std::vector single_gpio{"gpio1"}; + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", single_gpio}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1"}}}); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + const auto result_of_initialization = controller.init( + "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); + + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); + + StateType gpio_state_msg; + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); + + ASSERT_EQ(gpio_state_msg.interface_groups.size(), single_gpio.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); +} + +TEST_F( + GpioCommandControllerTestSuite, + ControllerShouldPublishGpioStatesWithCurrentValuesWhenOnlyStateInterfacesAreSet) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + const auto result_of_initialization = + controller.init("test_gpio_command_controller", "", 0, "", node_options); + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + controller.assign_interfaces({}, std::move(state_interfaces)); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); + + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); + + StateType gpio_state_msg; + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); + + ASSERT_EQ(gpio_state_msg.interface_groups.size(), gpio_names.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_groups.at(1), "gpio2"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).interface_names.at(0), "ana.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).values.at(0), 3.1); +} diff --git a/gpio_controllers/test/test_load_gpio_command_controller.cpp b/gpio_controllers/test/test_load_gpio_command_controller.cpp new file mode 100644 index 0000000000..379a53b048 --- /dev/null +++ b/gpio_controllers/test/test_load_gpio_command_controller.cpp @@ -0,0 +1,37 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// 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/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadGpioCommandController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + + ASSERT_NO_THROW( + cm.load_controller("test_gpio_command_controller", "gpio_controllers/GpioCommandController")); + + rclcpp::shutdown(); +} diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index ccde6cf812..4969658788 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -24,6 +24,7 @@ effort_controllers force_torque_sensor_broadcaster forward_command_controller + gpio_controllers imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller From 4f447f4be74da751462a9f8e74e1b0b45c2adb67 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 18 Nov 2024 19:50:08 +0100 Subject: [PATCH 77/97] Add few warning flags to error in all ros2_controllers packages and fix tests (#1370) --- ackermann_steering_controller/CMakeLists.txt | 3 +- .../test_ackermann_steering_controller.hpp | 7 +-- admittance_controller/CMakeLists.txt | 3 +- .../test/test_admittance_controller.hpp | 18 +++---- bicycle_steering_controller/CMakeLists.txt | 3 +- .../test/test_bicycle_steering_controller.hpp | 18 ++++--- diff_drive_controller/CMakeLists.txt | 4 +- effort_controllers/CMakeLists.txt | 3 +- .../CMakeLists.txt | 3 +- .../test_force_torque_sensor_broadcaster.cpp | 4 +- .../test_force_torque_sensor_broadcaster.hpp | 2 +- forward_command_controller/CMakeLists.txt | 3 +- gripper_controllers/CMakeLists.txt | 4 +- imu_sensor_broadcaster/CMakeLists.txt | 3 +- .../test/test_imu_sensor_broadcaster.hpp | 2 +- joint_state_broadcaster/CMakeLists.txt | 3 +- joint_trajectory_controller/CMakeLists.txt | 4 +- parallel_gripper_controller/CMakeLists.txt | 3 +- pid_controller/CMakeLists.txt | 4 +- pose_broadcaster/CMakeLists.txt | 3 +- .../test/test_pose_broadcaster.hpp | 2 +- position_controllers/CMakeLists.txt | 3 +- range_sensor_broadcaster/CMakeLists.txt | 4 +- .../src/range_sensor_broadcaster.cpp | 8 +-- .../test/test_range_sensor_broadcaster.cpp | 50 +++++++++---------- .../test/test_range_sensor_broadcaster.hpp | 1 + steering_controllers_library/CMakeLists.txt | 3 +- .../test_steering_controllers_library.hpp | 7 +-- tricycle_controller/CMakeLists.txt | 3 +- tricycle_steering_controller/CMakeLists.txt | 3 +- .../test_tricycle_steering_controller.hpp | 18 +++---- velocity_controllers/CMakeLists.txt | 3 +- 32 files changed, 116 insertions(+), 86 deletions(-) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 83edf7c157..2ffc413633 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(ackermann_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() # find dependencies diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 59637a072f..c45601d701 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -293,9 +293,10 @@ class AckermannSteeringControllerFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; - std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; + std::array joint_state_values_ = {{0.5, 0.5, 0.0, 0.0}}; + std::array joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}}; + std::array joint_reference_interfaces_ = { + {"linear/velocity", "angular/velocity"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index cd16714a31..477a343776 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(admittance_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 4f4635d861..7ee56b8c11 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -371,15 +371,15 @@ class AdmittanceControllerTest : public ::testing::Test 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::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_; diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index db1f5cf4ba..1f3a4599cc 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(bicycle_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() # find dependencies diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 65f1691a3b..0253078bb9 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -247,12 +247,13 @@ class BicycleSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - std::vector rear_wheels_names_ = {"rear_wheel_joint"}; - std::vector front_wheels_names_ = {"steering_axis_joint"}; - std::vector joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]}; + std::vector rear_wheels_names_ = {{"rear_wheel_joint"}}; + std::vector front_wheels_names_ = {{"steering_axis_joint"}}; + std::vector joint_names_ = {{rear_wheels_names_[0], front_wheels_names_[0]}}; - std::vector rear_wheels_preceeding_names_ = {"pid_controller/rear_wheel_joint"}; - std::vector front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"}; + std::vector rear_wheels_preceeding_names_ = {{"pid_controller/rear_wheel_joint"}}; + std::vector front_wheels_preceeding_names_ = { + {"pid_controller/steering_axis_joint"}}; std::vector preceeding_joint_names_ = { rear_wheels_preceeding_names_[0], front_wheels_preceeding_names_[0]}; @@ -260,9 +261,10 @@ class BicycleSteeringControllerFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - std::array joint_state_values_ = {3.3, 0.5}; - std::array joint_command_values_ = {1.1, 2.2}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; + std::array joint_state_values_ = {{3.3, 0.5}}; + std::array joint_command_values_ = {{1.1, 2.2}}; + std::array joint_reference_interfaces_ = { + {"linear/velocity", "angular/velocity"}}; std::string steering_interface_name_ = "position"; // defined in setup diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 4eeb98b9d2..4b3ff4f77f 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -2,7 +2,9 @@ cmake_minimum_required(VERSION 3.16) project(diff_drive_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index eae8642fb6..e79015fbbf 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -3,7 +3,8 @@ project(effort_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 02c343f050..d9b005e650 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -3,7 +3,8 @@ project(force_torque_sensor_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index e436beb2e5..26f959e88f 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -282,8 +282,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets { SetUpFTSBroadcaster(); - std::array force_offsets = {10.0, 30.0, -50.0}; - std::array torque_offsets = {1.0, -1.2, -5.2}; + std::array force_offsets = {{10.0, 30.0, -50.0}}; + std::array torque_offsets = {{1.0, -1.2, -5.2}}; // set the params 'sensor_name' and 'frame_id' fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index fe7fb0c174..5d485d9d12 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -55,7 +55,7 @@ class ForceTorqueSensorBroadcasterTest : public ::testing::Test protected: const std::string sensor_name_ = "fts_sensor"; const std::string frame_id_ = "fts_sensor_frame"; - std::array sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; + std::array sensor_values_ = {{1.1, 2.2, 3.3, 4.4, 5.5, 6.6}}; hardware_interface::StateInterface fts_force_x_{sensor_name_, "force.x", &sensor_values_[0]}; hardware_interface::StateInterface fts_force_y_{sensor_name_, "force.y", &sensor_values_[1]}; diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 4885c69c8a..bf027866d6 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(forward_command_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index c4a85dd453..4e9c72f79b 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -7,7 +7,9 @@ if(APPLE OR WIN32) endif() if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 5539dea9ff..38f0adbb54 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -3,7 +3,8 @@ project(imu_sensor_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 0f3286c302..70e01d593c 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -54,7 +54,7 @@ class IMUSensorBroadcasterTest : public ::testing::Test protected: const std::string sensor_name_ = "imu_sensor"; const std::string frame_id_ = "imu_sensor_frame"; - std::array sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; + std::array sensor_values_ = {{1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9, 10.10}}; hardware_interface::StateInterface imu_orientation_x_{ sensor_name_, "orientation.x", &sensor_values_[0]}; hardware_interface::StateInterface imu_orientation_y_{ diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index cc8dc18bf6..a75bbfa8f9 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -3,7 +3,8 @@ project(joint_state_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 438661f7bf..ec142c72f3 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -2,7 +2,9 @@ cmake_minimum_required(VERSION 3.16) project(joint_trajectory_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt index cb0f4fafc9..3d9be8dfac 100644 --- a/parallel_gripper_controller/CMakeLists.txt +++ b/parallel_gripper_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(parallel_gripper_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index f9a9abce89..ed9bdcd8cf 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -2,7 +2,9 @@ cmake_minimum_required(VERSION 3.16) project(pid_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() if(WIN32) diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt index 46028cf258..3cc434d2d0 100644 --- a/pose_broadcaster/CMakeLists.txt +++ b/pose_broadcaster/CMakeLists.txt @@ -6,7 +6,8 @@ project(pose_broadcaster if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/pose_broadcaster/test/test_pose_broadcaster.hpp b/pose_broadcaster/test/test_pose_broadcaster.hpp index a164b2c6ac..10b9c03d1c 100644 --- a/pose_broadcaster/test/test_pose_broadcaster.hpp +++ b/pose_broadcaster/test/test_pose_broadcaster.hpp @@ -39,7 +39,7 @@ class PoseBroadcasterTest : public ::testing::Test const std::string frame_id_ = "pose_base_frame"; const std::string tf_child_frame_id_ = "pose_frame"; - std::array pose_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7}; + std::array pose_values_ = {{1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7}}; hardware_interface::StateInterface pose_position_x_{pose_name_, "position.x", &pose_values_[0]}; hardware_interface::StateInterface pose_position_y_{pose_name_, "position.x", &pose_values_[1]}; diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index 18f3cb313a..e76b76555e 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -3,7 +3,8 @@ project(position_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/range_sensor_broadcaster/CMakeLists.txt b/range_sensor_broadcaster/CMakeLists.txt index 6f21607c9c..d70614ea53 100644 --- a/range_sensor_broadcaster/CMakeLists.txt +++ b/range_sensor_broadcaster/CMakeLists.txt @@ -8,7 +8,9 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp index 7c6d714be3..4ff817b2d3 100644 --- a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp @@ -75,10 +75,10 @@ controller_interface::CallbackReturn RangeSensorBroadcaster::on_configure( realtime_publisher_->lock(); realtime_publisher_->msg_.header.frame_id = params_.frame_id; - realtime_publisher_->msg_.radiation_type = params_.radiation_type; - realtime_publisher_->msg_.field_of_view = params_.field_of_view; - realtime_publisher_->msg_.min_range = params_.min_range; - realtime_publisher_->msg_.max_range = params_.max_range; + realtime_publisher_->msg_.radiation_type = static_cast(params_.radiation_type); + realtime_publisher_->msg_.field_of_view = static_cast(params_.field_of_view); + realtime_publisher_->msg_.min_range = static_cast(params_.min_range); + realtime_publisher_->msg_.max_range = static_cast(params_.max_range); // \note The versions conditioning is added here to support the source-compatibility with Humble #if SENSOR_MSGS_VERSION_MAJOR >= 5 realtime_publisher_->msg_.variance = params_.variance; diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index 59d27ebc0c..052c0384d3 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -208,11 +208,11 @@ TEST_F(RangeSensorBroadcasterTest, Publish_RangeBroadcaster_Success) subscribe_and_get_message(range_msg); EXPECT_EQ(range_msg.header.frame_id, frame_id_); - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(static_cast(sensor_range_))); EXPECT_EQ(range_msg.radiation_type, radiation_type_); - EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); - EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); - EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(static_cast(field_of_view_))); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(static_cast(min_range_))); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(static_cast(max_range_))); #if SENSOR_MSGS_VERSION_MAJOR >= 5 EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); #endif @@ -227,30 +227,30 @@ TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success) sensor_msgs::msg::Range range_msg; - sensor_range_ = 0.10; + sensor_range_ = 0.10f; subscribe_and_get_message(range_msg); EXPECT_EQ(range_msg.header.frame_id, frame_id_); - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(static_cast(sensor_range_))); EXPECT_EQ(range_msg.radiation_type, radiation_type_); - EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); - EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); - EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(static_cast(field_of_view_))); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(static_cast(min_range_))); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(static_cast(max_range_))); #if SENSOR_MSGS_VERSION_MAJOR >= 5 - EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(static_cast(variance_))); #endif sensor_range_ = 4.0; subscribe_and_get_message(range_msg); EXPECT_EQ(range_msg.header.frame_id, frame_id_); - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(static_cast(sensor_range_))); EXPECT_EQ(range_msg.radiation_type, radiation_type_); - EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); - EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); - EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(static_cast(field_of_view_))); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(static_cast(min_range_))); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(static_cast(max_range_))); #if SENSOR_MSGS_VERSION_MAJOR >= 5 - EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(static_cast(variance_))); #endif } @@ -268,13 +268,13 @@ TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Succe EXPECT_EQ(range_msg.header.frame_id, frame_id_); // Even out of boundaries you will get the out_of_range range value - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(static_cast(sensor_range_))); EXPECT_EQ(range_msg.radiation_type, radiation_type_); - EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); - EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); - EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(static_cast(field_of_view_))); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(static_cast(min_range_))); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(static_cast(max_range_))); #if SENSOR_MSGS_VERSION_MAJOR >= 5 - EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(static_cast(variance_))); #endif sensor_range_ = 6.0; @@ -282,13 +282,13 @@ TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Succe EXPECT_EQ(range_msg.header.frame_id, frame_id_); // Even out of boundaries you will get the out_of_range range value - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(static_cast(sensor_range_))); EXPECT_EQ(range_msg.radiation_type, radiation_type_); - EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); - EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); - EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); + EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(static_cast(field_of_view_))); + EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(static_cast(min_range_))); + EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(static_cast(max_range_))); #if SENSOR_MSGS_VERSION_MAJOR >= 5 - EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); + EXPECT_THAT(range_msg.variance, ::testing::FloatEq(static_cast(variance_))); #endif } diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp index 10696d071f..b7ffa7fe4a 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp @@ -38,6 +38,7 @@ class RangeSensorBroadcasterTest : public ::testing::Test // defining the parameter names same as in test/range_sensor_broadcaster_params.yaml const std::string sensor_name_ = "range_sensor"; const std::string frame_id_ = "range_sensor_frame"; + const std::string interface_name_ = "range"; const double field_of_view_ = 0.1; const int radiation_type_ = 1; diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index e2bfdbab71..fc79d54b7c 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -3,7 +3,8 @@ project(steering_controllers_library LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() # find dependencies diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 93ee823e0f..ff0ab972d5 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -314,10 +314,11 @@ class SteeringControllersLibraryFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; - std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; + std::array joint_state_values_ = {{0.5, 0.5, 0.0, 0.0}}; + std::array joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; + std::array joint_reference_interfaces_ = { + {"linear/velocity", "angular/velocity"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 7afe150c43..ee877272ab 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(tricycle_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index 24b4cd1a22..9c82ee3574 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -3,7 +3,8 @@ project(tricycle_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() # find dependencies diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 3b7a053937..c5dd71099f 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -260,15 +260,15 @@ class TricycleSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; - std::vector front_wheels_names_ = {"steering_axis_joint"}; - std::vector joint_names_ = { + std::vector rear_wheels_names_{"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector front_wheels_names_{"steering_axis_joint"}; + std::vector joint_names_{ rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0]}; - std::vector rear_wheels_preceeding_names_ = { + std::vector rear_wheels_preceeding_names_{ "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; - std::vector front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"}; - std::vector preceeding_joint_names_ = { + std::vector front_wheels_preceeding_names_{"pid_controller/steering_axis_joint"}; + std::vector preceeding_joint_names_{ rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], front_wheels_preceeding_names_[0]}; @@ -278,9 +278,9 @@ class TricycleSteeringControllerFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - std::array joint_state_values_ = {0.5, 0.5, 0.0}; - std::array joint_command_values_ = {1.1, 3.3, 2.2}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; + std::array joint_state_values_{{0.5, 0.5, 0.0}}; + std::array joint_command_values_{{1.1, 3.3, 2.2}}; + std::array joint_reference_interfaces_{{"linear/velocity", "angular/velocity"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index a39cd162fd..feb4eae74f 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -3,7 +3,8 @@ project(velocity_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format) + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS From 3b600f28067f2a2146861db58f3d7c10925e492b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 18 Nov 2024 20:22:09 +0100 Subject: [PATCH 78/97] Add compatibility build for humble+jazzy distro (#1368) --- .../workflows/humble-semi-binary-build.yml | 2 +- .github/workflows/iron-semi-binary-build.yml | 2 +- .github/workflows/jazzy-semi-binary-build.yml | 2 +- ...ling-compatibility-humble-binary-build.yml | 48 +++++++++++++++++++ ...lling-compatibility-jazzy-binary-build.yml | 48 +++++++++++++++++++ .../workflows/rolling-semi-binary-build.yml | 2 +- 6 files changed, 100 insertions(+), 4 deletions(-) create mode 100644 .github/workflows/rolling-compatibility-humble-binary-build.yml create mode 100644 .github/workflows/rolling-compatibility-jazzy-binary-build.yml diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index 4246d18160..c53c46032a 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: humble ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml index 3aca5e5b70..a0782e50bb 100644 --- a/.github/workflows/iron-semi-binary-build.yml +++ b/.github/workflows/iron-semi-binary-build.yml @@ -40,7 +40,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: iron ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index ffa4f914b9..a462d9040a 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -41,7 +41,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: jazzy ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml new file mode 100644 index 0000000000..7aab02fa62 --- /dev/null +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -0,0 +1,48 @@ +name: Check Rolling Compatibility on Humble +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Humble distro.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + +jobs: + build-on-humble: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [humble] + ROS_REPO: [testing] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml new file mode 100644 index 0000000000..37ffbb84a4 --- /dev/null +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -0,0 +1,48 @@ +name: Check Rolling Compatibility on Jazzy +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Jazzy distro.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + +jobs: + build-on-jazzy: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [jazzy] + ROS_REPO: [testing] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index 872c509931..db0dd1fe64 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -41,7 +41,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: rolling ros_repo: ${{ matrix.ROS_REPO }} From 29e84b977be4b8fda791e17fd1ba47ec5b2ef3ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 18 Nov 2024 22:11:03 +0100 Subject: [PATCH 79/97] Remove iron workflows and update readme (#1338) Co-authored-by: Dr. Denis --- .github/dependabot.yml | 7 --- .github/mergify.yml | 9 ---- .github/workflows/iron-abi-compatibility.yml | 27 ---------- .github/workflows/iron-binary-build.yml | 50 ------------------- .github/workflows/iron-check-docs.yml | 18 ------- .github/workflows/iron-coverage-build.yml | 38 -------------- .github/workflows/iron-debian-build.yml | 30 ----------- .github/workflows/iron-pre-commit.yml | 13 ----- .../workflows/iron-rhel-semi-binary-build.yml | 29 ----------- .github/workflows/iron-semi-binary-build.yml | 48 ------------------ .github/workflows/iron-source-build.yml | 28 ----------- README.md | 1 - ros2_controllers-not-released.iron.repos | 6 --- ros2_controllers.iron.repos | 21 -------- 14 files changed, 325 deletions(-) delete mode 100644 .github/workflows/iron-abi-compatibility.yml delete mode 100644 .github/workflows/iron-binary-build.yml delete mode 100644 .github/workflows/iron-check-docs.yml delete mode 100644 .github/workflows/iron-coverage-build.yml delete mode 100644 .github/workflows/iron-debian-build.yml delete mode 100644 .github/workflows/iron-pre-commit.yml delete mode 100644 .github/workflows/iron-rhel-semi-binary-build.yml delete mode 100644 .github/workflows/iron-semi-binary-build.yml delete mode 100644 .github/workflows/iron-source-build.yml delete mode 100644 ros2_controllers-not-released.iron.repos delete mode 100644 ros2_controllers.iron.repos diff --git a/.github/dependabot.yml b/.github/dependabot.yml index aafd67c236..f5e9921f23 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -18,10 +18,3 @@ updates: schedule: interval: "weekly" target-branch: "humble" - - package-ecosystem: "github-actions" - # Workflow files stored in the - # default location of `.github/workflows` - directory: "/" - schedule: - interval: "weekly" - target-branch: "iron" diff --git a/.github/mergify.yml b/.github/mergify.yml index 39ee6b6bc0..c2eda7385b 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -8,15 +8,6 @@ pull_request_rules: branches: - humble - - name: Backport to iron at reviewers discretion - conditions: - - base=master - - "label=backport-iron" - actions: - backport: - branches: - - iron - - name: Ask to resolve conflict conditions: - conflict diff --git a/.github/workflows/iron-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml deleted file mode 100644 index be1aec6680..0000000000 --- a/.github/workflows/iron-abi-compatibility.yml +++ /dev/null @@ -1,27 +0,0 @@ -name: Iron - ABI Compatibility Check -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '.github/workflows/iron-abi-compatibility.yml' - - '**.yaml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers-not-released.iron.repos' - -jobs: - abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: iron - ROS_REPO: testing - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true diff --git a/.github/workflows/iron-binary-build.yml b/.github/workflows/iron-binary-build.yml deleted file mode 100644 index 0d2f0ff7f3..0000000000 --- a/.github/workflows/iron-binary-build.yml +++ /dev/null @@ -1,50 +0,0 @@ -name: Iron Binary Build -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - pull_request: - branches: - - iron - - '*feature*' - - '*feature/**' - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers-not-released.iron.repos' - push: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers-not-released.iron.repos' - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_REPO: [main, testing] - with: - ros_distro: iron - ros_repo: ${{ matrix.ROS_REPO }} - upstream_workspace: ros2_controllers-not-released.iron.repos - ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-check-docs.yml b/.github/workflows/iron-check-docs.yml deleted file mode 100644 index f842902055..0000000000 --- a/.github/workflows/iron-check-docs.yml +++ /dev/null @@ -1,18 +0,0 @@ -name: Iron Check Docs - -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.rst' - - '**.md' - - '**.yaml' - -jobs: - check-docs: - name: Check Docs - uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@iron - with: - ROS2_CONTROLLERS_PR: ${{ github.ref }} diff --git a/.github/workflows/iron-coverage-build.yml b/.github/workflows/iron-coverage-build.yml deleted file mode 100644 index e1d2e84bc8..0000000000 --- a/.github/workflows/iron-coverage-build.yml +++ /dev/null @@ -1,38 +0,0 @@ -name: Coverage Build - Iron -on: - workflow_dispatch: - push: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-coverage-build.yml' - - 'codecov.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-coverage-build.yml' - - 'codecov.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - -jobs: - coverage_iron: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master - secrets: inherit - with: - ros_distro: iron diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml deleted file mode 100644 index ebd2f46c18..0000000000 --- a/.github/workflows/iron-debian-build.yml +++ /dev/null @@ -1,30 +0,0 @@ -name: Debian Iron Source Build -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-debian-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' - - -jobs: - iron_debian: - name: Iron debian build - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master - with: - ros_distro: iron - upstream_workspace: ros2_controllers.iron.repos - ref_for_scheduled_build: iron - skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/iron-pre-commit.yml b/.github/workflows/iron-pre-commit.yml deleted file mode 100644 index a128958031..0000000000 --- a/.github/workflows/iron-pre-commit.yml +++ /dev/null @@ -1,13 +0,0 @@ -name: Pre-Commit - Iron - -on: - workflow_dispatch: - pull_request: - branches: - - iron - -jobs: - pre-commit: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master - with: - ros_distro: iron diff --git a/.github/workflows/iron-rhel-semi-binary-build.yml b/.github/workflows/iron-rhel-semi-binary-build.yml deleted file mode 100644 index 109dac7550..0000000000 --- a/.github/workflows/iron-rhel-semi-binary-build.yml +++ /dev/null @@ -1,29 +0,0 @@ -name: RHEL Iron Binary Build -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.yaml' - - '.github/workflows/iron-rhel-semi-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' - - -jobs: - iron_rhel_binary: - name: Iron RHEL binary build - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master - with: - ros_distro: iron - upstream_workspace: ros2_controllers.iron.repos - ref_for_scheduled_build: iron - skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml deleted file mode 100644 index a0782e50bb..0000000000 --- a/.github/workflows/iron-semi-binary-build.yml +++ /dev/null @@ -1,48 +0,0 @@ -name: Iron Semi-Binary Build -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - pull_request: - branches: - - iron - - '*feature*' - - '*feature/**' - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-semi-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - push: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.yaml' - - '.github/workflows/iron-semi-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' - -jobs: - semi_binary: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_REPO: [testing] - with: - ros_distro: iron - ros_repo: ${{ matrix.ROS_REPO }} - upstream_workspace: ros2_controllers.iron.repos - ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml deleted file mode 100644 index c54ead7877..0000000000 --- a/.github/workflows/iron-source-build.yml +++ /dev/null @@ -1,28 +0,0 @@ -name: Iron Source Build -on: - workflow_dispatch: - push: - branches: - - iron - paths: - - '**.hpp' - - '**.h' - - '**.cpp' - - '**.py' - - '**.yaml' - - '.github/workflows/iron-source-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - 'ros2_controllers.iron.repos' - schedule: - # Run every day to detect flakiness and broken dependencies - - cron: '03 3 * * *' - -jobs: - source: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master - with: - ros_distro: iron - ref: iron - ros2_repo_branch: iron - os_name: ubuntu-22.04 diff --git a/README.md b/README.md index 57a9ecdda1..28a501dcdc 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,6 @@ 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) | [control.ros.org](https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) **Jazzy** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Jazzy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-binary-build.yml?branch=master)
[![Jazzy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/jazzy/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#jazzy) -**Iron** | [`iron`](https://github.com/ros-controls/ros2_controllers/tree/iron) | [![Iron Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-binary-build.yml?branch=master)
[![Iron Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/iron/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#iron) **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.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/humble/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#humble) ### Explanation of different build types diff --git a/ros2_controllers-not-released.iron.repos b/ros2_controllers-not-released.iron.repos deleted file mode 100644 index 1b3910e7e7..0000000000 --- a/ros2_controllers-not-released.iron.repos +++ /dev/null @@ -1,6 +0,0 @@ -repositories: - ## EXAMPLE DEPENDENCY -# : -# type: git -# url: git@github.com:/.git -# version: master diff --git a/ros2_controllers.iron.repos b/ros2_controllers.iron.repos deleted file mode 100644 index 7f2db052cb..0000000000 --- a/ros2_controllers.iron.repos +++ /dev/null @@ -1,21 +0,0 @@ -repositories: - ros2_control: - type: git - url: https://github.com/ros-controls/ros2_control.git - version: iron - realtime_tools: - type: git - url: https://github.com/ros-controls/realtime_tools.git - version: master - control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: master - 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 79358e38100139cd1d91ba8d090a388677d464b3 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 19 Nov 2024 09:04:51 +0100 Subject: [PATCH 80/97] Add Mecanum Drive Controller (#512) --- doc/controllers_index.rst | 1 + doc/release_notes.rst | 4 + mecanum_drive_controller/CMakeLists.txt | 110 ++++ mecanum_drive_controller/doc/userdoc.rst | 61 ++ .../mecanum_drive_controller.hpp | 162 +++++ .../mecanum_drive_controller/odometry.hpp | 104 ++++ .../visibility_control.h | 49 ++ .../mecanum_drive_controller.xml | 7 + mecanum_drive_controller/package.xml | 50 ++ .../src/mecanum_drive_controller.cpp | 527 ++++++++++++++++ .../src/mecanum_drive_controller.yaml | 148 +++++ mecanum_drive_controller/src/odometry.cpp | 124 ++++ .../test/mecanum_drive_controller_params.yaml | 19 + ...um_drive_controller_preceeding_params.yaml | 24 + .../test_load_mecanum_drive_controller.cpp | 52 ++ .../test/test_mecanum_drive_controller.cpp | 576 ++++++++++++++++++ .../test/test_mecanum_drive_controller.hpp | 317 ++++++++++ ...st_mecanum_drive_controller_preceeding.cpp | 99 +++ ros2_controllers/package.xml | 1 + steering_controllers_library/doc/userdoc.rst | 2 +- 20 files changed, 2436 insertions(+), 1 deletion(-) create mode 100644 mecanum_drive_controller/CMakeLists.txt create mode 100644 mecanum_drive_controller/doc/userdoc.rst create mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp create mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp create mode 100644 mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h create mode 100644 mecanum_drive_controller/mecanum_drive_controller.xml create mode 100644 mecanum_drive_controller/package.xml create mode 100644 mecanum_drive_controller/src/mecanum_drive_controller.cpp create mode 100644 mecanum_drive_controller/src/mecanum_drive_controller.yaml create mode 100644 mecanum_drive_controller/src/odometry.cpp create mode 100644 mecanum_drive_controller/test/mecanum_drive_controller_params.yaml create mode 100644 mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml create mode 100644 mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp create mode 100644 mecanum_drive_controller/test/test_mecanum_drive_controller.cpp create mode 100644 mecanum_drive_controller/test/test_mecanum_drive_controller.hpp create mode 100644 mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 0a9c6ec485..fe09839ce5 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -28,6 +28,7 @@ Controllers for Wheeled Mobile Robots :titlesonly: Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> + Mecanum Drive Controllers <../mecanum_drive_controller/doc/userdoc.rst> Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Tricycle Controller <../tricycle_controller/doc/userdoc.rst> diff --git a/doc/release_notes.rst b/doc/release_notes.rst index b1db026df2..974bca880f 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -50,6 +50,10 @@ joint_trajectory_controller * Add the boolean parameter ``set_last_command_interface_value_as_state_on_activation``. When set to ``true``, the last command interface value is used as both the current state and the last commanded state upon activation. When set to ``false``, the current state is used for both (`#1231 `_). +mecanum_drive_controller +************************ +* 🚀 The mecanum_drive_controller was added 🎉 (`#512 `_). + pid_controller ************************ * 🚀 The PID controller was added 🎉 (`#434 `_). diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt new file mode 100644 index 0000000000..48bac58fe7 --- /dev/null +++ b/mecanum_drive_controller/CMakeLists.txt @@ -0,0 +1,110 @@ +cmake_minimum_required(VERSION 3.16) +project(mecanum_drive_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + generate_parameter_library + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + tf2 + tf2_geometry_msgs + tf2_msgs +) + +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(mecanum_drive_controller_parameters + src/mecanum_drive_controller.yaml +) + +add_library( + mecanum_drive_controller + SHARED + src/mecanum_drive_controller.cpp + src/odometry.cpp +) +target_compile_features(mecanum_drive_controller PUBLIC cxx_std_17) +target_include_directories(mecanum_drive_controller PUBLIC + "$" + "$") +target_link_libraries(mecanum_drive_controller PUBLIC + mecanum_drive_controller_parameters) +ament_target_dependencies(mecanum_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(mecanum_drive_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface mecanum_drive_controller.xml) + +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) + + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_mecanum_drive_controller test/test_load_mecanum_drive_controller.cpp) + ament_target_dependencies(test_load_mecanum_drive_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock( + test_mecanum_drive_controller test/test_mecanum_drive_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_params.yaml) + target_include_directories(test_mecanum_drive_controller PRIVATE include) + target_link_libraries(test_mecanum_drive_controller mecanum_drive_controller) + ament_target_dependencies( + test_mecanum_drive_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_mecanum_drive_controller_preceeding test/test_mecanum_drive_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_preceeding_params.yaml) + target_include_directories(test_mecanum_drive_controller_preceeding PRIVATE include) + target_link_libraries(test_mecanum_drive_controller_preceeding mecanum_drive_controller) + ament_target_dependencies( + test_mecanum_drive_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/mecanum_drive_controller +) + +install( + TARGETS mecanum_drive_controller mecanum_drive_controller_parameters + EXPORT export_mecanum_drive_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_mecanum_drive_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/mecanum_drive_controller/doc/userdoc.rst b/mecanum_drive_controller/doc/userdoc.rst new file mode 100644 index 0000000000..37c8d7d0e7 --- /dev/null +++ b/mecanum_drive_controller/doc/userdoc.rst @@ -0,0 +1,61 @@ +.. _mecanum_drive_controller_userdoc: + +mecanum_drive_controller +========================= + +Library with shared functionalities for mobile robot controllers with mecanum drive (four wheels). +The library implements generic odometry and update methods and defines the main interfaces. + +Execution logic of the controller +---------------------------------- + +The controller uses velocity input, i.e., stamped Twist messages where linear ``x``, ``y``, and angular ``z`` components are used. +Values in other components are ignored. +In the chain mode, the controller provides three reference interfaces, one for linear velocity and one for steering angle position. +Other relevant features are: + + - odometry publishing as Odometry and TF message; + - input command timeout based on a parameter. + +Note about odometry calculation: +In the DiffDRiveController, the velocity is filtered out, but we prefer to return it raw and let the user perform post-processing at will. +We prefer this way of doing so as filtering introduces delay (which makes it difficult to interpret and compare behavior curves). + + +Description of controller's interfaces +-------------------------------------- + +References (from a preceding controller) +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +- / [double] # in [rad] or [rad/s] + +Commands +,,,,,,,,, +- / [double] # in [rad] or [rad/s] + +States +,,,,,,, +- / [double] # in [rad] or [rad/s] + ..note :: + + ``joint_names[i]`` can be of ``state_joint_names`` parameter (if used), ``command_joint_names`` otherwise. + + +Subscribers +,,,,,,,,,,,, +Used when the controller is not in chained mode (``in_chained_mode == false``). + +- /reference [geometry_msgs/msg/TwistStamped] + +Publishers +,,,,,,,,,,, +- /odometry [nav_msgs/msg/Odometry] +- /tf_odometry [tf2_msgs/msg/TFMessage] +- /controller_state [control_msgs/msg/MecanumDriveControllerState] + +Parameters +,,,,,,,,,,, + +For a list of parameters and their meaning, see the YAML file in the ``src`` folder of the controller's package. + +For an exemplary parameterization, see the ``test`` folder of the controller's package. diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp new file mode 100644 index 0000000000..0fd7f6af29 --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -0,0 +1,162 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ +#define MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "mecanum_drive_controller/odometry.hpp" +#include "mecanum_drive_controller/visibility_control.h" +#include "mecanum_drive_controller_parameters.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/set_bool.hpp" + +#include "control_msgs/msg/mecanum_drive_controller_state.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +namespace mecanum_drive_controller +{ +// name constants for state interfaces +static constexpr size_t NR_STATE_ITFS = 4; + +// name constants for command interfaces +static constexpr size_t NR_CMD_ITFS = 4; + +// name constants for reference interfaces +static constexpr size_t NR_REF_ITFS = 3; + +class MecanumDriveController : public controller_interface::ChainableControllerInterface +{ +public: + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + MecanumDriveController(); + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + using ControllerReferenceMsg = geometry_msgs::msg::TwistStamped; + using OdomStateMsg = nav_msgs::msg::Odometry; + using TfStateMsg = tf2_msgs::msg::TFMessage; + using ControllerStateMsg = control_msgs::msg::MecanumDriveControllerState; + +protected: + std::shared_ptr param_listener_; + mecanum_drive_controller::Params params_; + + /** + * The list is sorted in the following order: + * - front left wheel + * - front right wheel + * - back right wheel + * - back left wheel + */ + enum WheelIndex : std::size_t + { + FRONT_LEFT = 0, + FRONT_RIGHT = 1, + REAR_RIGHT = 2, + REAR_LEFT = 3 + }; + + /** + * Internal lists with joint names sorted as in `WheelIndex` enum. + */ + std::vector command_joint_names_; + + /** + * Internal lists with joint names sorted as in `WheelIndex` enum. + * If parameters for state joint names are *not* defined, this list is the same as + * `command_joint_names_`. + */ + std::vector state_joint_names_; + + // Names of the references, ex: high level vel commands from MoveIt, Nav2, etc. + // used for preceding controller + std::vector reference_names_; + + // Command subscribers and Controller State, odom state, tf state publishers + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); + + using OdomStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr odom_s_publisher_; + std::unique_ptr rt_odom_state_publisher_; + + using TfStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; + std::unique_ptr rt_tf_odom_state_publisher_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr controller_s_publisher_; + std::unique_ptr controller_state_publisher_; + + // override methods from ChainableControllerInterface + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; + + Odometry odometry_; + +private: + // callback for topic interface + MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL + void reference_callback(const std::shared_ptr msg); + + double velocity_in_center_frame_linear_x_; // [m/s] + double velocity_in_center_frame_linear_y_; // [m/s] + double velocity_in_center_frame_angular_z_; // [rad/s] +}; + +} // namespace mecanum_drive_controller + +#endif // MECANUM_DRIVE_CONTROLLER__MECANUM_DRIVE_CONTROLLER_HPP_ diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp new file mode 100644 index 0000000000..ac1ad060dd --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -0,0 +1,104 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ +#define MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ + +#include "geometry_msgs/msg/twist.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" + +#define PLANAR_POINT_DIM 3 + +namespace mecanum_drive_controller +{ +/// \brief The Odometry class handles odometry readings +/// (2D pose and velocity with related timestamp) +class Odometry +{ +public: + /// Integration function, used to integrate the odometry: + typedef std::function IntegrationFunction; + + /// \brief Constructor + /// Timestamp will get the current time value + /// Value will be set to zero + Odometry(); + + /// \brief Initialize the odometry + /// \param time Current time + void init(const rclcpp::Time & time, std::array base_frame_offset); + + /// \brief Updates the odometry class with latest wheels position + /// \param wheel_front_left_vel Wheel velocity [rad/s] + /// \param wheel_rear_left_vel Wheel velocity [rad/s] + /// \param wheel_rear_right_vel Wheel velocity [rad/s] + /// \param wheel_front_right_vel Wheel velocity [rad/s] + /// \param time Current time + /// \return true if the odometry is actually updated + bool update( + const double wheel_front_left_vel, const double wheel_rear_left_vel, + const double wheel_rear_right_vel, const double wheel_front_right_vel, const double dt); + + /// \return position (x component) [m] + double getX() const { return position_x_in_base_frame_; } + /// \return position (y component) [m] + double getY() const { return position_y_in_base_frame_; } + /// \return orientation (z component) [m] + double getRz() const { return orientation_z_in_base_frame_; } + /// \return body velocity of the base frame (linear x component) [m/s] + double getVx() const { return velocity_in_base_frame_linear_x; } + /// \return body velocity of the base frame (linear y component) [m/s] + double getVy() const { return velocity_in_base_frame_linear_y; } + /// \return body velocity of the base frame (angular z component) [m/s] + double getWz() const + { + return velocity_in_base_frame_angular_z; + ; + } + + /// \brief Sets the wheels parameters: mecanum geometric param and radius + /// \param sum_of_robot_center_projection_on_X_Y_axis Wheels geometric param + /// (used in mecanum wheels' ik) [m] + /// \param wheels_radius Wheels radius [m] + void setWheelsParams( + const double sum_of_robot_center_projection_on_X_Y_axis, const double wheels_radius); + +private: + /// Current timestamp: + rclcpp::Time timestamp_; + + /// Reference frame (wrt to center frame). [x, y, theta] + std::array base_frame_offset_; + + /// Current pose: + double position_x_in_base_frame_; // [m] + double position_y_in_base_frame_; // [m] + double orientation_z_in_base_frame_; // [rad] + + double velocity_in_base_frame_linear_x; // [m/s] + double velocity_in_base_frame_linear_y; // [m/s] + double velocity_in_base_frame_angular_z; // [rad/s] + + /// Wheels kinematic parameters [m]: + /// lx and ly represent the distance from the robot's center to the wheels + /// projected on the x and y axis with origin at robots center respectively, + /// sum_of_robot_center_projection_on_X_Y_axis_ = lx+ly + double sum_of_robot_center_projection_on_X_Y_axis_; + double wheels_radius_; // [m] +}; + +} // namespace mecanum_drive_controller + +#endif // MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ */ diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h new file mode 100644 index 0000000000..3222b2fa52 --- /dev/null +++ b/mecanum_drive_controller/include/mecanum_drive_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define MECANUM_DRIVE_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 MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef MECANUM_DRIVE_CONTROLLER__VISIBILITY_BUILDING_DLL +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT +#else +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT +#endif +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC_TYPE MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL +#else +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_LOCAL +#endif +#define MECANUM_DRIVE_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // MECANUM_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/mecanum_drive_controller/mecanum_drive_controller.xml b/mecanum_drive_controller/mecanum_drive_controller.xml new file mode 100644 index 0000000000..c012ad53ca --- /dev/null +++ b/mecanum_drive_controller/mecanum_drive_controller.xml @@ -0,0 +1,7 @@ + + + + The mecanum drive controller transforms linear and angular velocity messages into signals for each wheel(s) for a 4 mecanum wheeled robot. + + diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml new file mode 100644 index 0000000000..97aadf4327 --- /dev/null +++ b/mecanum_drive_controller/package.xml @@ -0,0 +1,50 @@ + + + + mecanum_drive_controller + 0.0.0 + Implementation of mecanum drive controller for 4 wheel drive. + + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Dr.-Ing. Denis Štogl + Giridhar Bukka + + ament_cmake + + generate_parameter_library + + control_msgs + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + rcpputils + std_srvs + tf2 + tf2_geometry_msgs + tf2_msgs + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp new file mode 100644 index 0000000000..2fd5fdbdcb --- /dev/null +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -0,0 +1,527 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "mecanum_drive_controller/mecanum_drive_controller.hpp" + +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ // utility + +using ControllerReferenceMsg = + mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg( + const std::shared_ptr & msg, + const std::shared_ptr & node) +{ + msg->header.stamp = node->now(); + msg->twist.linear.x = std::numeric_limits::quiet_NaN(); + msg->twist.linear.y = std::numeric_limits::quiet_NaN(); + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = std::numeric_limits::quiet_NaN(); +} + +} // namespace + +namespace mecanum_drive_controller +{ +MecanumDriveController::MecanumDriveController() +: controller_interface::ChainableControllerInterface() +{ +} + +controller_interface::CallbackReturn MecanumDriveController::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MecanumDriveController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + auto prepare_lists_with_joint_names = + [&command_joints = this->command_joint_names_, &state_joints = this->state_joint_names_]( + const std::size_t index, const std::string & command_joint_name, + const std::string & state_joint_name) + { + command_joints[index] = command_joint_name; + if (state_joint_name.empty()) + { + state_joints[index] = command_joint_name; + } + else + { + state_joints[index] = state_joint_name; + } + }; + + command_joint_names_.resize(4); + state_joint_names_.resize(4); + + // The joint names are sorted according to the order documented in the header file! + prepare_lists_with_joint_names( + FRONT_LEFT, params_.front_left_wheel_command_joint_name, + params_.front_left_wheel_state_joint_name); + prepare_lists_with_joint_names( + FRONT_RIGHT, params_.front_right_wheel_command_joint_name, + params_.front_right_wheel_state_joint_name); + prepare_lists_with_joint_names( + REAR_RIGHT, params_.rear_right_wheel_command_joint_name, + params_.rear_right_wheel_state_joint_name); + prepare_lists_with_joint_names( + REAR_LEFT, params_.rear_left_wheel_command_joint_name, + params_.rear_left_wheel_state_joint_name); + + // Set wheel params for the odometry computation + odometry_.setWheelsParams( + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, + params_.kinematics.wheels_radius); + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&MecanumDriveController::reference_callback, this, std::placeholders::_1)); + + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ref_.writeFromNonRT(msg); + + try + { + // Odom state publisher + odom_s_publisher_ = + get_node()->create_publisher("~/odometry", rclcpp::SystemDefaultsQoS()); + rt_odom_state_publisher_ = std::make_unique(odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_odom_state_publisher_->lock(); + rt_odom_state_publisher_->msg_.header.stamp = get_node()->now(); + rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id; + rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; + + auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + } + rt_odom_state_publisher_->unlock(); + + try + { + // Tf State publisher + tf_odom_s_publisher_ = + get_node()->create_publisher("~/tf_odometry", rclcpp::SystemDefaultsQoS()); + rt_tf_odom_state_publisher_ = std::make_unique(tf_odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_tf_odom_state_publisher_->lock(); + rt_tf_odom_state_publisher_->msg_.transforms.resize(1); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.stamp = get_node()->now(); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.odom_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = params_.base_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; + rt_tf_odom_state_publisher_->unlock(); + + try + { + // controller State publisher + controller_s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + controller_state_publisher_ = + std::make_unique(controller_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, + "Exception thrown during publisher creation at configure stage " + "with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + controller_state_publisher_->lock(); + controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + controller_state_publisher_->unlock(); + + RCLCPP_INFO(get_node()->get_logger(), "MecanumDriveController configured successfully"); + + return controller_interface::CallbackReturn::SUCCESS; +} + +void MecanumDriveController::reference_callback(const std::shared_ptr msg) +{ + // If no timestamp provided, use current time for command timestamp + if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + msg->header.stamp = get_node()->now(); + } + + const auto age_of_last_command = get_node()->now() - msg->header.stamp; + + // Check the timeout condition + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + input_ref_.writeFromNonRT(msg); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older by %.10f than allowed timeout (%.4f).", + rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + + reset_controller_reference_msg(msg, get_node()); + } +} + +controller_interface::InterfaceConfiguration +MecanumDriveController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(command_joint_names_.size()); + for (const auto & joint : command_joint_names_) + { + command_interfaces_config.names.push_back(joint + "/" + hardware_interface::HW_IF_VELOCITY); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration MecanumDriveController::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(state_joint_names_.size()); + + for (const auto & joint : state_joint_names_) + { + state_interfaces_config.names.push_back(joint + "/" + hardware_interface::HW_IF_VELOCITY); + } + + return state_interfaces_config; +} + +std::vector +MecanumDriveController::on_export_reference_interfaces() +{ + reference_interfaces_.resize(NR_REF_ITFS, std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + + reference_interfaces.reserve(reference_interfaces_.size()); + + std::vector reference_interface_names = { + "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + + for (size_t i = 0; i < reference_interfaces_.size(); ++i) + { + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), reference_interface_names[i], &reference_interfaces_[i])); + } + + return reference_interfaces; +} + +bool MecanumDriveController::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn MecanumDriveController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set default value in command + reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MecanumDriveController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + bool value_set_no_error = true; + for (size_t i = 0; i < NR_CMD_ITFS; ++i) + { + value_set_no_error &= + command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + } + if (!value_set_no_error) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Setting values to command interfaces has failed! " + "This means that you are maybe blocking the interface in your hardware for too long."); + return controller_interface::CallbackReturn::FAILURE; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type MecanumDriveController::update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + auto current_ref = *(input_ref_.readFromRT()); + const auto age_of_last_command = time - (current_ref)->header.stamp; + + // send message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + if ( + !std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && + !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.linear.y; + reference_interfaces_[2] = current_ref->twist.angular.z; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } + } + } + else + { + if ( + !std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && + !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + reference_interfaces_[2] = 0.0; + + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.linear.y = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type MecanumDriveController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // FORWARD KINEMATICS (odometry). + const double wheel_front_left_state_vel = state_interfaces_[FRONT_LEFT].get_value(); + const double wheel_front_right_state_vel = state_interfaces_[FRONT_RIGHT].get_value(); + const double wheel_rear_right_state_vel = state_interfaces_[REAR_RIGHT].get_value(); + const double wheel_rear_left_state_vel = state_interfaces_[REAR_LEFT].get_value(); + + if ( + !std::isnan(wheel_front_left_state_vel) && !std::isnan(wheel_rear_left_state_vel) && + !std::isnan(wheel_rear_right_state_vel) && !std::isnan(wheel_front_right_state_vel)) + { + // Estimate twist (using joint information) and integrate + odometry_.update( + wheel_front_left_state_vel, wheel_rear_left_state_vel, wheel_rear_right_state_vel, + wheel_front_right_state_vel, period.seconds()); + } + + // INVERSE KINEMATICS (move robot). + // Compute wheels velocities (this is the actual ik): + // NOTE: the input desired twist (from topic `~/reference`) is a body twist. + if ( + !std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]) && + !std::isnan(reference_interfaces_[2])) + { + tf2::Quaternion quaternion; + quaternion.setRPY(0.0, 0.0, params_.kinematics.base_frame_offset.theta); + /// \note The variables meaning: + /// rotation_from_base_to_center: Rotation transformation matrix, to transform from + /// base frame to center frame + /// linear_trans_from_base_to_center: offset/linear transformation matrix, to + /// transform from base frame to center frame + + tf2::Matrix3x3 rotation_from_base_to_center = tf2::Matrix3x3((quaternion)); + tf2::Vector3 velocity_in_base_frame_w_r_t_center_frame_ = + rotation_from_base_to_center * + tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); + tf2::Vector3 linear_trans_from_base_to_center = tf2::Vector3( + params_.kinematics.base_frame_offset.x, params_.kinematics.base_frame_offset.y, 0.0); + + velocity_in_center_frame_linear_x_ = + velocity_in_base_frame_w_r_t_center_frame_.x() + + linear_trans_from_base_to_center.y() * reference_interfaces_[2]; + velocity_in_center_frame_linear_y_ = + velocity_in_base_frame_w_r_t_center_frame_.y() - + linear_trans_from_base_to_center.x() * reference_interfaces_[2]; + velocity_in_center_frame_angular_z_ = reference_interfaces_[2]; + + const double wheel_front_left_vel = + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ - + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); + const double wheel_front_right_vel = + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ + + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); + const double wheel_rear_right_vel = + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ + + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); + const double wheel_rear_left_vel = + 1.0 / params_.kinematics.wheels_radius * + (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + velocity_in_center_frame_angular_z_); + + // Set wheels velocities - The joint names are sorted according to the order documented in the + // header file! + const bool value_set_error = + command_interfaces_[FRONT_LEFT].set_value(wheel_front_left_vel) && + command_interfaces_[FRONT_RIGHT].set_value(wheel_front_right_vel) && + command_interfaces_[REAR_RIGHT].set_value(wheel_rear_right_vel) && + command_interfaces_[REAR_LEFT].set_value(wheel_rear_left_vel); + RCLCPP_ERROR_EXPRESSION( + get_node()->get_logger(), !value_set_error, + "Setting values to command interfaces has failed! " + "This means that you are maybe blocking the interface in your hardware for too long."); + } + else + { + const bool value_set_error = command_interfaces_[FRONT_LEFT].set_value(0.0) && + command_interfaces_[FRONT_RIGHT].set_value(0.0) && + command_interfaces_[REAR_RIGHT].set_value(0.0) && + command_interfaces_[REAR_LEFT].set_value(0.0); + RCLCPP_ERROR_EXPRESSION( + get_node()->get_logger(), !value_set_error, + "Setting values to command interfaces has failed! " + "This means that you are maybe blocking the interface in your hardware for too long."); + } + + // Publish odometry message + // Compute and store orientation info + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getRz()); + + // Populate odom message and publish + if (rt_odom_state_publisher_->trylock()) + { + rt_odom_state_publisher_->msg_.header.stamp = time; + rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.getX(); + rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.getY(); + rt_odom_state_publisher_->msg_.pose.pose.orientation = tf2::toMsg(orientation); + rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.getVx(); + rt_odom_state_publisher_->msg_.twist.twist.linear.y = odometry_.getVy(); + rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.getWz(); + rt_odom_state_publisher_->unlockAndPublish(); + } + + // Publish tf /odom frame + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) + { + rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = odometry_.getX(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = odometry_.getY(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = + tf2::toMsg(orientation); + rt_tf_odom_state_publisher_->unlockAndPublish(); + } + + if (controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_publisher_->msg_.front_left_wheel_velocity = + state_interfaces_[FRONT_LEFT].get_value(); + controller_state_publisher_->msg_.front_right_wheel_velocity = + state_interfaces_[FRONT_RIGHT].get_value(); + controller_state_publisher_->msg_.back_right_wheel_velocity = + state_interfaces_[REAR_RIGHT].get_value(); + controller_state_publisher_->msg_.back_left_wheel_velocity = + state_interfaces_[REAR_LEFT].get_value(); + controller_state_publisher_->msg_.reference_velocity.linear.x = reference_interfaces_[0]; + controller_state_publisher_->msg_.reference_velocity.linear.y = reference_interfaces_[1]; + controller_state_publisher_->msg_.reference_velocity.angular.z = reference_interfaces_[2]; + controller_state_publisher_->unlockAndPublish(); + } + + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[2] = std::numeric_limits::quiet_NaN(); + + return controller_interface::return_type::OK; +} + +} // namespace mecanum_drive_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + mecanum_drive_controller::MecanumDriveController, + controller_interface::ChainableControllerInterface) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml new file mode 100644 index 0000000000..896b731953 --- /dev/null +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -0,0 +1,148 @@ +mecanum_drive_controller: + reference_timeout: { + type: double, + default_value: 0.0, + description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behavior if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", + } + + # Command joint names + front_left_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding front left wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + front_right_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding front right wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + rear_right_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding rear right wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + rear_left_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding rear left wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + # State joint names + front_left_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of front left wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + front_right_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of front right wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + rear_right_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of rear right wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + rear_left_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of rear left wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + kinematics: + base_frame_offset: + x: { + type: double, + default_value: 0.0, + description: "Base frame offset along X axis of base_frame (base_link frame)." , + read_only: true, + } + y: { + type: double, + default_value: 0.0, + description: "Base frame offset along Y axis of base_frame (base_link frame)." , + read_only: true, + } + theta: { + type: double, + default_value: 0.0, + description: "Base frame offset along Theta axis of base_frame (base_link frame).", + read_only: true, + } + + wheels_radius: { + type: double, + default_value: 0.0, + description: "Wheel's radius.", + read_only: false, + validation: { + gt<>: [0.0] + } + } + + sum_of_robot_center_projection_on_X_Y_axis: { + type: double, + default_value: 0.0, + description: "Wheels geometric param used in mecanum wheels' IK. lx and ly represent the distance from the robot's center to the wheels projected on the x and y axis with origin at robots center respectively, sum_of_robot_center_projection_on_X_Y_axis = lx+ly", + read_only: false, + } + + base_frame_id: { + type: string, + default_value: "base_link", + description: "Base frame_id set to value of base_frame_id.", + read_only: false, + + } + odom_frame_id: { + type: string, + default_value: "odom", + description: "Odometry frame_id set to value of odom_frame_id.", + read_only: false, + } + enable_odom_tf: { + type: bool, + default_value: true, + description: "Publishing to tf is enabled or disabled?", + read_only: false, + } + + twist_covariance_diagonal: { + type: double_array, + default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + description: "Diagonal values of twist covariance matrix.", + read_only: false, + } + + pose_covariance_diagonal: { + type: double_array, + default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + description: "Diagonal values of pose covariance matrix.", + read_only: false, + } diff --git a/mecanum_drive_controller/src/odometry.cpp b/mecanum_drive_controller/src/odometry.cpp new file mode 100644 index 0000000000..bb873fcfdb --- /dev/null +++ b/mecanum_drive_controller/src/odometry.cpp @@ -0,0 +1,124 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "mecanum_drive_controller/odometry.hpp" + +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace mecanum_drive_controller +{ +Odometry::Odometry() +: timestamp_(0.0), + position_x_in_base_frame_(0.0), + position_y_in_base_frame_(0.0), + orientation_z_in_base_frame_(0.0), + velocity_in_base_frame_linear_x(0.0), + velocity_in_base_frame_linear_y(0.0), + velocity_in_base_frame_angular_z(0.0), + sum_of_robot_center_projection_on_X_Y_axis_(0.0), + wheels_radius_(0.0) +{ +} + +void Odometry::init( + const rclcpp::Time & time, std::array base_frame_offset) +{ + // Reset timestamp: + timestamp_ = time; + + // Base frame offset (wrt to center frame). + base_frame_offset_[0] = base_frame_offset[0]; + base_frame_offset_[1] = base_frame_offset[1]; + base_frame_offset_[2] = base_frame_offset[2]; +} + +bool Odometry::update( + const double wheel_front_left_vel, const double wheel_rear_left_vel, + const double wheel_rear_right_vel, const double wheel_front_right_vel, const double dt) +{ + /// We cannot estimate the speed with very small time intervals: + // const double dt = (time - timestamp_).toSec(); + if (dt < 0.0001) return false; // Interval too small to integrate with + + /// Compute FK (i.e. compute mobile robot's body twist out of its wheels velocities): + /// NOTE: the mecanum IK gives the body speed at the center frame, we then offset this velocity + /// at the base frame. + /// NOTE: in the diff drive the velocity is filtered out, but we prefer to return it raw and + /// let the user perform post-processing at will. + /// We prefer this way of doing as filtering introduces delay (which makes it difficult + /// to interpret and compare behavior curves). + + /// \note The variables meaning: + /// angular_transformation_from_center_2_base: Rotation transformation matrix, to transform + /// from center frame to base frame + /// linear_transformation_from_center_2_base: offset/linear transformation matrix, + /// to transform from center frame to base frame + + double velocity_in_center_frame_linear_x = + 0.25 * wheels_radius_ * + (wheel_front_left_vel + wheel_rear_left_vel + wheel_rear_right_vel + wheel_front_right_vel); + double velocity_in_center_frame_linear_y = + 0.25 * wheels_radius_ * + (-wheel_front_left_vel + wheel_rear_left_vel - wheel_rear_right_vel + wheel_front_right_vel); + double velocity_in_center_frame_angular_z = + 0.25 * wheels_radius_ / sum_of_robot_center_projection_on_X_Y_axis_ * + (-wheel_front_left_vel - wheel_rear_left_vel + wheel_rear_right_vel + wheel_front_right_vel); + + tf2::Quaternion orientation_R_c_b; + orientation_R_c_b.setRPY(0.0, 0.0, -base_frame_offset_[2]); + + tf2::Matrix3x3 angular_transformation_from_center_2_base = tf2::Matrix3x3((orientation_R_c_b)); + tf2::Vector3 velocity_in_center_frame_w_r_t_base_frame_ = + angular_transformation_from_center_2_base * + tf2::Vector3(velocity_in_center_frame_linear_x, velocity_in_center_frame_linear_y, 0.0); + tf2::Vector3 linear_transformation_from_center_2_base = + angular_transformation_from_center_2_base * + tf2::Vector3(-base_frame_offset_[0], -base_frame_offset_[1], 0.0); + + velocity_in_base_frame_linear_x = + velocity_in_center_frame_w_r_t_base_frame_.x() + + linear_transformation_from_center_2_base.y() * velocity_in_center_frame_angular_z; + velocity_in_base_frame_linear_y = + velocity_in_center_frame_w_r_t_base_frame_.y() - + linear_transformation_from_center_2_base.x() * velocity_in_center_frame_angular_z; + velocity_in_base_frame_angular_z = velocity_in_center_frame_angular_z; + + /// Integration. + /// NOTE: the position is expressed in the odometry frame , unlike the twist which is + /// expressed in the body frame. + orientation_z_in_base_frame_ += velocity_in_base_frame_angular_z * dt; + + tf2::Quaternion orientation_R_b_odom; + orientation_R_b_odom.setRPY(0.0, 0.0, orientation_z_in_base_frame_); + + tf2::Matrix3x3 angular_transformation_from_base_2_odom = tf2::Matrix3x3((orientation_R_b_odom)); + tf2::Vector3 velocity_in_base_frame_w_r_t_odom_frame_ = + angular_transformation_from_base_2_odom * + tf2::Vector3(velocity_in_base_frame_linear_x, velocity_in_base_frame_linear_y, 0.0); + + position_x_in_base_frame_ += velocity_in_base_frame_w_r_t_odom_frame_.x() * dt; + position_y_in_base_frame_ += velocity_in_base_frame_w_r_t_odom_frame_.y() * dt; + + return true; +} + +void Odometry::setWheelsParams( + const double sum_of_robot_center_projection_on_X_Y_axis, const double wheels_radius) +{ + sum_of_robot_center_projection_on_X_Y_axis_ = sum_of_robot_center_projection_on_X_Y_axis; + wheels_radius_ = wheels_radius; +} + +} // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml new file mode 100644 index 0000000000..580b4058d7 --- /dev/null +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -0,0 +1,19 @@ +test_mecanum_drive_controller: + ros__parameters: + reference_timeout: 0.9 + + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "back_right_wheel_joint" + rear_left_wheel_command_joint_name: "back_left_wheel_joint" + + kinematics: + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } + wheels_radius: 0.5 + sum_of_robot_center_projection_on_X_Y_axis: 1.0 + + base_frame_id: "base_link" + odom_frame_id: "odom" + enable_odom_tf: true + twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml new file mode 100644 index 0000000000..c4e5bb391a --- /dev/null +++ b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml @@ -0,0 +1,24 @@ +test_mecanum_drive_controller: + ros__parameters: + reference_timeout: 0.1 + + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "back_right_wheel_joint" + rear_left_wheel_command_joint_name: "back_left_wheel_joint" + + front_left_wheel_state_joint_name: "state_front_left_wheel_joint" + front_right_wheel_state_joint_name: "state_front_right_wheel_joint" + rear_right_wheel_state_joint_name: "state_back_right_wheel_joint" + rear_left_wheel_state_joint_name: "state_back_left_wheel_joint" + + kinematics: + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } + wheels_radius: 0.5 + sum_of_robot_center_projection_on_X_Y_axis: 1.0 + + base_frame_id: "base_link" + odom_frame_id: "odom" + enable_odom_tf: true + twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] diff --git a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp new file mode 100644 index 0000000000..16fa3e815a --- /dev/null +++ b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp @@ -0,0 +1,52 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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(TestLoadMecanumDriveController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/mecanum_drive_controller_params.yaml"; + + cm.set_parameter({"test_mecanum_drive_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_mecanum_drive_controller.type", "mecanum_drive_controller/MecanumDriveController"}); + + ASSERT_NE(cm.load_controller("test_mecanum_drive_controller"), nullptr); + + rclcpp::shutdown(); +} + +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/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp new file mode 100644 index 0000000000..5327e890e9 --- /dev/null +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -0,0 +1,576 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_mecanum_drive_controller.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using mecanum_drive_controller::NR_CMD_ITFS; +using mecanum_drive_controller::NR_REF_ITFS; +using mecanum_drive_controller::NR_STATE_ITFS; + +class MecanumDriveControllerTest +: public MecanumDriveControllerFixture +{ +}; + +namespace +{ +// Floating-point value comparison threshold +const double EPS = 1e-3; +} // namespace + +TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set) +{ + SetUpController(); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.0); + ASSERT_EQ(controller_->params_.kinematics.wheels_radius, 0.0); + ASSERT_EQ(controller_->params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, 0.0); + + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.x, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); + + ASSERT_TRUE(controller_->params_.front_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.9); + ASSERT_EQ(controller_->params_.kinematics.wheels_radius, 0.5); + ASSERT_EQ(controller_->params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis, 1.0); + + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.x, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); + ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); + + ASSERT_EQ( + controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_right_wheel_command_joint_name, TEST_FRONT_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.rear_right_wheel_command_joint_name, TEST_REAR_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_command_joint_name, TEST_REAR_LEFT_CMD_JOINT_NAME); + ASSERT_THAT(controller_->command_joint_names_, testing::ElementsAreArray(command_joint_names_)); + + // When state joint names are empty they are the same as the command joint names + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(command_joint_names_)); +} + +TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check command itfs configuration + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); + } + + // check state itfs configuration + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { + EXPECT_EQ(state_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); + } + + // check ref itfs configuration, + + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), NR_REF_ITFS); + + for (size_t i = 0; i < reference_interface_names.size(); ++i) + { + const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + + std::string("/") + reference_interface_names[i]; + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), reference_interface_names[i]); + } +} + +TEST_F(MecanumDriveControllerTest, when_controller_is_activated_expect_reference_reset) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + + ASSERT_TRUE(std::isnan((*msg)->twist.angular.z)); +} + +TEST_F(MecanumDriveControllerTest, when_controller_active_and_update_called_expect_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_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(MecanumDriveControllerTest, when_active_controller_is_deactivated_expect_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( + MecanumDriveControllerTest, + when_controller_is_reactivated_expect_cmd_itfs_not_set_and_update_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_->command_interfaces_[NR_CMD_ITFS - 4].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value())); + + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +// when update is called expect the previously set reference before calling update, +// inside the controller state message +TEST_F(MecanumDriveControllerTest, when_update_is_called_expect_status_message) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + controller_->reference_interfaces_[0] = 1.5; + + ControllerStateMsg msg; + subscribe_to_controller_status_execute_update_and_get_messages(msg); + + EXPECT_EQ(msg.reference_velocity.linear.x, 1.5); +} + +// reference_interfaces and command_interfaces values depend on the reference_msg, +// the below test shows two cases when reference_msg is not received and when it is received. +TEST_F( + MecanumDriveControllerTest, + when_reference_msg_received_expect_updated_commands_and_status_message) +{ + 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); + // no reference_msg sent + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ControllerStateMsg msg; + subscribe_to_controller_status_execute_update_and_get_messages(msg); + joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; + + EXPECT_TRUE(std::isnan(msg.reference_velocity.linear.x)); + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(controller_->get_node()->now()); + controller_->wait_for_commands(executor); + + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // REAR LEFT vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + // joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); + + subscribe_to_controller_status_execute_update_and_get_messages(msg); + + ASSERT_EQ(msg.reference_velocity.linear.x, 1.5); + ASSERT_EQ(msg.back_left_wheel_velocity, 0.1); + ASSERT_EQ(msg.back_right_wheel_velocity, 0.1); +} + +// when too old msg is sent expect reference msg reset +TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_reference) +{ + 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); + + auto reference = *(controller_->input_ref_.readFromNonRT()); + auto old_timestamp = reference->header.stamp; + EXPECT_TRUE(std::isnan(reference->twist.linear.x)); + EXPECT_TRUE(std::isnan(reference->twist.linear.y)); + EXPECT_TRUE(std::isnan(reference->twist.angular.z)); + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands( + controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1)); + controller_->wait_for_commands(executor); + ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp); + EXPECT_TRUE(std::isnan((reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((reference)->twist.linear.y)); + EXPECT_TRUE(std::isnan((reference)->twist.angular.z)); +} + +// when time stamp is zero expect that time stamp is set to current time stamp +TEST_F( + MecanumDriveControllerTest, + when_reference_msg_has_timestamp_zero_expect_reference_set_and_timestamp_set_to_current_time) +{ + 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); + + auto reference = controller_->input_ref_.readFromNonRT(); + auto old_timestamp = (*reference)->header.stamp; + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(rclcpp::Time(0)); + + controller_->wait_for_commands(executor); + ASSERT_EQ(old_timestamp.sec, (*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); + EXPECT_NE((*(controller_->input_ref_.readFromNonRT()))->header.stamp.sec, 0.0); +} + +// when the reference_msg has valid timestamp then the timeout check in reference_callback() +// shall pass and reference_msg is not reset +TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_reference_set) +{ + 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); + + auto reference = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*reference)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*reference)->twist.angular.z)); + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(controller_->get_node()->now()); + + controller_->wait_for_commands(executor); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); +} + +// when not in chainable mode and ref_msg_timedout expect +// command_interfaces are set to 0.0 and when ref_msg is not timedout expect +// command_interfaces are set to valid command values +TEST_F( + MecanumDriveControllerTest, + when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds) +{ + // 1. age>ref_timeout 2. ageget_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; + + std::shared_ptr msg = std::make_shared(); + + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 0); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); + } + + std::shared_ptr msg_2 = std::make_shared(); + msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01); + msg_2->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg_2->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg_2->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg_2->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg_2); + const auto age_of_last_command_2 = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // age_of_last_command_2 < ref_timeout_ + ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NE(joint_command_values_[controller_->get_rear_left_wheel_index()], command_lin_x); + // BACK Left vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + // joint_command_values_[controller_->get_rear_left_wheel_index()] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * + // 0.0) + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0); + } +} + +// when in chained mode the reference_interfaces of chained controller and command_interfaces +// of preceding controller point to same memory location, hence reference_interfaces are not +// exclusively set by the update method of chained controller +TEST_F( + MecanumDriveControllerTest, + when_controller_in_chainable_mode_expect_receiving_commands_from_reference_interfaces_directly) +{ + 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); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; + // imitating preceding controllers command_interfaces setting reference_interfaces of chained + // controller. + controller_->reference_interfaces_[0] = 3.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + + // reference_callback() is implicitly called when publish_commands() is called + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(controller_->get_node()->now()); + + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NE(joint_command_values_[controller_->get_rear_left_wheel_index()], command_lin_x); + + // REAR LEFT vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + + // joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (3.0 - 0.0 - 1 * 0.0) + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 6.0); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 6.0); + } +} + +// when ref_timeout = 0 expect reference_msg is accepted only once and command_interfaces +// are calculated to valid values and reference_interfaces are unset +TEST_F( + MecanumDriveControllerTest, + when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once) +{ + 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); + + // set command statically + joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; + + controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); + std::shared_ptr msg = std::make_shared(); + + msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_FALSE(std::isnan(joint_command_values_[1])); + EXPECT_NE(joint_command_values_[controller_->get_rear_left_wheel_index()], command_lin_x); + // REAR LEFT vel = + // 1.0 / params_.kinematics.wheels_radius * + // (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + // params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * + // velocity_in_center_frame_angular_z_); + // joint_command_values_[REAR_LEFT] = 1.0 / 0.5 * (1.5 - 0.0 - 1 * 0.0) + EXPECT_EQ(joint_command_values_[controller_->get_rear_left_wheel_index()], 3.0); + ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); +} + +TEST_F( + MecanumDriveControllerTest, + when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once) +{ + 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); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); + + // reference_callback() is called implicitly when publish_commands() is called. + // reference_msg is published with provided time stamp when publish_commands( time_stamp) + // is called + publish_commands(controller_->get_node()->now()); + + controller_->wait_for_commands(executor); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, 1.5); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.y, 0.0); + EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, 0.0); +} + +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/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp new file mode 100644 index 0000000000..5f7cf36dc7 --- /dev/null +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -0,0 +1,317 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_MECANUM_DRIVE_CONTROLLER_HPP_ +#define TEST_MECANUM_DRIVE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "mecanum_drive_controller/mecanum_drive_controller.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using ControllerStateMsg = mecanum_drive_controller::MecanumDriveController::ControllerStateMsg; +using ControllerReferenceMsg = + mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; +using TfStateMsg = mecanum_drive_controller::MecanumDriveController::TfStateMsg; +using OdomStateMsg = mecanum_drive_controller::MecanumDriveController::OdomStateMsg; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableMecanumDriveController : public mecanum_drive_controller::MecanumDriveController +{ + FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set); + FRIEND_TEST( + MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces); + FRIEND_TEST(MecanumDriveControllerTest, when_controller_is_activated_expect_reference_reset); + FRIEND_TEST(MecanumDriveControllerTest, when_controller_active_and_update_called_expect_success); + FRIEND_TEST(MecanumDriveControllerTest, when_active_controller_is_deactivated_expect_success); + FRIEND_TEST( + MecanumDriveControllerTest, + when_controller_is_reactivated_expect_cmd_itfs_not_set_and_update_success); + FRIEND_TEST(MecanumDriveControllerTest, when_update_is_called_expect_status_message); + FRIEND_TEST( + MecanumDriveControllerTest, + when_reference_msg_received_expect_updated_commands_and_status_message); + FRIEND_TEST(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_reference); + FRIEND_TEST( + MecanumDriveControllerTest, + when_reference_msg_has_timestamp_zero_expect_reference_set_and_timestamp_set_to_current_time); + FRIEND_TEST(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_reference_set); + FRIEND_TEST( + MecanumDriveControllerTest, + when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds); + FRIEND_TEST( + MecanumDriveControllerTest, + when_controller_in_chainable_mode_expect_receiving_commands_from_reference_interfaces_directly); + FRIEND_TEST( + MecanumDriveControllerTest, + when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once); + FRIEND_TEST( + MecanumDriveControllerTest, + when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + return mecanum_drive_controller::MecanumDriveController::on_configure(previous_state); + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return mecanum_drive_controller::MecanumDriveController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + */ + void wait_for_command( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + void wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + wait_for_command(executor, timeout); + } + + size_t get_front_left_wheel_index() { return WheelIndex::FRONT_LEFT; } + + size_t get_front_right_wheel_index() { return WheelIndex::FRONT_RIGHT; } + + size_t get_rear_right_wheel_index() { return WheelIndex::REAR_RIGHT; } + + size_t get_rear_left_wheel_index() { return WheelIndex::REAR_LEFT; } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class MecanumDriveControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_mecanum_drive_controller/reference", rclcpp::SystemDefaultsQoS()); + + odom_s_publisher_node_ = std::make_shared("odom_s_publisher"); + odom_s_publisher_ = odom_s_publisher_node_->create_publisher( + "/test_mecanum_drive_controller/odometry", rclcpp::SystemDefaultsQoS()); + + tf_odom_s_publisher_node_ = std::make_shared("tf_odom_s_publisher"); + tf_odom_s_publisher_ = tf_odom_s_publisher_node_->create_publisher( + "/test_mecanum_drive_controller/tf_odometry", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_mecanum_drive_controller") + { + const auto urdf = ""; + const auto ns = ""; + ASSERT_EQ( + controller_->init(controller_name, urdf, 0, ns, controller_->define_custom_node_options()), + controller_interface::return_type::OK); + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + for (size_t i = 0; i < joint_command_values_.size(); ++i) + { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + command_joint_names_[i], interface_name_, &joint_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + for (size_t i = 0; i < joint_state_values_.size(); ++i) + { + state_itfs_.emplace_back(hardware_interface::StateInterface( + command_joint_names_[i], interface_name_, &joint_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_to_controller_status_execute_update_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; + auto subscription = test_subscription_node.create_subscription( + "/test_mecanum_drive_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); + // call update to publish the test value + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); + + // take message from subscription + msg = *received_msg; + } + + void publish_commands( + const rclcpp::Time & stamp, const double & twist_linear_x = 1.5, + const double & twist_linear_y = 0.0, const double & twist_angular_z = 0.0) + { + 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(10)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.header.stamp = stamp; + msg.twist.linear.x = twist_linear_x; + msg.twist.linear.y = twist_linear_y; + msg.twist.linear.z = std::numeric_limits::quiet_NaN(); + msg.twist.angular.x = std::numeric_limits::quiet_NaN(); + msg.twist.angular.y = std::numeric_limits::quiet_NaN(); + msg.twist.angular.z = twist_angular_z; + + command_publisher_->publish(msg); + } + +protected: + std::vector reference_interface_names = { + "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + + static constexpr char TEST_FRONT_LEFT_CMD_JOINT_NAME[] = "front_left_wheel_joint"; + static constexpr char TEST_FRONT_RIGHT_CMD_JOINT_NAME[] = "front_right_wheel_joint"; + static constexpr char TEST_REAR_RIGHT_CMD_JOINT_NAME[] = "back_right_wheel_joint"; + static constexpr char TEST_REAR_LEFT_CMD_JOINT_NAME[] = "back_left_wheel_joint"; + std::vector command_joint_names_ = { + TEST_FRONT_LEFT_CMD_JOINT_NAME, TEST_FRONT_RIGHT_CMD_JOINT_NAME, TEST_REAR_RIGHT_CMD_JOINT_NAME, + TEST_REAR_LEFT_CMD_JOINT_NAME}; + + static constexpr char TEST_FRONT_LEFT_STATE_JOINT_NAME[] = "state_front_left_wheel_joint"; + static constexpr char TEST_FRONT_RIGHT_STATE_JOINT_NAME[] = "state_front_right_wheel_joint"; + static constexpr char TEST_REAR_RIGHT_STATE_JOINT_NAME[] = "state_back_right_wheel_joint"; + static constexpr char TEST_REAR_LEFT_STATE_JOINT_NAME[] = "state_back_left_wheel_joint"; + std::vector state_joint_names_ = { + TEST_FRONT_LEFT_STATE_JOINT_NAME, TEST_FRONT_RIGHT_STATE_JOINT_NAME, + TEST_REAR_RIGHT_STATE_JOINT_NAME, TEST_REAR_LEFT_STATE_JOINT_NAME}; + std::string interface_name_ = hardware_interface::HW_IF_VELOCITY; + + // Controller-related parameters + + std::array joint_state_values_ = {{0.1, 0.1, 0.1, 0.1}}; + std::array joint_command_values_ = {{101.101, 101.101, 101.101, 101.101}}; + + static constexpr double TEST_LINEAR_VELOCITY_X = 1.5; + static constexpr double TEST_LINEAR_VELOCITY_y = 0.0; + static constexpr double TEST_ANGULAR_VELOCITY_Z = 0.0; + double command_lin_x = 111; + + std::vector state_itfs_; + std::vector command_itfs_; + + double ref_timeout_ = 0.1; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Node::SharedPtr odom_s_publisher_node_; + rclcpp::Publisher::SharedPtr odom_s_publisher_; + rclcpp::Node::SharedPtr tf_odom_s_publisher_node_; + rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; +}; + +#endif // TEST_MECANUM_DRIVE_CONTROLLER_HPP_ diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp new file mode 100644 index 0000000000..edbee8a702 --- /dev/null +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -0,0 +1,99 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_mecanum_drive_controller.hpp" + +#include +#include +#include +#include +#include + +using mecanum_drive_controller::NR_CMD_ITFS; +using mecanum_drive_controller::NR_REF_ITFS; +using mecanum_drive_controller::NR_STATE_ITFS; + +class MecanumDriveControllerTest +: public MecanumDriveControllerFixture +{ +}; + +TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_parameters_set) +{ + SetUpController(); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.0); + + ASSERT_TRUE(controller_->params_.front_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(controller_->params_.reference_timeout, 0.1); + + ASSERT_EQ( + controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_right_wheel_command_joint_name, TEST_FRONT_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ( + controller_->params_.rear_right_wheel_command_joint_name, TEST_REAR_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_command_joint_name, TEST_REAR_LEFT_CMD_JOINT_NAME); + ASSERT_THAT(controller_->command_joint_names_, testing::ElementsAreArray(command_joint_names_)); + + ASSERT_EQ( + controller_->params_.front_left_wheel_state_joint_name, TEST_FRONT_LEFT_STATE_JOINT_NAME); + ASSERT_EQ( + controller_->params_.front_right_wheel_state_joint_name, TEST_FRONT_RIGHT_STATE_JOINT_NAME); + ASSERT_EQ( + controller_->params_.rear_right_wheel_state_joint_name, TEST_REAR_RIGHT_STATE_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_state_joint_name, TEST_REAR_LEFT_STATE_JOINT_NAME); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); +} + +// checking if all interfaces, command and state interfaces are exported as expected +TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], command_joint_names_[i] + "/" + interface_name_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { + EXPECT_EQ(state_intefaces.names[i], state_joint_names_[i] + "/" + interface_name_); + } +} + +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/ros2_controllers/package.xml b/ros2_controllers/package.xml index 4969658788..99ca06c77b 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -28,6 +28,7 @@ imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + mecanum_drive_controller pid_controller position_controllers range_sensor_broadcaster diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 44b180162e..8889824d62 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -10,7 +10,7 @@ steering_controllers_library .. _twist_msg: https://github.com/ros2/common_interfaces/blob/{DISTRO}/geometry_msgs/msg/TwistStamped.msg .. _tf_msg: https://github.com/ros2/geometry2/blob/{DISTRO}/tf2_msgs/msg/TFMessage.msg -Library with shared functionalities for mobile robot controllers with steering drives, with so-called non-holonomic constraints. +Library with shared functionalities for mobile robot controllers with steering drives (2 degrees of freedom), with so-called non-holonomic constraints. The library implements generic odometry and update methods and defines the main interfaces. From a5bb11bf1402c9d2e50e21e4ce7380f2b4f14218 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 22 Nov 2024 11:15:31 +0100 Subject: [PATCH 81/97] Add missing deps for test_nodes (#1378) --- ros2_controllers_test_nodes/package.xml | 2 ++ .../test_publisher_forward_position_controller_launch.py | 6 ++---- .../test_publisher_joint_trajectory_controller_launch.py | 5 ++--- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index c53f1b9971..0b97d6bb0c 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -21,6 +21,8 @@ trajectory_msgs python3-pytest + launch_testing_ros + launch_ros ament_python diff --git a/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py index 8ebb2df7b3..3f4c9da21f 100644 --- a/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py +++ b/ros2_controllers_test_nodes/test/test_publisher_forward_position_controller_launch.py @@ -34,15 +34,13 @@ from launch import LaunchDescription from launch.substitutions import PathJoinSubstitution +import launch_ros.actions from launch_ros.substitutions import FindPackageShare from launch_testing.actions import ReadyToTest -from launch_testing_ros import WaitForTopics - import launch_testing.markers +from launch_testing_ros import WaitForTopics import rclpy -import launch_ros.actions from rclpy.node import Node - from std_msgs.msg import Float64MultiArray diff --git a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py index d8d6e2a577..62ad25550d 100644 --- a/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py +++ b/ros2_controllers_test_nodes/test/test_publisher_joint_trajectory_controller_launch.py @@ -34,13 +34,12 @@ from launch import LaunchDescription from launch.substitutions import PathJoinSubstitution +import launch_ros.actions from launch_ros.substitutions import FindPackageShare from launch_testing.actions import ReadyToTest -from launch_testing_ros import WaitForTopics - import launch_testing.markers +from launch_testing_ros import WaitForTopics import rclpy -import launch_ros.actions from rclpy.node import Node from trajectory_msgs.msg import JointTrajectory From 1bd6870a1115a0551506bbd45afd88c6ce16c2a8 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 22 Nov 2024 11:22:20 +0100 Subject: [PATCH 82/97] [JTC] Sample at t + dT instead of the beginning of the control cycle (#1306) --- .../joint_trajectory_controller.hpp | 2 + .../trajectory.hpp | 11 +- .../src/joint_trajectory_controller.cpp | 29 ++- .../src/trajectory.cpp | 8 +- .../test/test_trajectory_controller.cpp | 166 ++++++++++++------ .../test/test_trajectory_controller_utils.hpp | 4 +- 6 files changed, 147 insertions(+), 73 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 2f0a1f4df0..67954b8378 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 @@ -112,6 +112,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Preallocate variables used in the realtime update() function trajectory_msgs::msg::JointTrajectoryPoint state_current_; trajectory_msgs::msg::JointTrajectoryPoint command_current_; + trajectory_msgs::msg::JointTrajectoryPoint command_next_; trajectory_msgs::msg::JointTrajectoryPoint state_desired_; trajectory_msgs::msg::JointTrajectoryPoint state_error_; @@ -124,6 +125,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Parameters from ROS for joint_trajectory_controller std::shared_ptr param_listener_; Params params_; + rclcpp::Duration update_period_{0, 0}; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; /// Specify interpolation method. Default to splines. diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 14373b006e..d650c369d0 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -69,8 +69,10 @@ class Trajectory * acceleration respectively. Deduction assumes that the provided velocity or acceleration have to * be reached at the time defined in the segment. * - * This function assumes that sampling is only done at monotonically increasing \p sample_time - * for any trajectory. + * This function by default assumes that sampling is only done at monotonically increasing \p + * sample_time for any trajectory. That means, it will only search for a point matching the sample + * time after the point it has been called before. If this isn't desired, set \p + * search_monotonically_increasing to false. * * Specific case returns for start_segment_itr and end_segment_itr: * - Sampling before the trajectory start: @@ -94,13 +96,16 @@ class Trajectory * description above. * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See * description above. + * \param[in] search_monotonically_increasing If set to true, the next sample call will start + * searching in the trajectory at the index of this call's result. */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool sample( const rclcpp::Time & sample_time, const interpolation_methods::InterpolationMethod interpolation_method, trajectory_msgs::msg::JointTrajectoryPoint & output_state, - TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr); + TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr, + const bool search_monotonically_increasing = true); /** * Do interpolation between 2 states given a time in between their respective timestamps diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index bea37e02be..0fc39ecaaf 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -180,6 +181,7 @@ controller_interface::return_type JointTrajectoryController::update( if (has_active_trajectory()) { bool first_sample = false; + TrajectoryPointConstIter start_segment_itr, end_segment_itr; // if sampling the first time, set the point before you sample if (!traj_external_point_ptr_->is_sampled_already()) { @@ -196,11 +198,15 @@ controller_interface::return_type JointTrajectoryController::update( } } - // find segment for current timestamp - TrajectoryPointConstIter start_segment_itr, end_segment_itr; - const bool valid_point = traj_external_point_ptr_->sample( + // Sample expected state from the trajectory + traj_external_point_ptr_->sample( time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + // Sample setpoint for next control cycle + const bool valid_point = traj_external_point_ptr_->sample( + time + update_period_, interpolation_method_, command_next_, start_segment_itr, + end_segment_itr, false); + if (valid_point) { const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start(); @@ -276,7 +282,7 @@ controller_interface::return_type JointTrajectoryController::update( // Update PIDs for (auto i = 0ul; i < dof_; ++i) { - tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + + tmp_command_[i] = (command_next_.velocities[i] * ff_velocity_scale_[i]) + pids_[i]->computeCommand( state_error_.positions[i], state_error_.velocities[i], (uint64_t)period.nanoseconds()); @@ -286,7 +292,7 @@ controller_interface::return_type JointTrajectoryController::update( // set values for next hardware write() if (has_position_command_interface_) { - assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); + assign_interface_from_point(joint_command_interface_[0], command_next_.positions); } if (has_velocity_command_interface_) { @@ -296,12 +302,12 @@ controller_interface::return_type JointTrajectoryController::update( } else { - assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); + assign_interface_from_point(joint_command_interface_[1], command_next_.velocities); } } if (has_acceleration_command_interface_) { - assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); + assign_interface_from_point(joint_command_interface_[2], command_next_.accelerations); } if (has_effort_command_interface_) { @@ -309,7 +315,7 @@ controller_interface::return_type JointTrajectoryController::update( } // store the previous command. Used in open-loop control mode - last_commanded_state_ = state_desired_; + last_commanded_state_ = command_next_; } if (active_goal) @@ -867,6 +873,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::string(get_node()->get_name()) + "/query_state", std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); + if (get_update_rate() == 0) + { + throw std::runtime_error("Controller's update rate is set to 0. This should not happen!"); + } + update_period_ = + rclcpp::Duration(0.0, static_cast(1.0e9 / static_cast(get_update_rate()))); + return CallbackReturn::SUCCESS; } diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 512b16f3c5..1c50d26c62 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -91,7 +91,8 @@ bool Trajectory::sample( const rclcpp::Time & sample_time, const interpolation_methods::InterpolationMethod interpolation_method, trajectory_msgs::msg::JointTrajectoryPoint & output_state, - TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr) + TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr, + const bool search_monotonically_increasing) { THROW_ON_NULLPTR(trajectory_msg_) @@ -176,7 +177,10 @@ bool Trajectory::sample( } start_segment_itr = begin() + i; end_segment_itr = begin() + (i + 1); - last_sample_idx_ = i; + if (search_monotonically_increasing) + { + last_sample_idx_ = i; + } return true; } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index f188c0f04b..21d0277541 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -53,7 +53,7 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) traj_controller_->get_node()->set_parameter( rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); - const auto state = traj_controller_->get_node()->configure(); + const auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // send msg @@ -84,7 +84,7 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names) rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); - const auto state = traj_controller_->get_node()->configure(); + const auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); compare_joints(joint_names_, joint_names_); @@ -97,7 +97,7 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_); SetUpTrajectoryController(executor, {command_joint_names_param}); - const auto state = traj_controller_->get_node()->configure(); + const auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); compare_joints(joint_names_, command_joint_names_); @@ -108,7 +108,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); - auto state = traj_controller_->get_node()->configure(); + auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); auto cmd_if_conf = traj_controller_->command_interface_configuration(); @@ -162,7 +162,7 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup_after_configure) SetUpTrajectoryController(executor); // configure controller - auto state = traj_controller_->get_node()->configure(); + auto state = traj_controller_->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); // cleanup controller @@ -178,6 +178,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param SetUpTrajectoryController(executor); traj_controller_->get_node()->set_parameter( rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); + traj_controller_->get_node()->set_parameter(rclcpp::Parameter("update_rate", 10)); // This call is replacing the way parameters are set via launch auto state = traj_controller_->configure(); @@ -206,8 +207,10 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // wait for reaching the first point // controller would process the second point when deactivated below + // Since the trajectory will be sampled at time + update_period, we need to pass the time 0.15 + // seconds to sample at t=0.25 sec traj_controller_->update( - rclcpp::Time(static_cast(0.25 * 1e9)), rclcpp::Duration::from_seconds(0.15)); + rclcpp::Time(static_cast(0.15 * 1e9)), rclcpp::Duration::from_seconds(0.15)); EXPECT_TRUE(traj_controller_->has_active_traj()); if (traj_controller_->has_position_command_interface()) { @@ -644,7 +647,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun size_t n_joints = joint_names_.size(); // send msg - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(300); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; // *INDENT-OFF* std::vector> points{ @@ -653,7 +656,23 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun publish(time_from_start, points, rclcpp::Time()); traj_controller_->wait_for_trajectory(executor); - updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + const rclcpp::Duration controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); + + auto end_time = updateControllerAsync( + rclcpp::Duration(FIRST_POINT_TIME) - controller_period, rclcpp::Time(0, 0, RCL_STEADY_TIME), + controller_period); + if (traj_controller_->has_position_command_interface()) + { + // check command interface + // One step before the first point, the target should hit the setpoint + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); + } + + // Propagate to actual setpoint time + traj_controller_->update(end_time + controller_period, controller_period); // get states from class variables auto state_feedback = traj_controller_->get_state_feedback(); @@ -678,14 +697,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); EXPECT_NEAR(state_error.positions[2], state_reference.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], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); - } - if (traj_controller_->has_velocity_command_interface()) { // use_closed_loop_pid_adapter_ @@ -745,7 +756,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) size_t n_joints = joint_names_.size(); // send msg - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(300); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; // *INDENT-OFF* std::vector> points{ @@ -756,7 +767,23 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + const rclcpp::Duration controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); + auto end_time = updateControllerAsync( + rclcpp::Duration(FIRST_POINT_TIME) - controller_period, rclcpp::Time(0, 0, RCL_STEADY_TIME), + controller_period); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + // One step before the first point, the target should hit the setpoint + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); + } + + // Propagate to actual setpoint time + traj_controller_->update(end_time + controller_period, controller_period); // get states from class variables auto state_feedback = traj_controller_->get_state_feedback(); @@ -782,14 +809,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) EXPECT_NEAR( state_error.positions[2], state_reference.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], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); - } - if (traj_controller_->has_velocity_command_interface()) { // use_closed_loop_pid_adapter_ @@ -1548,16 +1567,27 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired = expected_actual; + traj_controller_->wait_for_trajectory(executor); + auto end_time = updateControllerAsync( + rclcpp::Duration(delay), rclcpp::Time(0, 0, RCL_STEADY_TIME), + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate())); + // Check that we reached end of points_old[0] trajectory - auto end_time = - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + if (traj_controller_->has_position_command_interface()) + { + auto state_feedback = traj_controller_->get_state_feedback(); + for (size_t i = 0; i < expected_actual.positions.size(); ++i) + { + EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], 0.01); + } + } 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(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; - expected_desired = expected_actual; + expected_desired = expected_actual; // robot should be standing still by now publish(time_from_start, points_new, new_traj_start); waitAndCompareState( expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); @@ -1577,10 +1607,20 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory // send points_old and wait to reach first point publish(time_from_start, points_old, rclcpp::Time()); 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 - auto end_time = - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + traj_controller_->wait_for_trajectory(executor); + auto end_time = updateControllerAsync( + rclcpp::Duration(delay), rclcpp::Time(0, 0, RCL_STEADY_TIME), + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate())); + + if (traj_controller_->has_position_command_interface()) + { + // Check that we reached end of points_old[0] trajectory + auto state_feedback = traj_controller_->get_state_feedback(); + for (size_t i = 0; i < expected_actual.positions.size(); ++i) + { + EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], 0.01); + } + } // send points_new before the old trajectory is finished RCLCPP_INFO( @@ -1653,6 +1693,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // only makes sense with position command interface return; } + auto controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1665,8 +1707,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; - double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / - (time_from_start.sec + time_from_start.nanosec * 1e-9); + double trajectory_frac = + controller_period.seconds() / (time_from_start.sec + time_from_start.nanosec * 1e-9); std::vector> points{{first_goal}}; publish( time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); @@ -1690,7 +1732,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // 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); - auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + auto end_time = updateControllerAsync(controller_period); // Expect backward commands at first, consider advancement of the trajectory // exact value is not directly predictable, because of the spline interpolation -> increase // tolerance @@ -1699,10 +1741,10 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro 0.1); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); @@ -1721,17 +1763,19 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // 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); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + end_time = updateControllerAsync(controller_period); // Expect backward commands at first, consider advancement of the trajectory + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance EXPECT_NEAR( joint_state_pos_[0] + (first_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], - COMMON_THRESHOLD); + 0.1); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); @@ -1747,13 +1791,17 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e rclcpp::executors::SingleThreadedExecutor executor; // set open loop to true, this should change behavior from above rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + rclcpp::Parameter update_rate_param("update_rate", 100); + SetUpAndActivateTrajectoryController( + executor, {is_open_loop_parameters, update_rate_param}, true); if (traj_controller_->has_position_command_interface() == false) { // only makes sense with position command interface return; } + auto controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1764,8 +1812,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; - double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / - (time_from_start.sec + time_from_start.nanosec * 1e-9); + double trajectory_frac = + controller_period.seconds() / (time_from_start.sec + time_from_start.nanosec * 1e-9); std::vector> points{{first_goal}}; publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); traj_controller_->wait_for_trajectory(executor); @@ -1788,17 +1836,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // 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); - auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + auto end_time = updateControllerAsync(controller_period); // There should not be backward commands + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance EXPECT_NEAR( - first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0], - COMMON_THRESHOLD); + first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0], 0.1); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); @@ -1817,17 +1866,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // 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); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); // There should not be a jump toward commands + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance EXPECT_NEAR( - second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0], - COMMON_THRESHOLD); + second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0], 0.1); EXPECT_LT(joint_pos_[0], second_goal[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); @@ -2098,7 +2148,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame // command interfaces: empty command_interface_types_ = {}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); - auto state = traj_controller_->get_node()->configure(); + auto state = traj_controller_->configure(); EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); // command interfaces: bad_name @@ -2143,7 +2193,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame command_interface_types_ = {"velocity"}; state_interface_types_ = {"position"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); - state = traj_controller_->get_node()->configure(); + state = traj_controller_->configure(); EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); state_interface_types_ = {"velocity"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); @@ -2152,7 +2202,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame command_interface_types_ = {"effort"}; state_interface_types_ = {"position"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); - state = traj_controller_->get_node()->configure(); + state = traj_controller_->configure(); EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); state_interface_types_ = {"velocity"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index a850891331..7dff81f08f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -278,7 +278,7 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_->set_node_options(node_options); return traj_controller_->init( - controller_name_, urdf, 0, "", traj_controller_->define_custom_node_options()); + controller_name_, urdf, 100, "", traj_controller_->define_custom_node_options()); } void SetPidParameters(double p_value = 0.0, double ff_value = 1.0) @@ -324,7 +324,7 @@ class TrajectoryControllerTest : public ::testing::Test // set pid parameters before configure SetPidParameters(k_p, ff); - traj_controller_->get_node()->configure(); + traj_controller_->configure(); ActivateTrajectoryController( separate_cmd_and_state_values, initial_pos_joints, initial_vel_joints, initial_acc_joints, From 9b344c7b5c06879b85cd4bcc135e35b5f7eedc2f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 22 Nov 2024 14:22:49 +0100 Subject: [PATCH 83/97] Add another dependency (#1382) --- ros2_controllers_test_nodes/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 0b97d6bb0c..21eb31f6df 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -17,6 +17,7 @@ https://github.com/ros-controls/ros2_controllers/ rclpy + sensor_msgs std_msgs trajectory_msgs From d8b30f864ea70126e989cc451ddab482be8850f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 27 Nov 2024 22:47:43 +0100 Subject: [PATCH 84/97] Rename Twist->TwistStamped (#1393) --- .../diff_drive_controller.hpp | 14 +++++------ .../src/diff_drive_controller.cpp | 23 ++++++++++--------- 2 files changed, 19 insertions(+), 18 deletions(-) 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 ac34b81eca..74526b199c 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 @@ -45,7 +45,7 @@ namespace diff_drive_controller { class DiffDriveController : public controller_interface::ControllerInterface { - using Twist = geometry_msgs::msg::TwistStamped; + using TwistStamped = geometry_msgs::msg::TwistStamped; public: DIFF_DRIVE_CONTROLLER_PUBLIC @@ -128,20 +128,20 @@ class DiffDriveController : public controller_interface::ControllerInterface realtime_odometry_transform_publisher_ = nullptr; bool subscriber_is_active_ = false; - rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; - std::queue previous_commands_; // last two commands + std::queue previous_commands_; // last two commands // speed limiters SpeedLimiter limiter_linear_; SpeedLimiter limiter_angular_; bool publish_limited_velocity_ = false; - std::shared_ptr> limited_velocity_publisher_ = nullptr; - std::shared_ptr> realtime_limited_velocity_publisher_ = - nullptr; + std::shared_ptr> limited_velocity_publisher_ = nullptr; + std::shared_ptr> + realtime_limited_velocity_publisher_ = nullptr; rclcpp::Time previous_update_timestamp_{0}; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 66da6d6738..d21cac602d 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -111,7 +111,7 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; + std::shared_ptr last_command_msg; received_velocity_msg_ptr_.get(last_command_msg); if (last_command_msg == nullptr) @@ -130,7 +130,7 @@ controller_interface::return_type DiffDriveController::update( // command may be limited further by SpeedLimit, // without affecting the stored twist command - Twist command = *last_command_msg; + TwistStamped command = *last_command_msg; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; @@ -318,23 +318,24 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( if (publish_limited_velocity_) { - limited_velocity_publisher_ = - get_node()->create_publisher(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); + limited_velocity_publisher_ = get_node()->create_publisher( + DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_limited_velocity_publisher_ = - std::make_shared>(limited_velocity_publisher_); + std::make_shared>( + limited_velocity_publisher_); } - const Twist empty_twist; - received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); + const TwistStamped empty_twist; + received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); // Fill last two commands with default constructed commands previous_commands_.emplace(empty_twist); previous_commands_.emplace(empty_twist); // initialize command subscriber - velocity_command_subscriber_ = get_node()->create_subscription( + 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_) { @@ -475,7 +476,7 @@ controller_interface::CallbackReturn DiffDriveController::on_cleanup( return controller_interface::CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); + received_velocity_msg_ptr_.set(std::make_shared()); return controller_interface::CallbackReturn::SUCCESS; } @@ -493,7 +494,7 @@ bool DiffDriveController::reset() odometry_.resetOdometry(); // release the old queue - std::queue empty; + std::queue empty; std::swap(previous_commands_, empty); registered_left_wheel_handles_.clear(); From f64c964b2ed772d7d75e8d7e5d46a1f35c8159bb Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 29 Nov 2024 16:21:01 +0100 Subject: [PATCH 85/97] JTC: sum periods (#1395) --- .../joint_trajectory_controller.hpp | 2 ++ .../src/joint_trajectory_controller.cpp | 11 ++++++++--- .../test/test_trajectory_actions.cpp | 9 ++++++--- 3 files changed, 16 insertions(+), 6 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 67954b8378..2377b2c0b6 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 @@ -127,6 +127,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa Params params_; rclcpp::Duration update_period_{0, 0}; + rclcpp::Time traj_time_; + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; /// Specify interpolation method. Default to splines. interpolation_methods::InterpolationMethod interpolation_method_{ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 0fc39ecaaf..82d9c94aab 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -196,15 +196,20 @@ controller_interface::return_type JointTrajectoryController::update( traj_external_point_ptr_->set_point_before_trajectory_msg( time, state_current_, joints_angle_wraparound_); } + traj_time_ = time; + } + else + { + traj_time_ += period; } // Sample expected state from the trajectory traj_external_point_ptr_->sample( - time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + traj_time_, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); // Sample setpoint for next control cycle const bool valid_point = traj_external_point_ptr_->sample( - time + update_period_, interpolation_method_, command_next_, start_segment_itr, + traj_time_ + update_period_, interpolation_method_, command_next_, start_segment_itr, end_segment_itr, false); if (valid_point) @@ -217,7 +222,7 @@ controller_interface::return_type JointTrajectoryController::update( // time_difference is // - negative until first point is reached // - counting from zero to time_from_start of next point - double time_difference = time.seconds() - segment_time_from_start.seconds(); + double time_difference = traj_time_.seconds() - segment_time_from_start.seconds(); bool tolerance_violated_while_moving = false; bool outside_goal_tolerance = false; bool within_goal_time = true; diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 164f7d0e11..d4979deef8 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -83,12 +83,15 @@ class TestTrajectoryActions : public TrajectoryControllerTest { // controller hardware cycle update loop auto clock = rclcpp::Clock(RCL_STEADY_TIME); - auto start_time = clock.now(); + auto now_time = clock.now(); + auto last_time = now_time; rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0); - auto end_time = start_time + wait; + auto end_time = last_time + wait; while (clock.now() < end_time) { - traj_controller_->update(clock.now(), clock.now() - start_time); + now_time = clock.now(); + traj_controller_->update(now_time, now_time - last_time); + last_time = now_time; } }); From ff3c8a0242bb65e00f7bd65b85add08275c2bcfa Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 1 Dec 2024 11:09:50 +0100 Subject: [PATCH 86/97] Bump version of pre-commit hooks (#1401) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 205e0f63ab..75c5402ffe 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.3 + rev: v19.1.4 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.4 + rev: 0.30.0 hooks: - id: check-github-workflows args: ["--verbose"] From 36068e180aa510ab79a9c19a91f9de8ad5e74f06 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 1 Dec 2024 23:02:56 +0100 Subject: [PATCH 87/97] Add explicit cast to period.count() (#1404) --- .../include/gripper_controllers/hardware_interface_adapter.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index 2cc24794b8..98fd5a56b1 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -165,7 +165,8 @@ class HardwareInterfaceAdapter // Time since the last call to update const auto period = std::chrono::steady_clock::now() - last_update_time_; // Update PIDs - double command = pid_->computeCommand(error_position, error_velocity, period.count()); + double command = + pid_->computeCommand(error_position, error_velocity, static_cast(period.count())); command = std::min( fabs(max_allowed_effort), std::max(-fabs(max_allowed_effort), command)); joint_handle_->get().set_value(command); From f9edc41a451877e292b206069aa2e8bfd25148c0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 4 Dec 2024 11:16:28 +0100 Subject: [PATCH 88/97] Fix RealtimeBox API changes (#1385) --- .../diff_drive_controller.hpp | 1 + .../src/diff_drive_controller.cpp | 29 ++++++++++--------- .../test/test_diff_drive_controller.cpp | 3 +- .../tricycle_controller.hpp | 1 + .../src/tricycle_controller.cpp | 26 +++++++++-------- .../test/test_tricycle_controller.cpp | 3 +- 6 files changed, 35 insertions(+), 28 deletions(-) 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 74526b199c..84712fe748 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 @@ -131,6 +131,7 @@ class DiffDriveController : public controller_interface::ControllerInterface rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + std::shared_ptr last_command_msg_; std::queue previous_commands_; // last two commands diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index d21cac602d..d710c81257 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -111,26 +111,27 @@ controller_interface::return_type DiffDriveController::update( return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; - received_velocity_msg_ptr_.get(last_command_msg); + // if the mutex is unable to lock, last_command_msg_ won't be updated + received_velocity_msg_ptr_.try_get([this](const std::shared_ptr & msg) + { last_command_msg_ = msg; }); - if (last_command_msg == nullptr) + if (last_command_msg_ == nullptr) { RCLCPP_WARN(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; + 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; + last_command_msg_->twist.linear.x = 0.0; + last_command_msg_->twist.angular.z = 0.0; } // command may be limited further by SpeedLimit, // without affecting the stored twist command - TwistStamped command = *last_command_msg; + TwistStamped command = *last_command_msg_; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; @@ -325,12 +326,12 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( limited_velocity_publisher_); } - const TwistStamped empty_twist; - received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); - + last_command_msg_ = std::make_shared(); + received_velocity_msg_ptr_.set([this](std::shared_ptr & stored_value) + { stored_value = last_command_msg_; }); // Fill last two commands with default constructed commands - previous_commands_.emplace(empty_twist); - previous_commands_.emplace(empty_twist); + previous_commands_.emplace(*last_command_msg_); + previous_commands_.emplace(*last_command_msg_); // initialize command subscriber velocity_command_subscriber_ = get_node()->create_subscription( @@ -350,7 +351,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( "time, this message will only be shown once"); msg->header.stamp = get_node()->get_clock()->now(); } - received_velocity_msg_ptr_.set(std::move(msg)); + received_velocity_msg_ptr_.set([msg](std::shared_ptr & stored_value) + { stored_value = std::move(msg); }); }); // initialize odometry publisher and message @@ -476,7 +478,6 @@ controller_interface::CallbackReturn DiffDriveController::on_cleanup( return controller_interface::CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 72ae4dbfd7..e29c6bbfee 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -49,7 +49,8 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr std::shared_ptr getLastReceivedTwist() { std::shared_ptr ret; - received_velocity_msg_ptr_.get(ret); + received_velocity_msg_ptr_.get( + [&ret](const std::shared_ptr & msg) { ret = msg; }); return ret; } diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 010a890f52..1f9361dadd 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -139,6 +139,7 @@ class TricycleController : public controller_interface::ControllerInterface rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + std::shared_ptr last_command_msg_; rclcpp::Service::SharedPtr reset_odom_service_; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index ec7ca7bd5e..1d6daba958 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -95,25 +95,27 @@ controller_interface::return_type TricycleController::update( } 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) + + // if the mutex is unable to lock, last_command_msg_ won't be updated + received_velocity_msg_ptr_.try_get([this](const std::shared_ptr & msg) + { last_command_msg_ = 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; + 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; + 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; + 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 @@ -271,9 +273,9 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & return CallbackReturn::ERROR; } - const TwistStamped empty_twist; - received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); - + last_command_msg_ = std::make_shared(); + received_velocity_msg_ptr_.set([this](std::shared_ptr & stored_value) + { stored_value = last_command_msg_; }); // Fill last two commands with default constructed commands const AckermannDrive empty_ackermann_drive; previous_commands_.emplace(empty_ackermann_drive); @@ -307,7 +309,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & "time, this message will only be shown once"); msg->header.stamp = get_node()->get_clock()->now(); } - received_velocity_msg_ptr_.set(std::move(msg)); + received_velocity_msg_ptr_.set([msg](std::shared_ptr & stored_value) + { stored_value = std::move(msg); }); }); // initialize odometry publisher and message @@ -397,7 +400,6 @@ CallbackReturn TricycleController::on_cleanup(const rclcpp_lifecycle::State &) return CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); return CallbackReturn::SUCCESS; } diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 9d43c2590d..5e868ebeea 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -54,7 +54,8 @@ class TestableTricycleController : public tricycle_controller::TricycleControlle std::shared_ptr getLastReceivedTwist() { std::shared_ptr ret; - received_velocity_msg_ptr_.get(ret); + received_velocity_msg_ptr_.get( + [&ret](const std::shared_ptr & msg) { ret = msg; }); return ret; } From cd730558b2c449568c627f066cdf8bfa8bc4c3bf Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Dec 2024 15:06:45 +0100 Subject: [PATCH 89/97] Use the .hpp headers from `realtime_tools` package (#1406) --- .../include/admittance_controller/admittance_controller.hpp | 4 ++-- .../include/diff_drive_controller/diff_drive_controller.hpp | 4 ++-- .../force_torque_sensor_broadcaster.hpp | 2 +- .../forward_command_controller/forward_controllers_base.hpp | 2 +- .../include/gpio_controllers/gpio_command_controller.hpp | 4 ++-- .../gripper_controllers/gripper_action_controller.hpp | 4 ++-- .../imu_sensor_broadcaster/imu_sensor_broadcaster.hpp | 2 +- .../joint_state_broadcaster/joint_state_broadcaster.hpp | 2 +- .../joint_trajectory_controller.hpp | 6 +++--- .../mecanum_drive_controller/mecanum_drive_controller.hpp | 4 ++-- .../include/mecanum_drive_controller/odometry.hpp | 4 ++-- .../parallel_gripper_action_controller.hpp | 4 ++-- pid_controller/include/pid_controller/pid_controller.hpp | 4 ++-- .../include/pose_broadcaster/pose_broadcaster.hpp | 2 +- .../range_sensor_broadcaster/range_sensor_broadcaster.hpp | 2 +- .../steering_controllers_library.hpp | 4 ++-- .../include/tricycle_controller/tricycle_controller.hpp | 4 ++-- 17 files changed, 29 insertions(+), 29 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 9be6c3298c..89cd195ce4 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -32,8 +32,8 @@ #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/force_torque_sensor.hpp" namespace admittance_controller 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 84712fe748..4fc4d37d4b 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 @@ -34,8 +34,8 @@ #include "nav_msgs/msg/odometry.hpp" #include "odometry.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_box.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_box.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "tf2_msgs/msg/tf_message.hpp" // auto-generated by generate_parameter_library 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 e5a5349c32..2364dd7c8b 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 @@ -28,7 +28,7 @@ #include "force_torque_sensor_broadcaster_parameters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/force_torque_sensor.hpp" namespace force_torque_sensor_broadcaster diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index 4858e5ab64..fce7941d8a 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -23,7 +23,7 @@ #include "forward_command_controller/visibility_control.h" #include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_buffer.hpp" #include "std_msgs/msg/float64_multi_array.hpp" namespace forward_command_controller diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 5149c74a1c..febac1294e 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -26,8 +26,8 @@ #include "gpio_controllers/visibility_control.h" #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 "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" namespace gpio_controllers { diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 38e0bd8191..478168391b 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -37,8 +37,8 @@ #include "gripper_controllers/visibility_control.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_server_goal_handle.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_server_goal_handle.hpp" // Project #include "gripper_controllers/hardware_interface_adapter.hpp" 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 1ba0b032ac..449020b6e2 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 @@ -26,7 +26,7 @@ // auto-generated by generate_parameter_library #include "imu_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/imu_sensor.hpp" #include "sensor_msgs/msg/imu.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 ecc3c767f6..7ac98eccfb 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 @@ -25,7 +25,7 @@ #include "joint_state_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "joint_state_broadcaster_parameters.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "sensor_msgs/msg/joint_state.hpp" #include "urdf/model.h" 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 2377b2c0b6..324ccfe4f1 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 @@ -36,9 +36,9 @@ #include "rclcpp/timer.hpp" #include "rclcpp_action/server.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" -#include "realtime_tools/realtime_server_goal_handle.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_server_goal_handle.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 0fd7f6af29..f835752d43 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -29,8 +29,8 @@ #include "mecanum_drive_controller_parameters.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "std_srvs/srv/set_bool.hpp" #include "control_msgs/msg/mecanum_drive_controller_state.hpp" diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index ac1ad060dd..241809e8c2 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -16,8 +16,8 @@ #define MECANUM_DRIVE_CONTROLLER__ODOMETRY_HPP_ #include "geometry_msgs/msg/twist.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #define PLANAR_POINT_DIM 3 diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp index d303990b89..739d1c570a 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -37,8 +37,8 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "parallel_gripper_controller/visibility_control.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_server_goal_handle.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_server_goal_handle.hpp" // Project #include "parallel_gripper_action_controller_parameters.hpp" diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index ce66fd06d4..0aba6cf849 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -29,8 +29,8 @@ #include "pid_controller/visibility_control.h" #include "pid_controller_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "std_srvs/srv/set_bool.hpp" namespace pid_controller diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp index 621a90cc85..9317fce5e1 100644 --- a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -25,7 +25,7 @@ #include "pose_broadcaster_parameters.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/pose_sensor.hpp" #include "tf2_msgs/msg/tf_message.hpp" diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp index 5a93f95982..2e4e47b018 100644 --- a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp @@ -25,7 +25,7 @@ #include "range_sensor_broadcaster/visibility_control.h" #include "range_sensor_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/range_sensor.hpp" #include "sensor_msgs/msg/range.hpp" diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index 84a892d79e..49236986ee 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -23,8 +23,8 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "steering_controllers_library/steering_odometry.hpp" #include "steering_controllers_library/visibility_control.h" #include "steering_controllers_library_parameters.hpp" diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 1f9361dadd..4d64549613 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -33,8 +33,8 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_box.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_box.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "tricycle_controller/odometry.hpp" From b9a7cfc7dcdd4cb6c49971ae906344d45ac2fb15 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 6 Dec 2024 10:15:25 +0100 Subject: [PATCH 90/97] Don't call shutdown() after an exception (#1400) --- .../publisher_forward_position_controller.py | 6 +----- .../publisher_joint_trajectory_controller.py | 6 +----- 2 files changed, 2 insertions(+), 10 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 51582a90d4..27fb4535df 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 @@ -70,14 +70,10 @@ def main(args=None): try: rclpy.spin(publisher_forward_position) - except KeyboardInterrupt: + except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): print("Keyboard interrupt received. Shutting down node.") except Exception as e: print(f"Unhandled exception: {e}") - finally: - if rclpy.ok(): - publisher_forward_position.destroy_node() - rclpy.shutdown() if __name__ == "__main__": 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 91d39855aa..cf38890407 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 @@ -186,14 +186,10 @@ def main(args=None): try: rclpy.spin(publisher_joint_trajectory) - except KeyboardInterrupt: + except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): print("Keyboard interrupt received. Shutting down node.") except Exception as e: print(f"Unhandled exception: {e}") - finally: - if rclpy.ok(): - publisher_joint_trajectory.destroy_node() - rclpy.shutdown() if __name__ == "__main__": From dc60f6fb888096b90a81cae4564c86653a95e4de Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 6 Dec 2024 10:38:09 +0100 Subject: [PATCH 91/97] Add missing dependency to gpio_controllers (#1410) --- gpio_controllers/package.xml | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index 9b98746545..c8c60fe522 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -4,10 +4,21 @@ gpio_controllers 4.15.0 Controllers to interact with gpios. - Maciej Bednarczyk - Wiktor Bajor + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Maciej Bednarczyk + Wiktor Bajor + ament_cmake controller_interface @@ -22,6 +33,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets From 27f59461b9506e32d26b46d86a79c2264c5b0498 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 7 Dec 2024 09:56:47 +0000 Subject: [PATCH 92/97] Update changelogs & align version numbers --- ackermann_steering_controller/CHANGELOG.rst | 6 + admittance_controller/CHANGELOG.rst | 7 + bicycle_steering_controller/CHANGELOG.rst | 6 + diff_drive_controller/CHANGELOG.rst | 9 + effort_controllers/CHANGELOG.rst | 6 + force_torque_sensor_broadcaster/CHANGELOG.rst | 7 + forward_command_controller/CHANGELOG.rst | 7 + gpio_controllers/CHANGELOG.rst | 214 ++++++++++++++++++ gpio_controllers/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 8 + imu_sensor_broadcaster/CHANGELOG.rst | 7 + joint_state_broadcaster/CHANGELOG.rst | 7 + joint_trajectory_controller/CHANGELOG.rst | 9 + mecanum_drive_controller/CHANGELOG.rst | 213 +++++++++++++++++ mecanum_drive_controller/package.xml | 2 +- parallel_gripper_controller/CHANGELOG.rst | 7 + pid_controller/CHANGELOG.rst | 7 + pose_broadcaster/CHANGELOG.rst | 7 + position_controllers/CHANGELOG.rst | 6 + range_sensor_broadcaster/CHANGELOG.rst | 7 + ros2_controllers/CHANGELOG.rst | 7 + ros2_controllers_test_nodes/CHANGELOG.rst | 9 + rqt_joint_trajectory_controller/CHANGELOG.rst | 5 + steering_controllers_library/CHANGELOG.rst | 8 + tricycle_controller/CHANGELOG.rst | 9 + tricycle_steering_controller/CHANGELOG.rst | 6 + velocity_controllers/CHANGELOG.rst | 6 + 27 files changed, 592 insertions(+), 2 deletions(-) create mode 100644 gpio_controllers/CHANGELOG.rst create mode 100644 mecanum_drive_controller/CHANGELOG.rst diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 1f275bd7ba..504617d317 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 0d7daa5f5d..aa2736002f 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- * Adding use of robot description parameter in the Admittance Controller (`#1247 `_) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 57d23a977a..27c0e3f27d 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index e79f1faf1d..cb6c718ffa 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Fix RealtimeBox API changes (`#1385 `_) +* Rename Twist->TwistStamped (`#1393 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 0abc5feee9..09dc92a195 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 6bd9e2febd..d3a7c3d0de 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- * [ForceTorqueSensorBroadcaster] added force and torque offsets to the parameters + export state interfaces (`#1215 `_) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index ae920d9f83..730a7a3c6a 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/gpio_controllers/CHANGELOG.rst b/gpio_controllers/CHANGELOG.rst new file mode 100644 index 0000000000..02ade18ca6 --- /dev/null +++ b/gpio_controllers/CHANGELOG.rst @@ -0,0 +1,214 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package gpio_controllers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Add missing dependency to gpio_controllers (`#1410 `_) +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Gpio command controller (`#1251 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Wiktor Bajor + +4.16.0 (2024-11-08) +------------------- + +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- + +4.11.0 (2024-07-09) +------------------- + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-02-12) +------------------ + +4.5.0 (2024-01-31) +------------------ + +4.4.0 (2024-01-11) +------------------ + +4.3.0 (2024-01-08) +------------------ + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-12-01) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.17.0 (2023-10-31) +------------------- + +3.16.0 (2023-09-20) +------------------- + +3.15.0 (2023-09-11) +------------------- + +3.14.0 (2023-08-16) +------------------- + +3.13.0 (2023-08-04) +------------------- + +3.12.0 (2023-07-18) +------------------- + +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +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/gpio_controllers/package.xml b/gpio_controllers/package.xml index c8c60fe522..4f800a4cad 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -2,7 +2,7 @@ gpio_controllers - 4.15.0 + 4.16.0 Controllers to interact with gpios. Bence Magyar diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 291cab691a..512a481bcd 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add explicit cast to period.count() (`#1404 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 2219d53fe4..ba19caac8c 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index ed2f4b31eb..c89bbc6737 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- * [JSB] Fix the behaviour of publishing unavailable state interfaces when they are previously available (`#1331 `_) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 87089f700a..3003ce830b 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* JTC: sum periods (`#1395 `_) +* [JTC] Sample at t + dT instead of the beginning of the control cycle (`#1306 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Felix Exner (fexner), Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- * [JTC] Fix the JTC length_error exceptions in the tests (`#1360 `_) diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst new file mode 100644 index 0000000000..82e43faea5 --- /dev/null +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -0,0 +1,213 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package mecanum_drive_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add Mecanum Drive Controller (`#512 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + +4.16.0 (2024-11-08) +------------------- + +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- + +4.11.0 (2024-07-09) +------------------- + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-02-12) +------------------ + +4.5.0 (2024-01-31) +------------------ + +4.4.0 (2024-01-11) +------------------ + +4.3.0 (2024-01-08) +------------------ + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-12-01) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.17.0 (2023-10-31) +------------------- + +3.16.0 (2023-09-20) +------------------- + +3.15.0 (2023-09-11) +------------------- + +3.14.0 (2023-08-16) +------------------- + +3.13.0 (2023-08-04) +------------------- + +3.12.0 (2023-07-18) +------------------- + +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +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/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index 97aadf4327..e446098de6 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -2,7 +2,7 @@ mecanum_drive_controller - 0.0.0 + 4.16.0 Implementation of mecanum drive controller for 4 wheel drive. diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 6ee5646ac4..7d12f514e1 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 9c20af4e97..dc6b61831d 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- * fixes for windows compilation (`#1330 `_) diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index c12bc9da80..44af47b18c 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- * Add hardware_interface_testing dependency (`#1335 `_) diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index e5478f518a..0c6460ff34 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 5d97df0e0b..ab9dbb2f09 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index a45a425f24..62235a2440 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add Mecanum Drive Controller (`#512 `_) +* Gpio command controller (`#1251 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Dr. Denis, Wiktor Bajor + 4.16.0 (2024-11-08) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index d3c3059abf..782f49edb9 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Don't call shutdown() after an exception (`#1400 `_) +* Add another dependency (`#1382 `_) +* Add missing deps for test_nodes (`#1378 `_) +* test_nodes: catch keyboard interrupt and add simple launch tests (`#1369 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich + 4.16.0 (2024-11-08) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 555a636d7e..36c587c364 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 +----------- +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich + 4.16.0 (2024-11-08) ------------------- diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 417c4c9f05..e46f9db9f8 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Add Mecanum Drive Controller (`#512 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Dr. Denis, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 31258cac7c..5b7e16fc1f 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use the .hpp headers from `realtime_tools` package (`#1406 `_) +* Fix RealtimeBox API changes (`#1385 `_) +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* TractionLimiter: Fix wrong input checks (`#1341 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 975c48303d..db1532b6a3 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 90b143f675..1c4f38f430 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) +* Update maintainers and add url tags (`#1363 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.16.0 (2024-11-08) ------------------- From 2c7047e527b09916ca58c6031e4cb6ee665416d8 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 7 Dec 2024 09:57:16 +0000 Subject: [PATCH 93/97] 4.17.0 --- ackermann_steering_controller/CHANGELOG.rst | 4 ++-- ackermann_steering_controller/package.xml | 2 +- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- bicycle_steering_controller/CHANGELOG.rst | 4 ++-- bicycle_steering_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 +- gpio_controllers/CHANGELOG.rst | 4 ++-- gpio_controllers/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 +- mecanum_drive_controller/CHANGELOG.rst | 4 ++-- mecanum_drive_controller/package.xml | 2 +- parallel_gripper_controller/CHANGELOG.rst | 4 ++-- parallel_gripper_controller/package.xml | 2 +- pid_controller/CHANGELOG.rst | 4 ++-- pid_controller/package.xml | 2 +- pose_broadcaster/CHANGELOG.rst | 4 ++-- pose_broadcaster/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- range_sensor_broadcaster/CHANGELOG.rst | 4 ++-- range_sensor_broadcaster/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 +- steering_controllers_library/CHANGELOG.rst | 4 ++-- steering_controllers_library/package.xml | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- tricycle_steering_controller/CHANGELOG.rst | 4 ++-- tricycle_steering_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 52 files changed, 77 insertions(+), 77 deletions(-) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 504617d317..fe06a8aaa2 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 6ae2f0a438..dbb466cead 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.16.0 + 4.17.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Bence Magyar diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index aa2736002f..900bb68242 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 5b48c69695..fbd0dbc8dd 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.16.0 + 4.17.0 Implementation of admittance controllers for different input and output interface. Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 27c0e3f27d..37a975fd0d 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 3fafbda077..186818c71a 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.16.0 + 4.17.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index cb6c718ffa..5789720d29 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Fix RealtimeBox API changes (`#1385 `_) * Rename Twist->TwistStamped (`#1393 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 0602d91898..6147cfcc84 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.16.0 + 4.17.0 Controller for a differential-drive mobile base. Bence Magyar diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 09dc92a195..6ecf9eefc0 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index b33224db9b..28943d2984 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.16.0 + 4.17.0 Generic controller for forwarding commands. Bence Magyar diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index d3a7c3d0de..765aad14ee 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 ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index f2f5124001..446376ebb7 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.16.0 + 4.17.0 Controller to publish state of force-torque sensors. Bence Magyar diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 730a7a3c6a..668700a056 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index be71c32052..a332d5525e 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.16.0 + 4.17.0 Generic controller for forwarding commands. Bence Magyar diff --git a/gpio_controllers/CHANGELOG.rst b/gpio_controllers/CHANGELOG.rst index 02ade18ca6..7af5d2bd31 100644 --- a/gpio_controllers/CHANGELOG.rst +++ b/gpio_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gpio_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Add missing dependency to gpio_controllers (`#1410 `_) * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Gpio command controller (`#1251 `_) diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index 4f800a4cad..a0560389aa 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -2,7 +2,7 @@ gpio_controllers - 4.16.0 + 4.17.0 Controllers to interact with gpios. Bence Magyar diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 512a481bcd..f2f2fcb4a9 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add explicit cast to period.count() (`#1404 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 33f06f226a..0de8e1b14d 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.16.0 + 4.17.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index ba19caac8c..cc97a2d28d 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index a09f09dee2..905f6c9c97 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.16.0 + 4.17.0 Controller to publish readings of IMU sensors. Bence Magyar diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index c89bbc6737..d9a94bd335 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 575c17c5a0..61915ab2b0 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.16.0 + 4.17.0 Broadcaster to publish joint state Bence Magyar diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 3003ce830b..9c609ed561 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * JTC: sum periods (`#1395 `_) * [JTC] Sample at t + dT instead of the beginning of the control cycle (`#1306 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 830321e7b6..886d37b97a 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.16.0 + 4.17.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst index 82e43faea5..22b7afb932 100644 --- a/mecanum_drive_controller/CHANGELOG.rst +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mecanum_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add Mecanum Drive Controller (`#512 `_) * Contributors: Dr. Denis, Sai Kishor Kothakota diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index e446098de6..149601e2d8 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -2,7 +2,7 @@ mecanum_drive_controller - 4.16.0 + 4.17.0 Implementation of mecanum drive controller for 4 wheel drive. diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 7d12f514e1..b085543641 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 7e712b632a..219ce0b15a 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 4.16.0 + 4.17.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index dc6b61831d..3ef0927df3 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/pid_controller/package.xml b/pid_controller/package.xml index fb3f40184a..b8912e24bf 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.16.0 + 4.17.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index 44af47b18c..3abf140f40 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index dc5d629b50..3802a7495e 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -2,7 +2,7 @@ pose_broadcaster - 4.16.0 + 4.17.0 Broadcaster to publish cartesian states. Bence Magyar diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 0c6460ff34..3a843e74bd 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 8aa7bf8117..56fbf02b64 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.16.0 + 4.17.0 Generic controller for forwarding commands. Bence Magyar diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index ab9dbb2f09..716fbbbaf6 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index a81823a6a2..a368683dd2 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.16.0 + 4.17.0 Controller to publish readings of range sensors. Bence Magyar diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 62235a2440..b59b7f93a2 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Add Mecanum Drive Controller (`#512 `_) * Gpio command controller (`#1251 `_) * Update maintainers and add url tags (`#1363 `_) diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 99ca06c77b..ec612a3139 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.16.0 + 4.17.0 Metapackage for ros2_controllers related packages Bence Magyar diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 782f49edb9..365994cede 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 ------------ +4.17.0 (2024-12-07) +------------------- * Don't call shutdown() after an exception (`#1400 `_) * Add another dependency (`#1382 `_) * Add missing deps for test_nodes (`#1378 `_) diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 21eb31f6df..12164d8e3a 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.16.0 + 4.17.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Bence Magyar diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 3900392a10..cb7332f67e 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.16.0", + version="4.17.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 36c587c364..6d8fd768cc 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 ------------ +4.17.0 (2024-12-07) +------------------- * Update maintainers and add url tags (`#1363 `_) * Contributors: Christoph Fröhlich diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index da92d9d663..570e0a707e 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 - 4.16.0 + 4.17.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 f2ee71c3d7..01ae56e876 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.16.0", + version="4.17.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index e46f9db9f8..962a44e99a 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Add Mecanum Drive Controller (`#512 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index efe9f93a67..ddeabdf96c 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.16.0 + 4.17.0 Package for steering robot configurations including odometry and interfaces. Bence Magyar diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 5b7e16fc1f..1e10cc42b3 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Use the .hpp headers from `realtime_tools` package (`#1406 `_) * Fix RealtimeBox API changes (`#1385 `_) * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 516adef248..d365725518 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.16.0 + 4.17.0 Controller for a tricycle drive mobile base Bence Magyar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index db1532b6a3..92a416eb68 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 7c26e09ba0..c49822629b 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.16.0 + 4.17.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Bence Magyar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 1c4f38f430..975e8a2afd 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.17.0 (2024-12-07) +------------------- * Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) * Update maintainers and add url tags (`#1363 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 50e662cc58..362be2fe27 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.16.0 + 4.17.0 Generic controller for forwarding commands. Bence Magyar From a6c1c0c1c209fcfee8ed81825e7c7e7784d54eee Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Mon, 9 Dec 2024 11:56:32 +0100 Subject: [PATCH 94/97] Add wrench offset for admittance controller (#1249) --- admittance_controller/doc/userdoc.rst | 3 ++ .../admittance_controller.hpp | 3 ++ .../src/admittance_controller.cpp | 40 ++++++++++++++++++- doc/release_notes.rst | 1 + 4 files changed, 46 insertions(+), 1 deletion(-) diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst index 8056a017d7..c624269d86 100644 --- a/admittance_controller/doc/userdoc.rst +++ b/admittance_controller/doc/userdoc.rst @@ -32,6 +32,9 @@ Topics ~/joint_references (input topic) [trajectory_msgs::msg::JointTrajectoryPoint] Target joint commands when controller is not in chained mode. +~/wrench_reference (input topic) [geometry_msgs::msg::WrenchStamped] + Target wrench offset (WrenchStamped has to be in the frame of the FT-sensor). + ~/state (output topic) [control_msgs::msg::AdmittanceControllerState] Topic publishing internal states. diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 89cd195ce4..17b70b264d 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -133,6 +133,8 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // ROS subscribers rclcpp::Subscription::SharedPtr input_joint_command_subscriber_; + rclcpp::Subscription::SharedPtr + input_wrench_command_subscriber_; rclcpp::Publisher::SharedPtr s_publisher_; // admittance parameters @@ -144,6 +146,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // real-time buffer realtime_tools::RealtimeBuffer> input_joint_command_; + realtime_tools::RealtimeBuffer input_wrench_command_; std::unique_ptr> state_publisher_; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 6e0e38140a..58a5bcd6e2 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -27,6 +27,23 @@ namespace admittance_controller { + +geometry_msgs::msg::Wrench add_wrenches( + const geometry_msgs::msg::Wrench & a, const geometry_msgs::msg::Wrench & b) +{ + geometry_msgs::msg::Wrench res; + + res.force.x = a.force.x + b.force.x; + res.force.y = a.force.y + b.force.y; + res.force.z = a.force.z + b.force.z; + + res.torque.x = a.torque.x + b.torque.x; + res.torque.y = a.torque.y + b.torque.y; + res.torque.z = a.torque.z + b.torque.z; + + return res; +} + controller_interface::CallbackReturn AdmittanceController::on_init() { // initialize controller config @@ -116,6 +133,7 @@ AdmittanceController::on_export_reference_interfaces() reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); position_reference_ = {}; velocity_reference_ = {}; + input_wrench_command_.reset(); // assign reference interfaces auto index = 0ul; @@ -265,6 +283,24 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( input_joint_command_subscriber_ = get_node()->create_subscription( "~/joint_references", rclcpp::SystemDefaultsQoS(), joint_command_callback); + + input_wrench_command_subscriber_ = + get_node()->create_subscription( + "~/wrench_reference", rclcpp::SystemDefaultsQoS(), + [&](const geometry_msgs::msg::WrenchStamped & msg) + { + if ( + msg.header.frame_id != admittance_->parameters_.ft_sensor.frame.id && + !msg.header.frame_id.empty()) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Ignoring wrench reference as it is on the wrong frame: " + << msg.header.frame_id << ". Expected reference frame: " + << admittance_->parameters_.ft_sensor.frame.id); + return; + } + input_wrench_command_.writeFromNonRT(msg); + }); s_publisher_ = get_node()->create_publisher( "~/status", rclcpp::SystemDefaultsQoS()); state_publisher_ = @@ -398,8 +434,10 @@ controller_interface::return_type AdmittanceController::update_and_write_command // get all controller inputs read_state_from_hardware(joint_state_, ft_values_); + auto offsetted_ft_values = add_wrenches(ft_values_, input_wrench_command_.readFromRT()->wrench); + // apply admittance control to reference to determine desired state - admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_); + admittance_->update(joint_state_, offsetted_ft_values, reference_, period, reference_admittance_); // write calculated values to joint interfaces write_state_to_hardware(reference_admittance_); diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 974bca880f..c7149faf03 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -7,6 +7,7 @@ This list summarizes the changes between Iron (previous) and Jazzy (current) rel admittance_controller ************************ * Remove ``robot_description`` parameter from parameter YAML, because it is not used at all (`#963 `_). +* Added ``~/wrench_reference`` input topic which allows to provide a force-torque offset as WrenchStamped (`#1249 `_). diff_drive_controller ***************************** From de42962cdab61c110cf913846fde087c119e76de Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 9 Dec 2024 11:57:24 +0100 Subject: [PATCH 95/97] Add missing plugins to ros2_controllers dependencies (#1413) --- ros2_controllers/package.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index ec612a3139..294d851e40 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -25,11 +25,14 @@ force_torque_sensor_broadcaster forward_command_controller gpio_controllers + gripper_controllers imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller mecanum_drive_controller + parallel_gripper_controller pid_controller + pose_broadcaster position_controllers range_sensor_broadcaster steering_controllers_library From 330b9e4e4ea18b423ff1cc0f8c8f7f0724ee33f4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 9 Dec 2024 13:04:12 +0100 Subject: [PATCH 96/97] Add sai (#1416) --- .github/mergify.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/mergify.yml b/.github/mergify.yml index c2eda7385b..84ad380e90 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -23,7 +23,7 @@ pull_request_rules: - author=mergify[bot] actions: comment: - message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich? + message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich @saikishor? - name: development targets master branch conditions: @@ -31,6 +31,7 @@ pull_request_rules: - author!=bmagyar - author!=destogl - author!=christophfroehlich + - author!=saikishor - author!=mergify[bot] - author!=dependabot[bot] actions: From 66d160c6ae2b596dd3749e658cb01e2ac00d35a7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 9 Dec 2024 17:15:51 +0100 Subject: [PATCH 97/97] [CI] Add clang job and setup concurrency (#1407) --- .../workflows/humble-abi-compatibility.yml | 5 +++ .github/workflows/humble-binary-build.yml | 5 +++ .github/workflows/humble-check-docs.yml | 4 +++ .github/workflows/humble-coverage-build.yml | 5 +++ .github/workflows/humble-debian-build.yml | 4 +++ .github/workflows/humble-pre-commit.yml | 4 +++ .../humble-rhel-semi-binary-build.yml | 5 +++ .../workflows/humble-semi-binary-build.yml | 22 +++++++++--- .github/workflows/jazzy-abi-compatibility.yml | 5 +++ .github/workflows/jazzy-binary-build.yml | 5 +++ .github/workflows/jazzy-check-docs.yml | 4 +++ .github/workflows/jazzy-coverage-build.yml | 5 +++ .github/workflows/jazzy-debian-build.yml | 4 +++ .github/workflows/jazzy-pre-commit.yml | 4 +++ .../jazzy-rhel-semi-binary-build.yml | 4 +++ .github/workflows/jazzy-semi-binary-build.yml | 22 +++++++++--- .../workflows/rolling-abi-compatibility.yml | 5 +++ .github/workflows/rolling-binary-build.yml | 5 +++ .github/workflows/rolling-check-docs.yml | 4 +++ ...ling-compatibility-humble-binary-build.yml | 14 ++++---- ...lling-compatibility-jazzy-binary-build.yml | 14 ++++---- .github/workflows/rolling-coverage-build.yml | 5 +++ .github/workflows/rolling-debian-build.yml | 4 +++ .github/workflows/rolling-pre-commit.yml | 4 +++ .../rolling-rhel-semi-binary-build.yml | 4 +++ .../workflows/rolling-semi-binary-build.yml | 22 +++++++++--- .github/workflows/rosdoc2.yml | 3 ++ .../admittance_controller/admittance_rule.hpp | 9 ++--- .../admittance_rule_impl.hpp | 35 +++++++++++-------- .../src/admittance_controller.cpp | 4 +-- .../src/diff_drive_controller.cpp | 2 +- .../test_load_forward_command_controller.cpp | 2 +- .../test/test_joint_state_broadcaster.cpp | 4 +-- .../tolerances.hpp | 4 +-- .../trajectory.hpp | 4 +-- .../src/joint_trajectory_controller.cpp | 6 ++-- .../src/trajectory.cpp | 4 +-- .../test/test_trajectory_controller_utils.hpp | 4 ++- pid_controller/src/pid_controller.cpp | 2 +- ros2_controllers.humble.repos | 2 +- .../src/steering_controllers_library.cpp | 3 +- .../src/steering_odometry.cpp | 5 ++- .../src/tricycle_controller.cpp | 2 +- 43 files changed, 214 insertions(+), 69 deletions(-) diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 247371ba7d..6cfd2c6068 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -15,6 +15,11 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers-not-released.humble.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: abi_check: runs-on: ubuntu-latest diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml index a031d823cf..f013f18880 100644 --- a/.github/workflows/humble-binary-build.yml +++ b/.github/workflows/humble-binary-build.yml @@ -34,6 +34,11 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master diff --git a/.github/workflows/humble-check-docs.yml b/.github/workflows/humble-check-docs.yml index 0eeeaedf40..448ff4084b 100644 --- a/.github/workflows/humble-check-docs.yml +++ b/.github/workflows/humble-check-docs.yml @@ -10,6 +10,10 @@ on: - '**.md' - '**.yaml' +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: check-docs: name: Check Docs diff --git a/.github/workflows/humble-coverage-build.yml b/.github/workflows/humble-coverage-build.yml index d0dcb9c350..40d1de7052 100644 --- a/.github/workflows/humble-coverage-build.yml +++ b/.github/workflows/humble-coverage-build.yml @@ -30,6 +30,11 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: coverage_humble: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index cd25248caf..e52ffe6842 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -18,6 +18,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: humble_debian: diff --git a/.github/workflows/humble-pre-commit.yml b/.github/workflows/humble-pre-commit.yml index 5bb2408578..38a76ee025 100644 --- a/.github/workflows/humble-pre-commit.yml +++ b/.github/workflows/humble-pre-commit.yml @@ -6,6 +6,10 @@ on: branches: - humble +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: pre-commit: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master diff --git a/.github/workflows/humble-rhel-semi-binary-build.yml b/.github/workflows/humble-rhel-semi-binary-build.yml index 25c755ada5..6019d08e46 100644 --- a/.github/workflows/humble-rhel-semi-binary-build.yml +++ b/.github/workflows/humble-rhel-semi-binary-build.yml @@ -18,6 +18,11 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: humble_rhel_binary: name: Humble RHEL binary build diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index c53c46032a..2d0437782b 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -33,15 +33,27 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on humble branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: semi_binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_REPO: [testing] with: ros_distro: humble - ros_repo: ${{ matrix.ROS_REPO }} + ros_repo: testing upstream_workspace: ros2_controllers.humble.repos ref_for_scheduled_build: humble + semi_binary_clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: humble + ros_repo: testing + upstream_workspace: ros2_controllers-not-released.humble.repos + ref_for_scheduled_build: humble + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/jazzy-abi-compatibility.yml b/.github/workflows/jazzy-abi-compatibility.yml index 8e557b5ee3..5da5fb9d00 100644 --- a/.github/workflows/jazzy-abi-compatibility.yml +++ b/.github/workflows/jazzy-abi-compatibility.yml @@ -14,6 +14,11 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers-not-released.jazzy.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: abi_check: runs-on: ubuntu-latest diff --git a/.github/workflows/jazzy-binary-build.yml b/.github/workflows/jazzy-binary-build.yml index cda4969abf..7dd294a55b 100644 --- a/.github/workflows/jazzy-binary-build.yml +++ b/.github/workflows/jazzy-binary-build.yml @@ -35,6 +35,11 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master diff --git a/.github/workflows/jazzy-check-docs.yml b/.github/workflows/jazzy-check-docs.yml index 2f9eaf18bd..cfc67a3eac 100644 --- a/.github/workflows/jazzy-check-docs.yml +++ b/.github/workflows/jazzy-check-docs.yml @@ -10,6 +10,10 @@ on: - '**.md' - '**.yaml' +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: check-docs: name: Check Docs diff --git a/.github/workflows/jazzy-coverage-build.yml b/.github/workflows/jazzy-coverage-build.yml index c96ac88c32..2f587c7003 100644 --- a/.github/workflows/jazzy-coverage-build.yml +++ b/.github/workflows/jazzy-coverage-build.yml @@ -33,6 +33,11 @@ on: # - '**/CMakeLists.txt' # - 'ros2_controllers.jazzy.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: coverage_jazzy: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master diff --git a/.github/workflows/jazzy-debian-build.yml b/.github/workflows/jazzy-debian-build.yml index e3e3b8a353..b1a60f1528 100644 --- a/.github/workflows/jazzy-debian-build.yml +++ b/.github/workflows/jazzy-debian-build.yml @@ -18,6 +18,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: jazzy_debian: diff --git a/.github/workflows/jazzy-pre-commit.yml b/.github/workflows/jazzy-pre-commit.yml index d9ec610bbc..aab5ba52ac 100644 --- a/.github/workflows/jazzy-pre-commit.yml +++ b/.github/workflows/jazzy-pre-commit.yml @@ -6,6 +6,10 @@ on: branches: - master +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: pre-commit: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master diff --git a/.github/workflows/jazzy-rhel-semi-binary-build.yml b/.github/workflows/jazzy-rhel-semi-binary-build.yml index 1c62fcf2ac..f39c9cc570 100644 --- a/.github/workflows/jazzy-rhel-semi-binary-build.yml +++ b/.github/workflows/jazzy-rhel-semi-binary-build.yml @@ -18,6 +18,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: jazzy_rhel: diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index a462d9040a..e38fd5b7ba 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -35,15 +35,27 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: semi_binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_REPO: [testing] with: ros_distro: jazzy - ros_repo: ${{ matrix.ROS_REPO }} + ros_repo: testing upstream_workspace: ros2_controllers.jazzy.repos ref_for_scheduled_build: master + semi_binary_clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: jazzy + ros_repo: testing + upstream_workspace: ros2_controllers.jazzy.repos + ref_for_scheduled_build: jazzy + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index da3337554c..1f0ec61d40 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -14,6 +14,11 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers-not-released.rolling.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: abi_check: runs-on: ubuntu-latest diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml index b512eb9db7..a3686144f3 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build.yml @@ -36,6 +36,11 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master diff --git a/.github/workflows/rolling-check-docs.yml b/.github/workflows/rolling-check-docs.yml index 15b7eff57e..1144c4e6c0 100644 --- a/.github/workflows/rolling-check-docs.yml +++ b/.github/workflows/rolling-check-docs.yml @@ -11,6 +11,10 @@ on: - '**.yaml' - '.github/workflows/rolling-check-docs.yml' +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: check-docs: name: Check Docs diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml index 7aab02fa62..6370db30d1 100644 --- a/.github/workflows/rolling-compatibility-humble-binary-build.yml +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -33,16 +33,16 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: build-on-humble: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [humble] - ROS_REPO: [testing] with: - ros_distro: ${{ matrix.ROS_DISTRO }} - ros_repo: ${{ matrix.ROS_REPO }} + ros_distro: humble + ros_repo: testing upstream_workspace: ros2_controllers.rolling.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml index 37ffbb84a4..4da98f2d09 100644 --- a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -33,16 +33,16 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: build-on-jazzy: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [jazzy] - ROS_REPO: [testing] with: - ros_distro: ${{ matrix.ROS_DISTRO }} - ros_repo: ${{ matrix.ROS_REPO }} + ros_distro: jazzy + ros_repo: testing upstream_workspace: ros2_controllers.rolling.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml index 058ff9bb33..abea8a90db 100644 --- a/.github/workflows/rolling-coverage-build.yml +++ b/.github/workflows/rolling-coverage-build.yml @@ -30,6 +30,11 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: coverage_rolling: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index d7efd3ea0a..8f9394762b 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -18,6 +18,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: rolling_debian: diff --git a/.github/workflows/rolling-pre-commit.yml b/.github/workflows/rolling-pre-commit.yml index 7bc07ba802..792278d6d2 100644 --- a/.github/workflows/rolling-pre-commit.yml +++ b/.github/workflows/rolling-pre-commit.yml @@ -6,6 +6,10 @@ on: branches: - master +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + jobs: pre-commit: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master diff --git a/.github/workflows/rolling-rhel-semi-binary-build.yml b/.github/workflows/rolling-rhel-semi-binary-build.yml index 29a53b810a..0f03ad02c2 100644 --- a/.github/workflows/rolling-rhel-semi-binary-build.yml +++ b/.github/workflows/rolling-rhel-semi-binary-build.yml @@ -18,6 +18,10 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} jobs: rolling_rhel: diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index db0dd1fe64..06f4c55612 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -35,15 +35,27 @@ on: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' +concurrency: + # cancel previous runs of the same workflow, except for pushes on master branch + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + jobs: semi_binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_REPO: [testing] with: ros_distro: rolling - ros_repo: ${{ matrix.ROS_REPO }} + ros_repo: testing upstream_workspace: ros2_controllers.rolling.repos ref_for_scheduled_build: master + semi_binary_clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: rolling + ros_repo: testing + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: rolling + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/rosdoc2.yml b/.github/workflows/rosdoc2.yml index 0771248e79..434344fcaa 100644 --- a/.github/workflows/rosdoc2.yml +++ b/.github/workflows/rosdoc2.yml @@ -8,6 +8,9 @@ on: - ros2_controllers/rosdoc2.yaml - ros2_controllers/package.xml +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true jobs: check: diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index a326b663d0..bfb3a612c7 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -63,10 +63,11 @@ struct AdmittanceState 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); + auto idx = static_cast(num_joints); + current_joint_pos = Eigen::VectorXd::Zero(idx); + joint_pos = Eigen::VectorXd::Zero(idx); + joint_vel = Eigen::VectorXd::Zero(idx); + joint_acc = Eigen::VectorXd::Zero(idx); } Eigen::VectorXd current_joint_pos; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 13d4e67fbc..859c7e3336 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -136,9 +136,11 @@ void AdmittanceRule::apply_parameters_update() for (size_t i = 0; i < NUM_CARTESIAN_DOF; ++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]); + auto idx = static_cast(i); + admittance_state_.mass_inv[idx] = 1.0 / parameters_.admittance.mass[i]; + admittance_state_.damping[idx] = + parameters_.admittance.damping_ratio[i] * 2 * + sqrt(admittance_state_.mass[idx] * admittance_state_.stiffness[idx]); } } @@ -216,12 +218,13 @@ controller_interface::return_type AdmittanceRule::update( // update joint desired joint state for (size_t i = 0; i < num_joints_; ++i) { + auto idx = static_cast(i); desired_joint_state.positions[i] = - reference_joint_state.positions[i] + admittance_state_.joint_pos[i]; + reference_joint_state.positions[i] + admittance_state_.joint_pos[idx]; desired_joint_state.velocities[i] = - reference_joint_state.velocities[i] + admittance_state_.joint_vel[i]; + reference_joint_state.velocities[i] + admittance_state_.joint_vel[idx]; desired_joint_state.accelerations[i] = - reference_joint_state.accelerations[i] + admittance_state_.joint_acc[i]; + reference_joint_state.accelerations[i] + admittance_state_.joint_acc[idx]; } return controller_interface::return_type::OK; @@ -334,7 +337,7 @@ void AdmittanceRule::process_wrench_measurements( 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) + for (Eigen::Index i = 0; i < 6; ++i) { wrench_world_(i) = filters::exponentialSmoothing( new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient); @@ -345,18 +348,20 @@ const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_control { for (size_t i = 0; i < NUM_CARTESIAN_DOF; ++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]; + auto idx = static_cast(i); + state_message_.stiffness.data[i] = admittance_state_.stiffness[idx]; + state_message_.damping.data[i] = admittance_state_.damping[idx]; + state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[idx]); + state_message_.mass.data[i] = admittance_state_.mass[idx]; } for (size_t i = 0; i < parameters_.joints.size(); ++i) { + auto idx = static_cast(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_.joint_state.position[i] = admittance_state_.joint_pos[idx]; + state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[idx]; + state_message_.joint_state.effort[i] = admittance_state_.joint_acc[idx]; } state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0]; @@ -411,7 +416,7 @@ void AdmittanceRule::vec_to_eigen(const std::vector & data, T2 & matrix) { for (auto row = 0; row < matrix.rows(); row++) { - matrix(row, col) = data[row + col * matrix.rows()]; + matrix(row, col) = data[static_cast(row + col * matrix.rows())]; } } } diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 58a5bcd6e2..468c02624b 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -340,7 +340,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( state_interfaces_, admittance_->parameters_.joints, interface, joint_state_interface_[index])) @@ -355,7 +355,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index d710c81257..a93d3d3812 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -292,7 +292,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( 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(params_.velocity_rolling_window_size); + odometry_.setVelocityRollingWindowSize(static_cast(params_.velocity_rolling_window_size)); cmd_vel_timeout_ = std::chrono::milliseconds{static_cast(params_.cmd_vel_timeout * 1000.0)}; publish_limited_velocity_ = params_.publish_limited_velocity; diff --git a/forward_command_controller/test/test_load_forward_command_controller.cpp b/forward_command_controller/test/test_load_forward_command_controller.cpp index c192f1eb5f..ad2e53162a 100644 --- a/forward_command_controller/test/test_load_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_forward_command_controller.cpp @@ -30,7 +30,7 @@ TEST(TestLoadForwardCommandController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index de534c00b6..3ec7ff4ac2 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -1091,10 +1091,10 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( ASSERT_THAT( dynamic_joint_state_msg->interface_values[i].interface_names, ElementsAreArray(INTERFACE_NAMES)); - const auto index_in_original_order = std::distance( + const auto index_in_original_order = static_cast(std::distance( joint_names_.cbegin(), std::find( - joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg->joint_names[i])); + joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg->joint_names[i]))); ASSERT_THAT( dynamic_joint_state_msg->interface_values[i].values, Each(joint_values_[index_in_original_order])); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 4902fd1dcc..b33a2bf5c5 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -199,7 +199,7 @@ SegmentTolerances get_segment_tolerances( .c_str()); return default_tolerances; } - auto i = std::distance(joints.cbegin(), it); + auto i = static_cast(std::distance(joints.cbegin(), it)); std::string interface = ""; try { @@ -246,7 +246,7 @@ SegmentTolerances get_segment_tolerances( .c_str()); return default_tolerances; } - auto i = std::distance(joints.cbegin(), it); + auto i = static_cast(std::distance(joints.cbegin(), it)); std::string interface = ""; try { diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index d650c369d0..74d4e28f3a 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -206,8 +206,8 @@ inline std::vector mapping(const T & t1, const T & t2) } else { - const size_t t1_dist = std::distance(t1.begin(), t1_it); - const size_t t2_dist = std::distance(t2.begin(), t2_it); + const size_t t1_dist = static_cast(std::distance(t1.begin(), t1_it)); + const size_t t2_dist = static_cast(std::distance(t2.begin(), t2_it)); mapping_vector[t1_dist] = t2_dist; } } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 82d9c94aab..365ce993d0 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -385,7 +385,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN(logger, error_string.c_str()); + RCLCPP_WARN(logger, "%s", error_string.c_str()); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -907,7 +907,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { @@ -921,7 +921,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( state_interfaces_, params_.joints, interface, joint_state_interface_[index])) { diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 1c50d26c62..57366f88a8 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -175,8 +175,8 @@ bool Trajectory::sample( interpolate_between_points(t0, point, t1, next_point, sample_time, output_state); } - start_segment_itr = begin() + i; - end_segment_itr = begin() + (i + 1); + start_segment_itr = begin() + static_cast(i); + end_segment_itr = begin() + static_cast(i + 1); if (search_monotonically_increasing) { last_sample_idx_ = i; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 7dff81f08f..b0e66394d1 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -467,7 +467,9 @@ class TrajectoryControllerTest : public ::testing::Test if (joint_names.empty()) { traj_msg.joint_names = { - joint_names_.begin(), joint_names_.begin() + points_positions[0].size()}; + joint_names_.begin(), + joint_names_.begin() + + static_cast::difference_type>(points_positions[0].size())}; } else { diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 032dc1d666..f3b2ba33a0 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -320,7 +320,7 @@ void PidController::reference_callback(const std::shared_ptrdof_names.begin(), found_it); + auto position = static_cast(std::distance(ref_msg->dof_names.begin(), found_it)); ref_msg->values[position] = msg->values[i]; ref_msg->values_dot[position] = msg->values_dot[i]; } diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index a0432f65b3..548c95b44a 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -6,7 +6,7 @@ repositories: realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git - version: master + version: humble kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 836574f150..3c14013f40 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -81,7 +81,8 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { params_ = param_listener_->get_params(); - odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + odometry_.set_velocity_rolling_window_size( + static_cast(params_.velocity_rolling_window_size)); configure_odometry(); diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index dbe210ed41..8b8bb8bfd8 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -204,7 +204,10 @@ void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_ reset_accumulators(); } -void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ = type; } +void SteeringOdometry::set_odometry_type(const unsigned int type) +{ + config_type_ = static_cast(type); +} double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) { diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 1d6daba958..bc04451ad6 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -236,7 +236,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } odometry_.setWheelParams(params_.wheelbase, params_.wheel_radius); - odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); + odometry_.setVelocityRollingWindowSize(static_cast(params_.velocity_rolling_window_size)); cmd_vel_timeout_ = std::chrono::milliseconds{params_.cmd_vel_timeout}; params_.publish_ackermann_command =