From 9e01b61943fa02bee025ea060c8e270686c2b616 Mon Sep 17 00:00:00 2001 From: Max Polzin Date: Wed, 14 Feb 2024 23:15:17 +0100 Subject: [PATCH] Second commit. --- ros_encoder/CMakeLists.txt | 3 +- ros_encoder/ros_encoder/a_module_to_import.py | 4 - ros_encoder/ros_encoder/encoder_interface.py | 33 +++++++ .../ros_encoder/multiturn_encoder_node.py | 91 +++++++++++++++++++ ros_encoder/ros_encoder/python_node.py | 44 --------- 5 files changed, 126 insertions(+), 49 deletions(-) delete mode 100644 ros_encoder/ros_encoder/a_module_to_import.py create mode 100644 ros_encoder/ros_encoder/encoder_interface.py create mode 100755 ros_encoder/ros_encoder/multiturn_encoder_node.py delete mode 100755 ros_encoder/ros_encoder/python_node.py diff --git a/ros_encoder/CMakeLists.txt b/ros_encoder/CMakeLists.txt index 1a236c6..fb30d98 100644 --- a/ros_encoder/CMakeLists.txt +++ b/ros_encoder/CMakeLists.txt @@ -16,7 +16,8 @@ install(DIRECTORY ) install(PROGRAMS - ${PROJECT_NAME}/python_node.py + ${PROJECT_NAME}/multiturn_encoder_node.py + ${PROJECT_NAME}/encoder_interface.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/ros_encoder/ros_encoder/a_module_to_import.py b/ros_encoder/ros_encoder/a_module_to_import.py deleted file mode 100644 index 56b9090..0000000 --- a/ros_encoder/ros_encoder/a_module_to_import.py +++ /dev/null @@ -1,4 +0,0 @@ - - -def hello(): - return "abcd" \ No newline at end of file diff --git a/ros_encoder/ros_encoder/encoder_interface.py b/ros_encoder/ros_encoder/encoder_interface.py new file mode 100644 index 0000000..a209808 --- /dev/null +++ b/ros_encoder/ros_encoder/encoder_interface.py @@ -0,0 +1,33 @@ +import pigpio + +class Encoder: + def __init__(self, pi, pin_a, pin_b, scale=1): + self.pi = pi + self.scale = scale + self.pin_a = pin_a + self.pin_b = pin_b + self.position = 0 + + self.pi.set_mode(self.pin_a, pigpio.INPUT) + self.pi.set_mode(self.pin_b, pigpio.INPUT) + self.pi.set_pull_up_down(self.pin_a, pigpio.PUD_UP) + self.pi.set_pull_up_down(self.pin_b, pigpio.PUD_UP) + + self.cb_a = self.pi.callback(self.pin_a, pigpio.EITHER_EDGE, self._pulse) + self.cb_b = self.pi.callback(self.pin_b, pigpio.EITHER_EDGE, self._pulse) + + def _pulse(self, pin, level, tick): + a = self.pi.read(self.pin_a) + b = self.pi.read(self.pin_b) + if pin == self.pin_a: + if level == a: + self.position += 1 if a == b else -1 + else: # pin_b + if level == b: + self.position += 1 if a != b else -1 + + def get_position(self): + return self.position * self.scale + + def reset_position(self): + self.position = 0 diff --git a/ros_encoder/ros_encoder/multiturn_encoder_node.py b/ros_encoder/ros_encoder/multiturn_encoder_node.py new file mode 100755 index 0000000..fb27f6e --- /dev/null +++ b/ros_encoder/ros_encoder/multiturn_encoder_node.py @@ -0,0 +1,91 @@ +#!/bin/python3 + +import pigpio +import yaml + +import rclpy +from rclpy.node import Node + +from std_srvs.srv import Trigger +from std_msgs.msg import Float32 + +from ros_encoder.encoder_interface import Encoder + + +class MultiturnEncoder(Node): + + def __init__(self): + super().__init__('multiturn_encoder_node') + + self.pi = pigpio.pi() # Initialize pigpio + self.encoder = Encoder(self.pi, pin_a=17, pin_b=18, scale=1) # Adjust pins as needed + + self.reset_srv = self.create_service(Trigger, '~/reset', self.reset_callback) + self.publisher = self.create_publisher(Float32, '~/turns', 5) + + self.file_path = '/tmp/turns.yaml' + self.turns = self.load_turns_from_file() + self.publish_turns() + + timer_period = 0.5 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + self.timer_counter = 0 + + + def timer_callback(self): + current_position = self.encoder.get_position() # Get current position from encoder + self.turns = current_position / 360 # Convert to turns, adjust the divisor based on your encoder + self.publish_turns() + + self.timer_counter += 1 + if self.timer_counter == 10: # Adjust frequency of writing to file as needed + self.write_turns_to_file() + self.timer_counter = 0 + + + def reset_callback(self, request, response): + self.get_logger().info(f"Reset turns from {self.turns}") + self.turns = 0.0 + response.success=True + return response + + + def publish_turns(self): + msg_out = Float32() + msg_out.data = self.turns + self.publisher.publish(msg_out) + + + def load_turns_from_file(self): + try: + with open(self.file_path, 'r') as file: + data = yaml.safe_load(file) + if 'turns' in data: + return data['turns'] + except (FileNotFoundError, yaml.YAMLError, TypeError): + pass + return 0.0 # Default value if file doesn't exist or is invalid + + + def write_turns_to_file(self): + data = {'turns': self.turns} + with open(self.file_path, 'w') as file: + yaml.dump(data, file) + + +def main(args=None): + rclpy.init(args=args) + + multiturn_encoder = MultiturnEncoder() + + rclpy.spin(multiturn_encoder) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + multiturn_encoder.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ros_encoder/ros_encoder/python_node.py b/ros_encoder/ros_encoder/python_node.py deleted file mode 100755 index e379f06..0000000 --- a/ros_encoder/ros_encoder/python_node.py +++ /dev/null @@ -1,44 +0,0 @@ -#!/bin/python3 - -import rclpy -from rclpy.node import Node - -from ros_encoder import a_module_to_import - -from std_msgs.msg import String - - -class MinimalPublisher(Node): - - def __init__(self): - super().__init__('minimal_publisher') - self.publisher_ = self.create_publisher(String, 'topic', 10) - timer_period = 0.5 # seconds - self.timer = self.create_timer(timer_period, self.timer_callback) - self.i = 0 - - - def timer_callback(self): - msg = String() - msg.data = 'Hello World: %d' % self.i - self.publisher_.publish(msg) - self.get_logger().info('Publishing: "%s"' % msg.data) - self.i += 1 - - -def main(args=None): - rclpy.init(args=args) - - minimal_publisher = MinimalPublisher() - - rclpy.spin(minimal_publisher) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - minimal_publisher.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file