From be8d4eeaa650fcb95a883bd813f27ce099e03122 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 9 Feb 2022 09:43:31 +0100 Subject: [PATCH 1/5] Move test nodes from the ros2_control_demos repository. --- ros2_controllers_test_nodes/package.xml | 21 +++ .../resource/ros2_controllers_test_nodes | 0 .../ros2_controllers_test_nodes/__init__.py | 13 ++ .../publisher_forward_position_controller.py | 81 ++++++++++ .../publisher_joint_trajectory_controller.py | 152 ++++++++++++++++++ ros2_controllers_test_nodes/setup.cfg | 4 + ros2_controllers_test_nodes/setup.py | 57 +++++++ 7 files changed, 328 insertions(+) create mode 100644 ros2_controllers_test_nodes/package.xml create mode 100644 ros2_controllers_test_nodes/resource/ros2_controllers_test_nodes create mode 100644 ros2_controllers_test_nodes/ros2_controllers_test_nodes/__init__.py create mode 100644 ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py create mode 100644 ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py create mode 100644 ros2_controllers_test_nodes/setup.cfg create mode 100644 ros2_controllers_test_nodes/setup.py diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml new file mode 100644 index 0000000000..e35c3022c6 --- /dev/null +++ b/ros2_controllers_test_nodes/package.xml @@ -0,0 +1,21 @@ + + + + ros2_controllers_test_nodes + 0.0.0 + Demo nodes for showing and testing functionalities of the ros2_control framework. + + Denis Štogl + + Apache-2.0 + + rclpy + std_msgs + trajectory_msgs + + python3-pytest + + + ament_python + + diff --git a/ros2_controllers_test_nodes/resource/ros2_controllers_test_nodes b/ros2_controllers_test_nodes/resource/ros2_controllers_test_nodes new file mode 100644 index 0000000000..e69de29bb2 diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/__init__.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/__init__.py new file mode 100644 index 0000000000..019e363a25 --- /dev/null +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/__init__.py @@ -0,0 +1,13 @@ +# Copyright 2022 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. 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 new file mode 100644 index 0000000000..8c9afcd963 --- /dev/null +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py @@ -0,0 +1,81 @@ +# Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Author: Denis Štogl +# + +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Float64MultiArray + + +class PublisherForwardPosition(Node): + def __init__(self): + super().__init__("publisher_forward_position_controller") + # Declare all parameters + self.declare_parameter("controller_name", "forward_position_controller") + self.declare_parameter("wait_sec_between_publish", 5) + self.declare_parameter("goal_names", ["pos1", "pos2"]) + + # Read parameters + controller_name = self.get_parameter("controller_name").value + wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value + goal_names = self.get_parameter("goal_names").value + + # Read all positions from parameters + self.goals = [] + for name in goal_names: + self.declare_parameter(name) + goal = self.get_parameter(name).value + if goal is None or len(goal) == 0: + raise Exception(f'Values for goal "{name}" not set!') + + float_goal = [] + for value in goal: + float_goal.append(float(value)) + self.goals.append(float_goal) + + publish_topic = "/" + controller_name + "/" + "commands" + + self.get_logger().info( + f'Publishing {len(goal_names)} goals on topic "{publish_topic}"\ + every {wait_sec_between_publish} s' + ) + + self.publisher_ = self.create_publisher(Float64MultiArray, publish_topic, 1) + self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback) + self.i = 0 + + def timer_callback(self): + msg = Float64MultiArray() + msg.data = self.goals[self.i] + self.get_logger().info(f'Publishing: "{msg.data}"') + self.publisher_.publish(msg) + self.i += 1 + self.i %= len(self.goals) + + +def main(args=None): + rclpy.init(args=args) + + publisher_forward_position = PublisherForwardPosition() + + rclpy.spin(publisher_forward_position) + publisher_forward_position.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + 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 new file mode 100644 index 0000000000..a7097080e9 --- /dev/null +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py @@ -0,0 +1,152 @@ +# Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Author: Denis Štogl +# + +import rclpy +from rclpy.node import Node +from builtin_interfaces.msg import Duration + +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from sensor_msgs.msg import JointState + + +class PublisherJointTrajectory(Node): + def __init__(self): + super().__init__("publisher_position_trajectory_controller") + # Declare all parameters + self.declare_parameter("controller_name", "position_trajectory_controller") + self.declare_parameter("wait_sec_between_publish", 6) + self.declare_parameter("goal_names", ["pos1", "pos2"]) + self.declare_parameter("joints") + self.declare_parameter("check_starting_point", False) + self.declare_parameter("starting_point_limits") + + # Read parameters + controller_name = self.get_parameter("controller_name").value + wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value + goal_names = self.get_parameter("goal_names").value + self.joints = self.get_parameter("joints").value + self.check_starting_point = self.get_parameter("check_starting_point").value + self.starting_point = {} + + if self.joints is None or len(self.joints) == 0: + raise Exception('"joints" parameter is not set!') + + # starting point stuff + if self.check_starting_point: + # declare nested params + for name in self.joints: + param_name_tmp = "starting_point_limits" + "." + name + self.declare_parameter(param_name_tmp, [-2 * 3.14159, 2 * 3.14159]) + self.starting_point[name] = self.get_parameter(param_name_tmp).value + + for name in self.joints: + if len(self.starting_point[name]) != 2: + raise Exception('"starting_point" parameter is not set correctly!') + self.joint_state_sub = self.create_subscription( + JointState, "joint_states", self.joint_state_callback, 10 + ) + # initialize starting point status + if not self.check_starting_point: + self.starting_point_ok = True + else: + self.starting_point_ok = False + + self.joint_state_msg_received = False + + # Read all positions from parameters + self.goals = [] + for name in goal_names: + self.declare_parameter(name) + goal = self.get_parameter(name).value + if goal is None or len(goal) == 0: + raise Exception(f'Values for goal "{name}" not set!') + + float_goal = [] + for value in goal: + float_goal.append(float(value)) + self.goals.append(float_goal) + + publish_topic = "/" + controller_name + "/" + "joint_trajectory" + + self.get_logger().info( + 'Publishing {} goals on topic "{}" every {} s'.format( + len(goal_names), publish_topic, wait_sec_between_publish + ) + ) + + self.publisher_ = self.create_publisher(JointTrajectory, publish_topic, 1) + self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback) + self.i = 0 + + def timer_callback(self): + + if self.starting_point_ok: + + traj = JointTrajectory() + traj.joint_names = self.joints + point = JointTrajectoryPoint() + point.positions = self.goals[self.i] + point.time_from_start = Duration(sec=4) + + traj.points.append(point) + self.publisher_.publish(traj) + + self.i += 1 + self.i %= len(self.goals) + + elif self.check_starting_point and not self.joint_state_msg_received: + self.get_logger().warn( + 'Start configuration could not be checked! Check "joint_state" topic!' + ) + else: + self.get_logger().warn("Start configuration is not within configured limits!") + + def joint_state_callback(self, msg): + + if not self.joint_state_msg_received: + + # check start state + limit_exceeded = [False] * len(msg.name) + for idx, enum in enumerate(msg.name): + if (msg.position[idx] < self.starting_point[enum][0]) or ( + msg.position[idx] > self.starting_point[enum][1] + ): + self.get_logger().warn(f"Starting point limits exceeded for joint {enum} !") + limit_exceeded[idx] = True + + if any(limit_exceeded): + self.starting_point_ok = False + else: + self.starting_point_ok = True + + self.joint_state_msg_received = True + else: + return + + +def main(args=None): + rclpy.init(args=args) + + publisher_joint_trajectory = PublisherJointTrajectory() + + rclpy.spin(publisher_joint_trajectory) + publisher_joint_trajectory.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/ros2_controllers_test_nodes/setup.cfg b/ros2_controllers_test_nodes/setup.cfg new file mode 100644 index 0000000000..c00c975bcf --- /dev/null +++ b/ros2_controllers_test_nodes/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/ros2_controllers_test_nodes +[install] +install-scripts=$base/lib/ros2_controllers_test_nodes diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py new file mode 100644 index 0000000000..061e7aef38 --- /dev/null +++ b/ros2_controllers_test_nodes/setup.py @@ -0,0 +1,57 @@ +# Copyright 2022 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. + +from glob import glob + +from setuptools import setup + +package_name = "ros2_controllers_test_nodes" + +setup( + name=package_name, + version="0.0.1", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name, glob("launch/*.launch.py")), + ("share/" + package_name + "/configs", glob("configs/*.*")), + ], + install_requires=["setuptools"], + zip_safe=True, + author="Denis Štogl", + author_email="denis@stogl.de", + maintainer="Denis Štogl", + maintainer_email="denis@stogl.de", + keywords=["ROS"], + classifiers=[ + "Intended Audience :: Developers", + "License :: OSI Approved :: Apache Software License", + "Programming Language :: Python", + "Topic :: Software Development", + ], + description="Demo nodes for showing and testing functionalities of ros2_control framework.", + long_description="""\ +Demo nodes for showing and testing functionalities of the ros2_control framework.""", + license="Apache License, Version 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "publisher_forward_position_controller = \ + ros2_controllers_test_nodes.publisher_forward_position_controller:main", + "publisher_joint_trajectory_controller = \ + ros2_controllers_test_nodes.publisher_joint_trajectory_controller:main", + ], + }, +) From a9b3df7f9b2f90aa6f312b5bb35072e2751ca915 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 22 Feb 2022 22:36:25 +0100 Subject: [PATCH 2/5] Apply suggestions from code review Co-authored-by: Bence Magyar --- ros2_controllers_test_nodes/package.xml | 1 + .../publisher_forward_position_controller.py | 4 +--- .../publisher_joint_trajectory_controller.py | 5 +---- 3 files changed, 3 insertions(+), 7 deletions(-) diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index e35c3022c6..65ceb874e8 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -6,6 +6,7 @@ Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl + Bence Magyar Apache-2.0 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 8c9afcd963..5969e0b59f 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 @@ -42,9 +42,7 @@ def __init__(self): if goal is None or len(goal) == 0: raise Exception(f'Values for goal "{name}" not set!') - float_goal = [] - for value in goal: - float_goal.append(float(value)) + float_goal = [float(value) for value in goal] self.goals.append(float_goal) publish_topic = "/" + controller_name + "/" + "commands" 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 a7097080e9..36af3199d1 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 @@ -60,10 +60,7 @@ def __init__(self): JointState, "joint_states", self.joint_state_callback, 10 ) # initialize starting point status - if not self.check_starting_point: - self.starting_point_ok = True - else: - self.starting_point_ok = False + self.starting_point_ok = not self.check_starting_point self.joint_state_msg_received = False From 7c632a586542567e7ae1dd39469f9cd43162bc63 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 22 Feb 2022 22:42:26 +0100 Subject: [PATCH 3/5] Add Lovro as co-author Co-authored-by: livanov93 --- .../publisher_forward_position_controller.py | 2 +- .../publisher_joint_trajectory_controller.py | 2 +- 2 files changed, 2 insertions(+), 2 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 5969e0b59f..60ddd75854 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 @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# Author: Denis Štogl +# Authors: Denis Štogl, Lovro Ivanov # import rclpy 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 36af3199d1..77e04da356 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 @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# Author: Denis Štogl +# Authors: Denis Štogl, Lovro Ivanov # import rclpy From 50d11118abbffe523b35a337bd8887e06ca7ac69 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 22 Feb 2022 22:43:43 +0100 Subject: [PATCH 4/5] Update ci.yml --- .github/workflows/ci.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index bc70ce8262..8098874342 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -48,6 +48,7 @@ jobs: joint_trajectory_controller gripper_controllers position_controllers + ros2_controllers_test_nodes velocity_controllers vcs-repo-file-url: | https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/.github/workspace.repos From a5b5a48efde24694fe0483be2231d079add72a34 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 22 Feb 2022 22:44:03 +0100 Subject: [PATCH 5/5] Update lint.yml --- .github/workflows/lint.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 5ac782b9b5..8eec08251f 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -27,6 +27,7 @@ jobs: gripper_controllers position_controllers ros2_controllers + ros2_controllers_test_nodes velocity_controllers ament_lint_cpplint: @@ -52,4 +53,5 @@ jobs: gripper_controllers position_controllers ros2_controllers + ros2_controllers_test_nodes velocity_controllers