From a7a45df4f890bc7945453608fd516853f50711f4 Mon Sep 17 00:00:00 2001 From: xfiderek Date: Fri, 6 Sep 2024 13:57:00 +0200 Subject: [PATCH] NASA_Challenge_[@xfiderek] Add canadarm_wrench_publisher package (#42). canadarm_wrench_publisher package demonstrates how to use trick bridge into any ros2 node. It is also used in the end-to-end demo. --- .../canadarm_wrench_publisher/__init__.py | 13 ++ .../canadarm_wrench_publisher_node.py | 117 ++++++++++++++++++ .../canadarm_wrench_publisher/package.xml | 18 +++ .../resource/canadarm_wrench_publisher | 14 +++ .../canadarm_wrench_publisher/setup.cfg | 4 + .../canadarm_wrench_publisher/setup.py | 25 ++++ 6 files changed, 191 insertions(+) create mode 100644 ros_trick/ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/__init__.py create mode 100644 ros_trick/ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/canadarm_wrench_publisher_node.py create mode 100644 ros_trick/ros_src/canadarm_wrench_publisher/package.xml create mode 100644 ros_trick/ros_src/canadarm_wrench_publisher/resource/canadarm_wrench_publisher create mode 100644 ros_trick/ros_src/canadarm_wrench_publisher/setup.cfg create mode 100644 ros_trick/ros_src/canadarm_wrench_publisher/setup.py diff --git a/ros_trick/ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/__init__.py b/ros_trick/ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/__init__.py new file mode 100644 index 00000000..be353d1a --- /dev/null +++ b/ros_trick/ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/__init__.py @@ -0,0 +1,13 @@ +# Copyright 2024 Blazej Fiderek (xfiderek) +# +# 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 +# +# https://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/ros_trick/ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/canadarm_wrench_publisher_node.py b/ros_trick/ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/canadarm_wrench_publisher_node.py new file mode 100644 index 00000000..ba38587d --- /dev/null +++ b/ros_trick/ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/canadarm_wrench_publisher_node.py @@ -0,0 +1,117 @@ +# Copyright 2024 Blazej Fiderek (xfiderek) +# +# 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 +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +import time + +import rclpy +from geometry_msgs.msg import WrenchStamped +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from ros_trick_bridge.trick_variable_server_client import TrickVariableServerClient +from std_msgs.msg import Header + + +class WrenchPublisher(Node): + def __init__(self): + super().__init__("wrench_publisher") + self._trick_host = "localhost" + self._trick_port = 49765 + self._ndof = 7 + self._trick_variable_names = [ + item + for i in range(self._ndof) + for item in [ + f"CanadarmManip.manip.applied_torque[{i}]", + f"CanadarmManip.manip.friction_torque[{i}]", + ] + ] + + # let everything start + time.sleep(5.0) + + self.get_logger().info("starting trick variable server") + self._trick_variable_server_client = TrickVariableServerClient( + host=self._trick_host, + port=self._trick_port, + client_tag="wrench_pub", + trick_variable_names=self._trick_variable_names, + trick_var_cycle_time=0.1, + on_receive_callback=self.trick_data_callback, + ) + self.link_frames = ["B1", "B2", "B3", "B4", "B5", "B6", "EE_SSRMS"] + self.rot_axes = [ + [0.0, 0.0, 1.0], + [1.0, 0.0, 0.0], + [0.0, 0.0, 1.0], + [0.0, 0.0, 1.0], + [0.0, 0.0, -1.0], + [1.0, 0.0, 0.0], + [0.0, 0.0, 1.0], + ] + self.get_logger().info("spawning wrench publishers") + self.wrench_publishers = [ + publisher + for link_name in self.link_frames + for publisher in [ + self.create_publisher( + WrenchStamped, f"wrench_topic/{link_name}/total_torque", 10 + ), + self.create_publisher( + WrenchStamped, f"wrench_topic/{link_name}/friction_torque", 10 + ), + ] + ] + + self._trick_variable_server_client.start_listening() + + def trick_data_callback(self, trick_data): + # self.get_logger().info("received trick cb") + + for i in range(self._ndof): + total_torque_var_name = self._trick_variable_names[2 * i] + friction_torque_var_name = self._trick_variable_names[2 * i + 1] + total_torque_val = float(trick_data[total_torque_var_name]) + friction_torque_val = float(trick_data[friction_torque_var_name]) + total_torque_pub = self.wrench_publishers[2 * i] + friction_torque_pub = self.wrench_publishers[2 * i + 1] + now = self.get_clock().now().to_msg() + + msg = WrenchStamped() + msg.header = Header() + msg.header.frame_id = self.link_frames[i] + msg.header.stamp = now + + # send total torque + msg.wrench.torque.x = self.rot_axes[i][0] * total_torque_val + msg.wrench.torque.y = self.rot_axes[i][1] * total_torque_val + msg.wrench.torque.z = self.rot_axes[i][2] * total_torque_val + total_torque_pub.publish(msg) + + # send friction torque + msg.wrench.torque.x = self.rot_axes[i][0] * friction_torque_val + msg.wrench.torque.y = self.rot_axes[i][1] * friction_torque_val + msg.wrench.torque.z = self.rot_axes[i][2] * friction_torque_val + friction_torque_pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = WrenchPublisher() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/ros_trick/ros_src/canadarm_wrench_publisher/package.xml b/ros_trick/ros_src/canadarm_wrench_publisher/package.xml new file mode 100644 index 00000000..889b15db --- /dev/null +++ b/ros_trick/ros_src/canadarm_wrench_publisher/package.xml @@ -0,0 +1,18 @@ + + + + canadarm_wrench_publisher + 0.0.0 + TODO: Package description + blazej + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/ros_trick/ros_src/canadarm_wrench_publisher/resource/canadarm_wrench_publisher b/ros_trick/ros_src/canadarm_wrench_publisher/resource/canadarm_wrench_publisher new file mode 100644 index 00000000..c21c341a --- /dev/null +++ b/ros_trick/ros_src/canadarm_wrench_publisher/resource/canadarm_wrench_publisher @@ -0,0 +1,14 @@ +// Copyright 2024 Blazej Fiderek (xfiderek) +// +// 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 +// +// https://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/ros_trick/ros_src/canadarm_wrench_publisher/setup.cfg b/ros_trick/ros_src/canadarm_wrench_publisher/setup.cfg new file mode 100644 index 00000000..b76e8f59 --- /dev/null +++ b/ros_trick/ros_src/canadarm_wrench_publisher/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/canadarm_wrench_publisher +[install] +install_scripts=$base/lib/canadarm_wrench_publisher diff --git a/ros_trick/ros_src/canadarm_wrench_publisher/setup.py b/ros_trick/ros_src/canadarm_wrench_publisher/setup.py new file mode 100644 index 00000000..1917642a --- /dev/null +++ b/ros_trick/ros_src/canadarm_wrench_publisher/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = "canadarm_wrench_publisher" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Blazej Fiderek (xfiderek)", + maintainer_email="fiderekblazej@gmail.com", + description="Get joint torques from trick and publish them as GeometryWrench", + license="Apache-2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "canadarm_wrench_publisher_node = canadarm_wrench_publisher.canadarm_wrench_publisher_node:main" + ], + }, +)