diff --git a/ros_as5048b/launch/ros_as5048b.launch.py b/ros_as5048b/launch/ros_as5048b.launch.py index 7d376bb..74073b4 100644 --- a/ros_as5048b/launch/ros_as5048b.launch.py +++ b/ros_as5048b/launch/ros_as5048b.launch.py @@ -11,6 +11,9 @@ def generate_launch_description(): i2c_device_arg = DeclareLaunchArgument('i2c_device', default_value="1") i2c_address_arg = DeclareLaunchArgument('i2c_address', default_value="0x42") + encoder_topic_arg = DeclareLaunchArgument('encoder_topic', default_value="/encoder/at_0x42/deg") + turn_count_file_arg = DeclareLaunchArgument('turn_count_file', default_value='/tmp/turn_count.yaml') + read_encoder_node = Node( package='ros_as5048b', executable='read_encoder_node', @@ -21,27 +24,25 @@ def generate_launch_description(): ] ) - - 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"}, + {'encoder_topic': LaunchConfiguration('encoder_topic')}, ] ) nodes = [ read_encoder_node, - # turn_counter_node + turn_counter_node ] return LaunchDescription([ i2c_device_arg, i2c_address_arg, + encoder_topic_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 index d1b3df9..cd62b6d 100644 --- a/ros_as5048b/ros_as5048b/turn_counter_node.py +++ b/ros_as5048b/ros_as5048b/turn_counter_node.py @@ -3,7 +3,6 @@ from std_msgs.msg import Float32 from std_srvs.srv import Trigger -from winch_control_interfaces.srv import SetFloat32 import yaml import math @@ -11,76 +10,59 @@ class TurnCounterNode(Node): def __init__(self): - super().__init__('tendon_length_node') + super().__init__('turn_counter_node') - self.declare_parameter('file_path', '/tmp/tendon_length.yaml') + self.declare_parameter('file_path', '/tmp/turn_count.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.publisher = self.create_publisher(Float32, '~/turn_count', 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.turn_count = self.load_turn_count_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 + self.publish_turn_count() def reset_callback(self, request, response): - self.get_logger().info(f"Reset tendon length from {self.tendon_length}") - self.tendon_length = 0.0 + self.get_logger().info(f"Reset turn count from {self.turn_count}") + self.turn_count = 0.0 response.success=True return response def timer_callback(self): - self.publish_tendon_length() + self.publish_turn_count() self.timer_counter += 1 if self.timer_counter == 10: - self.write_tendon_length_to_file() + self.write_turn_count_to_file() self.timer_counter = 0 - def publish_tendon_length(self): + def publish_turn_count(self): msg_out = Float32() - msg_out.data = self.tendon_length + msg_out.data = self.turn_count 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 - - ### + encoder_movement = self.calculate_movement(encoder_value, self.prev_encoder_value) + delta_turn = encoder_movement/360.0 + self.turn_count += delta_turn self.prev_encoder_value = encoder_value @@ -97,19 +79,19 @@ def calculate_movement(self, current_value, previous_value): return movement - def load_tendon_length_from_file(self): + def load_turn_count_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'] + if 'turn_count' in data: + return data['turn_count'] 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} + def write_turn_count_to_file(self): + data = {'turn_count': self.turn_count} with open(self.file_path, 'w') as file: yaml.dump(data, file)