diff --git a/ros_as5048b/launch/ros_as5048b.launch.py b/ros_as5048b/launch/ros_as5048b.launch.py new file mode 100644 index 0000000..7d376bb --- /dev/null +++ b/ros_as5048b/launch/ros_as5048b.launch.py @@ -0,0 +1,49 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + i2c_device_arg = DeclareLaunchArgument('i2c_device', default_value="1") + i2c_address_arg = DeclareLaunchArgument('i2c_address', default_value="0x42") + + read_encoder_node = Node( + package='ros_as5048b', + executable='read_encoder_node', + name='read_encoder_node', + parameters=[ + {'i2c_device': LaunchConfiguration('i2c_device')}, + {'i2c_address': LaunchConfiguration('i2c_address')}, + ] + ) + + + turn_count_file_arg = DeclareLaunchArgument('turn_count_file', default_value='/tmp/turn_count.yaml') + + turn_counter_node = Node( + package='ros_as5048b', + executable='turn_counter_node', + name='turn_counter_node', + parameters=[ + {'file_path': LaunchConfiguration('turn_count_file')}, + {'encoder_topic': f"/encoder/at_{LaunchConfiguration('i2c_address')}/deg"}, + ] + ) + + nodes = [ + read_encoder_node, + # turn_counter_node + ] + + return LaunchDescription([ + i2c_device_arg, + i2c_address_arg, + turn_count_file_arg, + ] + nodes) + + + diff --git a/ros_as5048b/ros_as5048b/turn_counter_node.py b/ros_as5048b/ros_as5048b/turn_counter_node.py new file mode 100644 index 0000000..d1b3df9 --- /dev/null +++ b/ros_as5048b/ros_as5048b/turn_counter_node.py @@ -0,0 +1,136 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Float32 +from std_srvs.srv import Trigger +from winch_control_interfaces.srv import SetFloat32 + +import yaml +import math + +class TurnCounterNode(Node): + + def __init__(self): + super().__init__('tendon_length_node') + + self.declare_parameter('file_path', '/tmp/tendon_length.yaml') + self.file_path = self.get_parameter('file_path').value + + self.declare_parameter('encoder_topic', '/encoder/deg') + self.encoder_topic = self.get_parameter('encoder_topic').value + + self.reset_srv = self.create_service(Trigger, '~/reset', self.reset_callback) + self.calibrate_srv = self.create_service(SetFloat32, '~/calibrate', self.calibrate_callback) + + self.subscription = self.create_subscription(Float32, self.encoder_topic, self.encoder_callback, 20) + + self.publisher = self.create_publisher(Float32, '~/tendon_length', 5) + + self.timer = self.create_timer(0.1, self.timer_callback) + self.timer_counter = 0 + + self.tendon_length = self.load_tendon_length_from_file() + self.prev_encoder_value = None + + self.publish_tendon_length() + + + def calibrate_callback(self, request, response): + self.tendon_length = request.data + response.success=True + return response + + + def reset_callback(self, request, response): + self.get_logger().info(f"Reset tendon length from {self.tendon_length}") + self.tendon_length = 0.0 + response.success=True + return response + + + def timer_callback(self): + self.publish_tendon_length() + + self.timer_counter += 1 + + if self.timer_counter == 10: + self.write_tendon_length_to_file() + self.timer_counter = 0 + + + def publish_tendon_length(self): + msg_out = Float32() + msg_out.data = self.tendon_length + self.publisher.publish(msg_out) + + + def encoder_callback(self, encoder_msg): + + encoder_value = encoder_msg.data + + if self.prev_encoder_value is not None: + encoder_movement = self.calculate_movement(encoder_value, self.prev_encoder_value) + + ### TODO: Add more sophisticated tendon length model here + + drum_diameter=0.01 + drum_circumference=math.pi*drum_diameter + + delta_tendon = drum_circumference * encoder_movement/360.0 + + self.tendon_length -= delta_tendon + + ### + + self.prev_encoder_value = encoder_value + + + def calculate_movement(self, current_value, previous_value): + movement = current_value - previous_value + + # Handle the case of a jump from 360 to 0 (or vice versa) + if movement > 180: + movement -= 360 + elif movement < -180: + movement += 360 + + return movement + + + def load_tendon_length_from_file(self): + try: + with open(self.file_path, 'r') as file: + data = yaml.safe_load(file) + if 'tendon_length' in data: + return data['tendon_length'] + except (FileNotFoundError, yaml.YAMLError, TypeError): + pass + return 0.0 # Default value if file doesn't exist or is invalid + + + def write_tendon_length_to_file(self): + data = {'tendon_length': self.tendon_length} + with open(self.file_path, 'w') as file: + yaml.dump(data, file) + + + +def main(args=None): + rclpy.init(args=args) + + turn_counter_node = TurnCounterNode() + + try: + rclpy.spin(turn_counter_node) + except KeyboardInterrupt: + pass + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + turn_counter_node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == '__main__': + main() diff --git a/ros_as5048b/setup.py b/ros_as5048b/setup.py index 96ca7ae..cfd03f7 100644 --- a/ros_as5048b/setup.py +++ b/ros_as5048b/setup.py @@ -1,3 +1,5 @@ +import os +from glob import glob from setuptools import setup package_name = 'ros_as5048b' @@ -10,6 +12,7 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), ], install_requires=['setuptools'], zip_safe=True, @@ -19,8 +22,8 @@ license='BSD3', entry_points={ 'console_scripts': [ - 'ros_as5048b_publisher = ros_as5048b.read_encoder_node:main', - 'ros_as5048b_publisher = ros_as5048b.read_encoder_node:main', + 'read_encoder_node = ros_as5048b.read_encoder_node:main', + 'turn_counter_node = ros_as5048b.turn_counter_node:main', ], }, )