diff --git a/control/velocity_controller/setup.py b/control/velocity_controller/setup.py index 4212e72ef..8adeda275 100644 --- a/control/velocity_controller/setup.py +++ b/control/velocity_controller/setup.py @@ -1,24 +1,25 @@ from setuptools import find_packages, setup -package_name = 'velocity_controller' +package_name = "velocity_controller" setup( name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), + 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']), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='cyprian', - maintainer_email='cyprian@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], + maintainer="cyprian", + maintainer_email="cyprian@todo.todo", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], entry_points={ - 'console_scripts': [], + "console_scripts": [ + "velocity_controller = control/velocity_controller.velocity_controller:main" + ], }, ) diff --git a/control/velocity_controller/velocity_controller/velocity_controller.py b/control/velocity_controller/velocity_controller/velocity_controller.py index aa6d46dd3..a41998c77 100644 --- a/control/velocity_controller/velocity_controller/velocity_controller.py +++ b/control/velocity_controller/velocity_controller/velocity_controller.py @@ -1,2 +1,29 @@ #!usr/bin/env/python 3 import rclpy +from nav_msgs.msg import Odometry +from rclpy.node import Node + + +class VelocityNode(Node): + def __init__(self): + super().__init__("input_subscriber") + self.topic_subscriber = self.create_subscription( + Odometry, "/nucleus/odom", self.subscribe_callback, 10 + ) + + def subscribe_callback(self, msg: Odometry): + self.get_logger().info( + f"position_x: {msg.pose.pose.position.x}\n_y: {msg.pose.pose.position.y}\n_z: {msg.pose.pose.position.z}" + ) + + +def main(args=None): + rclpy.init(args=args) + node = VelocityNode() + rclpy.spin(node) + + rclpy.shutdown() + + +if __name__ == "__main__": + main()